alex/esp-idf
alex/esp-idf
1
0
Fork 0
mirror of https://github.com/espressif/esp-idf.git synced 2024-10-05 20:47:46 -04:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

Merge branch 'feature/pid_speed_control_of_brushed_DC' into 'master'

Browse Source 
MCPWM example: DC motor speed control

Closes IDF-3142

See merge request espressif/esp-idf!13392
This commit is contained in:
morris 2021-08-13 04:23:48 +00:00
commit 49acb81a87
15 changed files with 1398 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/common_components/pid_ctrl/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
set(srcs "src/pid_ctrl.c")
idf_component_register(SRCS "${srcs}"
INCLUDE_DIRS "include")

2
examples/common_components/pid_ctrl/component.mk Normal file
View File

@ -0,0 +1,2 @@
COMPONENT_ADD_INCLUDEDIRS := include
COMPONENT_SRCDIRS := src

100
examples/common_components/pid_ctrl/include/pid_ctrl.h Normal file
View File

@ -0,0 +1,100 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#pragma once
#include "esp_err.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief PID calculation type
*
*/
typedef enum {
PID_CAL_TYPE_INCREMENTAL, /*!< Incremental PID control */
PID_CAL_TYPE_POSITIONAL, /*!< Positional PID control */
} pid_calculate_type_t;
/**
* @brief Type of PID control block handle
*
*/
typedef struct pid_ctrl_block_t *pid_ctrl_block_handle_t;
/**
* @brief PID control parameters
*
*/
typedef struct {
float kp; // PID Kp parameter
float ki; // PID Ki parameter
float kd; // PID Kd parameter
float max_output; // PID maximum output limitation
float min_output; // PID minimum output limitation
float max_integral; // PID maximum integral value limitation
float min_integral; // PID minimum integral value limitation
pid_calculate_type_t cal_type; // PID calculation type
} pid_ctrl_parameter_t;
/**
* @brief PID control configuration
*
*/
typedef struct {
pid_ctrl_parameter_t init_param; // Initial parameters
} pid_ctrl_config_t;
/**
* @brief Create a new PID control session, returns the handle of control block
*
* @param[in] config PID control configuration
* @param[out] ret_pid Returned PID control block handle
* @return
* - ESP_OK: Created PID control block successfully
* - ESP_ERR_INVALID_ARG: Created PID control block failed because of invalid argument
* - ESP_ERR_NO_MEM: Created PID control block failed because out of memory
*/
esp_err_t pid_new_control_block(const pid_ctrl_config_t *config, pid_ctrl_block_handle_t *ret_pid);
/**
* @brief Delete the PID control block
*
* @param[in] pid PID control block handle, created by `pid_new_control_block()`
* @return
* - ESP_OK: Delete PID control block successfully
* - ESP_ERR_INVALID_ARG: Delete PID control block failed because of invalid argument
*/
esp_err_t pid_del_control_block(pid_ctrl_block_handle_t pid);
/**
* @brief Update PID parameters
*
* @param[in] pid PID control block handle, created by `pid_new_control_block()`
* @param[in] params PID parameters
* @return
* - ESP_OK: Update PID parameters successfully
* - ESP_ERR_INVALID_ARG: Update PID parameters failed because of invalid argument
*/
esp_err_t pid_update_parameters(pid_ctrl_block_handle_t pid, const pid_ctrl_parameter_t *params);
/**
* @brief Input error and get PID control result
*
* @param[in] pid PID control block handle, created by `pid_new_control_block()`
* @param[in] input_error error data that feed to the PID controller
* @param[out] ret_result result after PID calculation
* @return
* - ESP_OK: Run a PID compute successfully
* - ESP_ERR_INVALID_ARG: Run a PID compute failed because of invalid argument
*/
esp_err_t pid_compute(pid_ctrl_block_handle_t pid, float input_error, float *ret_result);
#ifdef __cplusplus
}
#endif

140
examples/common_components/pid_ctrl/src/pid_ctrl.c Normal file
View File

@ -0,0 +1,140 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <stdbool.h>
#include <sys/param.h>
#include "esp_check.h"
#include "esp_log.h"
#include "pid_ctrl.h"
static const char *TAG = "pid_ctrl";
typedef struct pid_ctrl_block_t pid_ctrl_block_t;
typedef float (*pid_cal_func_t)(pid_ctrl_block_t *pid, float error);
struct pid_ctrl_block_t {
float Kp; // PID Kp value
float Ki; // PID Ki value
float Kd; // PID Kd value
float previous_err1; // e(k-1)
float previous_err2; // e(k-2)
float integral_err; // Sum of error
float last_output; // PID output in last control period
float max_output; // PID maximum output limitation
float min_output; // PID minimum output limitation
float max_integral; // PID maximum integral value limitation
float min_integral; // PID minimum integral value limitation
pid_cal_func_t calculate_func; // calculation function, depends on actual PID type set by user
};
static float pid_calc_positional(pid_ctrl_block_t *pid, float error)
{
float output = 0;
/* Add current error to the integral error */
pid->integral_err += error;
/* If the integral error is out of the range, it will be limited */
pid->integral_err = MIN(pid->integral_err, pid->max_integral);
pid->integral_err = MAX(pid->integral_err, pid->min_integral);
/* Calculate the pid control value by location formula */
/* u(k) = e(k)*Kp + (e(k)-e(k-1))*Kd + integral*Ki */
output = error * pid->Kp +
(error - pid->previous_err1) * pid->Kd +
pid->integral_err * pid->Ki;
/* If the output is out of the range, it will be limited */
output = MIN(output, pid->max_output);
output = MAX(output, pid->min_output);
/* Update previous error */
pid->previous_err1 = error;
return output;
}
static float pid_calc_incremental(pid_ctrl_block_t *pid, float error)
{
float output = 0;
/* Calculate the pid control value by increment formula */
/* du(k) = (e(k)-e(k-1))*Kp + (e(k)-2*e(k-1)+e(k-2))*Kd + e(k)*Ki */
/* u(k) = du(k) + u(k-1) */
output = (error - pid->previous_err1) * pid->Kp +
(error - 2 * pid->previous_err1 + pid->previous_err2) * pid->Kd +
error * pid->Ki +
pid->last_output;
/* If the output is beyond the range, it will be limited */
output = MIN(output, pid->max_output);
output = MAX(output, pid->min_output);
/* Update previous error */
pid->previous_err2 = pid->previous_err1;
pid->previous_err1 = error;
/* Update last output */
pid->last_output = output;
return output;
}
esp_err_t pid_new_control_block(const pid_ctrl_config_t *config, pid_ctrl_block_handle_t *ret_pid)
{
esp_err_t ret = ESP_OK;
pid_ctrl_block_t *pid = NULL;
/* Check the input pointer */
ESP_GOTO_ON_FALSE(config && ret_pid, ESP_ERR_INVALID_ARG, err, TAG, "invalid argument");
pid = calloc(1, sizeof(pid_ctrl_block_t));
ESP_GOTO_ON_FALSE(pid, ESP_ERR_NO_MEM, err, TAG, "no mem for PID control block");
ESP_GOTO_ON_ERROR(pid_update_parameters(pid, &config->init_param), err, TAG, "init PID parameters failed");
*ret_pid = pid;
return ret;
err:
if (pid) {
free(pid);
}
return ret;
}
esp_err_t pid_del_control_block(pid_ctrl_block_handle_t pid)
{
ESP_RETURN_ON_FALSE(pid, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
free(pid);
return ESP_OK;
}
esp_err_t pid_compute(pid_ctrl_block_handle_t pid, float input_error, float *ret_result)
{
ESP_RETURN_ON_FALSE(pid && ret_result, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
*ret_result = pid->calculate_func(pid, input_error);
return ESP_OK;
}
esp_err_t pid_update_parameters(pid_ctrl_block_handle_t pid, const pid_ctrl_parameter_t *params)
{
ESP_RETURN_ON_FALSE(pid && params, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
pid->Kp = params->kp;
pid->Ki = params->ki;
pid->Kd = params->kd;
pid->max_output = params->max_output;
pid->min_output = params->min_output;
pid->max_integral = params->max_integral;
pid->min_integral = params->min_integral;
/* Set the calculate function according to the PID type */
switch (params->cal_type) {
case PID_CAL_TYPE_INCREMENTAL:
pid->calculate_func = pid_calc_incremental;
break;
case PID_CAL_TYPE_POSITIONAL:
pid->calculate_func = pid_calc_positional;
break;
default:
ESP_RETURN_ON_FALSE(false, ESP_ERR_INVALID_ARG, TAG, "invalid PID calculation type:%d", params->cal_type);
}
return ESP_OK;
}

3
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/CMakeLists.txt
View File

@ -2,5 +2,8 @@
# in this exact order for cmake to work correctly
cmake_minimum_required(VERSION 3.5)
set(EXTRA_COMPONENT_DIRS "$ENV{IDF_PATH}/examples/common_components"
"$ENV{IDF_PATH}/examples/peripherals/pcnt/rotary_encoder/components")
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(mcpwm_brushed_dc_control)

3
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/Makefile
View File

@ -5,4 +5,7 @@
PROJECT_NAME := mcpwm_brushed_dc_control
EXTRA_COMPONENT_DIRS += $(IDF_PATH)/examples/common_components
EXTRA_COMPONENT_DIRS += $(IDF_PATH)/examples/peripherals/pcnt/rotary_encoder/components
include $(IDF_PATH)/make/project.mk

255
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/README.md
View File

@ -1,25 +1,246 @@
| Supported Targets | ESP32 |
| ----------------- | ----- |
| Supported Targets | ESP32 | ESP32-S3 |
| ----------------- | ----- | -------- |
# MCPWM Brushed DC Motor Example
# MCPWM brushed dc motor control Example
(See the README.md file in the upper level 'examples' directory for more information about examples.)
This example will show you how to use MCPWM module to control brushed dc motor, you need to make connection between ESP32 and motor driver
This code is tested with L298 motor driver, user needs to make changes according to the driver they use
Motor first moves forward, then backward and then stops for 2 Secs each countinuously
This example mainly illustrates how to drive a brushed DC motor by generating two specific PWM signals. This example assumes an [L298N](https://www.st.com/content/st_com/en/products/motor-drivers/brushed-dc-motor-drivers/l298.html) H-bridge driver is used to provide the needed voltage and current for brushed DC motor. This example also implements a motor control command console such that users can configure and control the motors at run time using console commands.
## How to Use Example
Before project configuration and build, be sure to set the correct chip target using `idf.py set-target <chip_name>`.
### Hardware Required
* A development board with any Espressif SoC which features MCPWM and PCNT peripheral (e.g., ESP32-DevKitC, ESP-WROVER-KIT, etc.)
* A USB cable for Power supply and programming
* A separate 12V power supply for brushed DC motor and H-bridge (the voltage depends on the motor model used in the example)
* A motor driving board to transfer pwm signal into driving signal
* A brushed DC motor, e.g. [25GA370](http://www.tronsunmotor.com/data/upload/file/201807/e03b98802b5c5390d6570939def525ba.pdf)
* A quadrature encoder to detect speed
Connection :
```
Power(12V)
|
v
+----------------+ +--------------------+
| | | H-Bridge |
| GND +------------>| | +--------------+
| | | | | |
| GPIO15 +----PWM0A--->| IN_A OUT_A +----->| Brushed |
| | | | | DC |
| GPIO16 +----PWM0B--->| IN_A OUT_B +----->| Motor |
| | | | | |
| ESP | +--------------------+ | |
| | +------+-------+
| | |
| | +--------------------+ |
| VCC3.3 +------------>| Encoder | |
| | | | |
| GND +------------>| |<------------+
| | | |
| GPIO18 |<---PhaseA---+ C1 |
| | | |
| GPIO19 |<---PhaseB---+ C2 |
| | | |
+----------------+ +--------------------+
```
NOTE: If some other GPIO pins (e.g., 13/14) are chosen as the PCNT encoder pins, flashing might fail while the wires are connected. If this occurs, please try disconnecting the power supply of the encoder while flashing.
### Build and Flash
Run `idf.py -p PORT flash monitor` to build, flash and monitor the project.
(To exit the serial monitor, type ``Ctrl-]``.)
See the [Getting Started Guide](https://idf.espressif.com/) for full steps to configure and use ESP-IDF to build projects.
## Step 1: Pin assignment
* GPIO15 is assigned as the enable/input 1 for motor driver
* GPIO16 is assigned as the enable/input 2 for motor driver
## Example Output
Run the example, you will see the following output log:
## Step 2: Connection
* connect GPIO15 with input 1 of motor driver
* connect GPIO16 with input 2 of motor driver
```bash
...
Testing brushed motor with PID...
initializing mcpwm gpio...
Configuring Initial Parameters of mcpwm...
Type 'help' to get the list of commands.
Use UP/DOWN arrows to navigate through command history.
Press TAB when typing command name to auto-complete.
=================================================================
| Example of Motor Control |
| |
| 1. Try 'help', check all supported commands |
| 2. Try 'config' to set control period or pwm frequency |
| 3. Try 'pid' to configure pid paremeters |
| 4. Try 'expt' to set expectation value and mode |
| 5. Try 'motor' to start motor in several seconds or stop it |
| |
=================================================================
## Step 3: Initialize MCPWM
* You need to set the frequency and duty cycle of MCPWM timer
* You need to set the MCPWM unit you want to use, and bind the unit with one of the timers
Default configuration are shown as follows.
You can input 'config -s' to check current status.
-----------------------------------------------------------------
Current Configuration Status
Configuration
Period = 10 ms PID = enabled
PID - Increment
Kp = 0.800 Ki = 0.000 Kd = 0.100
Expectation - Triangle
init = 30.000 max = 50.000 min = -50.000 pace = 1.000
MCPWM
Frequency = 1000 Hz
Motor
Running seconds = 10
-----------------------------------------------------------------
mcpwm-motor>
```
### Check all supported commands and their usages
* Command: `help`
```bash
mcpwm-motor> help
help
Print the list of registered commands
config config -s
Enable or disable PID and set motor control period
--pid=<y|n> Enable or disable PID algorithm
-T, --period=<ms> Set motor control period
-s, --show Show current configurations
expt expt -i <duty> -m <fixed/tri/rect> -p <double> --max <duty> --min -50<duty>
Set initial value, limitation and wave mode of expectation. Both dynamic and
static mode are available
--max=<duty> Max limitation for dynamic expectation
--min=<duty> Min limitation for dynamic expectation
-p, --pace=<double> The increasing pace of expectation every 50ms
-i, --init=<duty> Initial expectation. Usually between -100~100
-m, --mode=<fixed/tri/rect> Select static or dynamic expectation wave mode. 'fixed' for static, 'tri' for triangle, 'rect' for rectangle
pid pid -p <double> -i <double> -d <double> -t <loc/inc>
Set parameters and type for PID algorithm
-p, --kp=<double> Set Kp value for PID
-i, --ki=<double> Set Ki value for PID
-d, --kd=<double> Set Kd value for PID
-t, --type=<loc/inc> Select locational PID or incremental PID
motor motor -u 10
Start or stop the motor
-u, --start=<seconds> Set running seconds for motor, set '0' to keep motor running
-d, --stop Stop the motor
```
### Check status
* Command: `config -s`
```bash
mcpwm-motor> config -s
-----------------------------------------------------------------
Current Configuration Status
Configuration
Period = 10 ms PID = enabled
PID - Increment
Kp = 0.800 Ki = 0.000 Kd = 0.100
Expectation - Triangle
init = 30.000 max = 50.000 min = -50.000 pace = -1.000
MCPWM
Frequency = 1000 Hz
Motor
Running seconds = 10
-----------------------------------------------------------------
```
### Enable or disable PID
* Command: `config --pid <y/n>`
* 'y' - enable PID
* 'n' - disable PID
```bash
mcpwm-motor> config --pid n
config: pid disabled
mcpwm-motor> config --pid y
config: pid enabled
```
### Set PID parameters
* Command: `pid -p <double> -i <double> -d <double> -t <loc/inc>`
* 'p' - proportion value
* 'i' - integral value
* 'd' - differential value
* 't' - PID calculation type (locational or incremental).
```bash
mcpwm-motor> pid -p 0.8 -i 0.02 -d 0.1 -t inc
pid: kp = 0.800
pid: ki = 0.020
pid: kd = 0.100
pid: type = increment
```
### Set expectation parameters
* Command: `expt -i <duty> -m <fixed/tri/rect> -p <double> --max <duty> --min <duty>`
* 'i' - initial duty if you set mode 'fixed'
* 'm' - expectation mode. 'fixed' means the expectation will never change, 'tri' means the expectation will changes with trigonometric wave, 'rect' means the expectation will changes with rectangular wave
* 'p' - the setp size of expectation changed in every 50ms, it can adjust the expectation changing speed
* 'max' - the maximum limitation of expectation
* 'min' - the minimum limitation of expectation
```bash
mcpwm-motor> expt -i 40 -m rect -p 1.5 --max 80 --min -60
expt: init = 40.000
expt: max = 80.000
expt: min = -60.000
expt: pace = 1.500
expt: mode = rectangle
```
### Start or stop motor
* Command: `motor -u <sec>`
* Command: `motor -d`
* 'u' - start the motor in <sec> seconds, if <sec> is 0, the motor won't stop until 'motor -d' is inputed
* 'd' - stop the motor right now
```bash
mcpwm-motor> motor -u 10
motor: motor starts to run in 10 seconds
mcpwm-motor> 1
2
3
4
5
6
7
8
9
10
Time up: motor stoped
```
## Troubleshooting
* Make sure your ESP board and H-bridge module have been connected to the same GND panel.
For any technical queries, please open an [issue](https://github.com/espressif/esp-idf/issues) on GitHub. We will get back to you soon.

5
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/CMakeLists.txt Normal file
View File

@ -0,0 +1,5 @@
set(COMPONENT_SRCS "motor_ctrl_timer.c")
idf_component_register(SRCS "${COMPONENT_SRCS}"
INCLUDE_DIRS .
PRIV_REQUIRES "driver")

1
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/component.mk Normal file
View File

@ -0,0 +1 @@
COMPONENT_ADD_INCLUDEDIRS := .

141
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.c Normal file
View File

@ -0,0 +1,141 @@
/* To set the control period for DC motor Timer
This example code is in the Public Domain (or CC0 licensed, at your option.)
Unless required by applicable law or agreed to in writing, this
software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
CONDITIONS OF ANY KIND, either express or implied.
*/
#include <stdio.h>
#include "motor_ctrl_timer.h"
#include "esp_check.h"
#define MOTOR_CTRL_TIMER_DIVIDER (16) // Hardware timer clock divider
#define MOTOR_CTRL_TIMER_SCALE (TIMER_BASE_CLK / MOTOR_CTRL_TIMER_DIVIDER) // convert counter value to seconds
#define MOTOR_CONTROL_TIMER_GROUP TIMER_GROUP_0
#define MOTOR_CONTROL_TIMER_ID TIMER_0
static const char *TAG = "motor_ctrl_timer";
/**
* @brief Callback function of timer intterupt
*
* @param args The parameter transmited to callback function from timer_isr_callback_add. Args here is for timer_info.
* @return
* - True Do task yield at the end of ISR
* - False Not do task yield at the end of ISR
*/
static bool IRAM_ATTR motor_ctrl_timer_isr_callback(void *args)
{
BaseType_t high_task_awoken = pdFALSE;
motor_ctrl_timer_info_t *info = (motor_ctrl_timer_info_t *) args;
info->pulse_info.pulse_cnt = info->pulse_info.get_pulse_callback(info->pulse_info.callback_args);
/* Now just send the event data back to the main program task */
xQueueSendFromISR(info->timer_evt_que, info, &high_task_awoken);
return high_task_awoken == pdTRUE; // return whether we need to yield at the end of ISR
}
/**
* @brief Initialize the motor control timer
*
* @param timer_info the secondary pointer of motor_ctrl_timer_info_t
* @param evt_que timer event queue
* @param ctrl_period_ms motor control period
* @param pulse_info quadrature encoder pulse information
* @return
* - ESP_OK: Motor control timer initialized successfully
* - ESP_FAIL: motor control timer failed to initialize because of other errors
*/
esp_err_t motor_ctrl_new_timer(motor_ctrl_timer_info_t **timer_info,
QueueHandle_t evt_que,
unsigned int ctrl_period_ms,
pulse_info_t pulse_info)
{
esp_err_t ret = ESP_FAIL;
/* Select and initialize basic parameters of the timer */
timer_config_t config = {
.divider = MOTOR_CTRL_TIMER_DIVIDER,
.counter_dir = TIMER_COUNT_UP,
.counter_en = TIMER_PAUSE,
.alarm_en = TIMER_ALARM_EN,
.auto_reload = true,
}; // default clock source is APB
ret = timer_init(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID, &config);
ESP_RETURN_ON_ERROR(ret, TAG, "timer init failed\n");
/* Timer's counter will initially start from value below.
Since auto_reload is set, this value will be automatically reload on alarm */
timer_set_counter_value(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID, 0);
/* Configure the alarm value and the interrupt on alarm. */
timer_set_alarm_value(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID, ctrl_period_ms * MOTOR_CTRL_TIMER_SCALE / 1000);
timer_enable_intr(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID);
/* Check the pointers */
ESP_GOTO_ON_FALSE(evt_que, ESP_ERR_INVALID_ARG, err, TAG, "timer event queue handler is NULL\n");
ESP_GOTO_ON_FALSE(timer_info, ESP_ERR_INVALID_ARG, err, TAG, "timer info structure pointer is NULL\n");
/* Alloc and config the infomation structure for this file */
*timer_info = calloc(1, sizeof(motor_ctrl_timer_info_t));
ESP_GOTO_ON_FALSE(*timer_info, ESP_ERR_NO_MEM, err, TAG, "timer_info calloc failed\n");
(*timer_info)->timer_group = MOTOR_CONTROL_TIMER_GROUP;
(*timer_info)->timer_idx = MOTOR_CONTROL_TIMER_ID;
(*timer_info)->timer_evt_que = evt_que;
(*timer_info)->ctrl_period_ms = ctrl_period_ms;
(*timer_info)->pulse_info = pulse_info;
timer_isr_callback_add(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID, motor_ctrl_timer_isr_callback, *timer_info, 0);
return ret;
err:
timer_deinit(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID);
return ret;
}
/**
* @brief Set timer alarm period
*
* @param period Timer alarm period
* @return
* - void
*/
void motor_ctrl_timer_set_period(unsigned int period)
{
timer_set_alarm_value(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID, period * MOTOR_CTRL_TIMER_SCALE / 1000);
}
/**
* @brief Start the timer
*/
void motor_ctrl_timer_start(void)
{
/* start the timer */
timer_start(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID);
}
/**
* @brief Pause the timer and clear the counting value
*/
void motor_ctrl_timer_stop(void)
{
/* stop the timer */
timer_pause(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID);
timer_set_counter_value(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID, 0);
}
/**
* @brief Deinitialize the timer
*
* @param timer_info the secondary pointer of motor_ctrl_timer_info_t, the memory will be freed
*/
void motor_ctrl_timer_deinit(motor_ctrl_timer_info_t **timer_info)
{
if (*timer_info != NULL) {
timer_deinit(MOTOR_CONTROL_TIMER_GROUP, MOTOR_CONTROL_TIMER_ID);
free(*timer_info);
*timer_info = NULL;
}
}

78
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.h Normal file
View File

@ -0,0 +1,78 @@
/* To set the control period for DC motor Timer
This example code is in the Public Domain (or CC0 licensed, at your option.)
Unless required by applicable law or agreed to in writing, this
software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
CONDITIONS OF ANY KIND, either express or implied.
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "driver/timer.h"
typedef struct {
int (*get_pulse_callback)(void *);
void *callback_args;
int pulse_cnt;
} pulse_info_t;
typedef struct {
timer_group_t timer_group; /* Timer Group number */
timer_idx_t timer_idx; /* Timer ID */
unsigned int ctrl_period_ms; /* Motor control period, unit in ms */
QueueHandle_t timer_evt_que; /* The queue of timer events */
pulse_info_t pulse_info;
} motor_ctrl_timer_info_t;
/**
* @brief Initialize the motor control timer
*
* @param timer_info the secondary pointer of motor_ctrl_timer_info_t
* @param evt_que timer event queue
* @param ctrl_period_ms motor control period
* @param pulse_info quadrature encoder pulse information
* @return
* - ESP_OK: Motor control timer initialized successfully
* - ESP_FAIL: motor control timer failed to initialize because of other errors
*/
esp_err_t motor_ctrl_new_timer(motor_ctrl_timer_info_t **timer_info,
QueueHandle_t evt_que,
unsigned int ctrl_period_ms,
pulse_info_t pulse_info);
/**
* @brief Set timer alarm period
*
* @param period Timer alarm period
*/
void motor_ctrl_timer_set_period(unsigned int period);
/**
* @brief Start the timer
*/
void motor_ctrl_timer_start(void);
/**
* @brief Pause the timer and clear the counting value
*/
void motor_ctrl_timer_stop(void);
/**
* @brief Deinitialize the timer
*
* @param timer_info the secondary pointer of motor_ctrl_timer_info_t, the memory will be freed
*/
void motor_ctrl_timer_deinit(motor_ctrl_timer_info_t **timer_info);
#ifdef __cplusplus
}
#endif

7
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/CMakeLists.txt
View File

@ -1,2 +1,5 @@
idf_component_register(SRCS "mcpwm_brushed_dc_control_example.c"
INCLUDE_DIRS ".")
set(COMPONENT_SRCS "mcpwm_brushed_dc_control_example.c"
"cmd_mcpwm_motor.c")
idf_component_register(SRCS "${COMPONENT_SRCS}"
INCLUDE_DIRS "./")

308
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/cmd_mcpwm_motor.c Normal file
View File

@ -0,0 +1,308 @@
/* cmd_mcpwm_motor.h
This example code is in the Public Domain (or CC0 licensed, at your option.)
Unless required by applicable law or agreed to in writing, this
software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
CONDITIONS OF ANY KIND, either express or implied.
*/
#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "argtable3/argtable3.h"
#include "esp_console.h"
#include "esp_log.h"
#include "mcpwm_brushed_dc_control_example.h"
#define MOTOR_CTRL_CMD_CHECK(ins) if(arg_parse(argc, argv, (void **)&ins)){ \
arg_print_errors(stderr, ins.end, argv[0]); \
return 0;}
static mcpwm_motor_control_t *mc;
extern SemaphoreHandle_t g_motor_mux;
static struct {
struct arg_str *pid_flag;
struct arg_int *period;
struct arg_lit *show;
struct arg_end *end;
} motor_ctrl_config_args;
static struct {
struct arg_dbl *max;
struct arg_dbl *min;
struct arg_dbl *pace;
struct arg_dbl *init;
struct arg_str *mode;
struct arg_end *end;
} motor_ctrl_expt_args;
static struct {
struct arg_dbl *kp;
struct arg_dbl *ki;
struct arg_dbl *kd;
struct arg_str *type;
struct arg_end *end;
} motor_ctrl_pid_args;
static struct {
struct arg_int *start;
struct arg_lit *stop;
struct arg_end *end;
} motor_ctrl_motor_args;
static void print_current_status(void)
{
printf("\n -----------------------------------------------------------------\n");
printf(" Current Configuration Status \n\n");
printf(" Configuration\n Period = %d ms\tPID = %s\n\n",
mc->cfg.ctrl_period, mc->cfg.pid_enable ? "enabled" : "disabled");
printf(" PID - %s\n Kp = %.3f\tKi = %.3f\tKd = %.3f\n\n",
(mc->pid_param.cal_type == PID_CAL_TYPE_POSITIONAL) ? "Location" : "Increment",
mc->pid_param.kp, mc->pid_param.ki, mc->pid_param.kd);
printf(" Expectation - %s\n init = %.3f\tmax = %.3f\tmin = %.3f\tpace = %.3f\n\n",
mc->cfg.expt_mode ? (mc->cfg.expt_mode == MOTOR_CTRL_MODE_TRIANGLE ? "Triangle" : "Rectangle") : "Fixed",
mc->cfg.expt_init, mc->cfg.expt_max, mc->cfg.expt_min, mc->cfg.expt_pace);
printf(" MCPWM\n Frequency = %d Hz\n\n", mc->cfg.pwm_freq);
printf(" Motor\n Running seconds = %d\n", mc->cfg.running_sec);
printf(" -----------------------------------------------------------------\n\n");
}
static int do_motor_ctrl_config_cmd(int argc, char **argv)
{
MOTOR_CTRL_CMD_CHECK(motor_ctrl_config_args);
xSemaphoreTake(g_motor_mux, portMAX_DELAY);
if (motor_ctrl_config_args.pid_flag->count) {
if (!strcmp(*motor_ctrl_config_args.pid_flag->sval, "n") ||
!strcmp(*motor_ctrl_config_args.pid_flag->sval, "no")) {
mc->cfg.pid_enable = false;
printf("config: pid disabled\n");
} else {
mc->cfg.pid_enable = true;
printf("config: pid enabled\n");
}
}
if (motor_ctrl_config_args.period->count) {
mc->cfg.ctrl_period = motor_ctrl_config_args.period->ival[0];
motor_ctrl_timer_set_period(mc->cfg.ctrl_period);
printf("config: control period = mc->cfg.ctrl_period\n");
}
if (motor_ctrl_config_args.show->count) {
print_current_status();
}
xSemaphoreGive(g_motor_mux);
return 0;
}
static int do_motor_ctrl_expt_cmd(int argc, char **argv)
{
MOTOR_CTRL_CMD_CHECK(motor_ctrl_expt_args);
xSemaphoreTake(g_motor_mux, portMAX_DELAY);
if (motor_ctrl_expt_args.init->count) {
mc->cfg.expt_init = motor_ctrl_expt_args.init->dval[0];
printf("expt: init = %.3f\n", mc->cfg.expt_init);
}
if (motor_ctrl_expt_args.max->count) {
mc->cfg.expt_max = motor_ctrl_expt_args.max->dval[0];
printf("expt: max = %.3f\n", mc->cfg.expt_max);
}
if (motor_ctrl_expt_args.min->count) {
mc->cfg.expt_min = motor_ctrl_expt_args.min->dval[0];
printf("expt: min = %.3f\n", mc->cfg.expt_min);
}
if (motor_ctrl_expt_args.pace->count) {
mc->cfg.expt_pace = motor_ctrl_expt_args.pace->dval[0];
printf("expt: pace = %.3f\n", mc->cfg.expt_pace);
}
if (motor_ctrl_expt_args.mode->count) {
if (!strcmp(*motor_ctrl_expt_args.mode->sval, "fixed")) {
mc->cfg.expt_mode = MOTOR_CTRL_MODE_FIXED;
printf("expt: mode = fixed\n");
} else if (!strcmp(*motor_ctrl_expt_args.mode->sval, "tri")) {
mc->cfg.expt_mode = MOTOR_CTRL_MODE_TRIANGLE;
printf("expt: mode = triangle\n");
} else if (!strcmp(*motor_ctrl_expt_args.mode->sval, "rect")) {
mc->cfg.expt_mode = MOTOR_CTRL_MODE_RECTANGLE;
printf("expt: mode = rectangle\n");
} else {
mc->cfg.expt_mode = MOTOR_CTRL_MODE_TRIANGLE;
printf("expt: mode = triangle\n");
}
}
xSemaphoreGive(g_motor_mux);
return 0;
}
static int do_motor_ctrl_pid_cmd(int argc, char **argv)
{
int ret = 0;
MOTOR_CTRL_CMD_CHECK(motor_ctrl_pid_args);
xSemaphoreTake(g_motor_mux, portMAX_DELAY);
if (motor_ctrl_pid_args.kp->count) {
mc->pid_param.kp = motor_ctrl_pid_args.kp->dval[0];
printf("pid: kp = %.3f\n", mc->pid_param.kp);
}
if (motor_ctrl_pid_args.ki->count) {
mc->pid_param.ki = motor_ctrl_pid_args.ki->dval[0];
printf("pid: ki = %.3f\n", mc->pid_param.ki);
}
if (motor_ctrl_pid_args.kd->count) {
mc->pid_param.kd = motor_ctrl_pid_args.kd->dval[0];
printf("pid: kd = %.3f\n", mc->pid_param.kd);
}
if (motor_ctrl_pid_args.type->count) {
if (!strcmp(motor_ctrl_pid_args.type->sval[0], "loc")) {
mc->pid_param.cal_type = PID_CAL_TYPE_POSITIONAL;
printf("pid: type = positional\n");
} else if (!strcmp(motor_ctrl_pid_args.type->sval[0], "inc")) {
mc->pid_param.cal_type = PID_CAL_TYPE_INCREMENTAL;
printf("pid: type = incremental\n");
} else {
printf("Invalid pid type:%s\n", motor_ctrl_pid_args.type->sval[0]);
ret = 1;
}
}
pid_update_parameters(mc->pid, &mc->pid_param);
xSemaphoreGive(g_motor_mux);
return ret;
}
static int do_motor_ctrl_motor_cmd(int argc, char **argv)
{
MOTOR_CTRL_CMD_CHECK(motor_ctrl_motor_args);
xSemaphoreTake(g_motor_mux, portMAX_DELAY);
if (motor_ctrl_motor_args.start->count) {
mc->cfg.running_sec = motor_ctrl_motor_args.start->ival[0];
// Start the motor
brushed_motor_start(mc);
mc->cfg.running_sec ?
printf("motor: motor starts to run in %d seconds\n", mc->cfg.running_sec) :
printf("motor: motor starts to run, input 'motor -d' to stop it\n");
}
if (motor_ctrl_motor_args.stop->count) {
// Stop the motor
brushed_motor_stop(mc);
printf("motor: motor stoped\n");
}
xSemaphoreGive(g_motor_mux);
return 0;
}
static void register_motor_ctrl_config(void)
{
motor_ctrl_config_args.pid_flag = arg_str0(NULL, "pid", "<y|n>", "Enable or disable PID algorithm");
motor_ctrl_config_args.period = arg_int0("T", "period", "<ms>", "Set motor control period");
motor_ctrl_config_args.show = arg_lit0("s", "show", "Show current configurations");
motor_ctrl_config_args.end = arg_end(2);
const esp_console_cmd_t motor_ctrl_cfg_cmd = {
.command = "config",
.help = "Enable or disable PID and set motor control period",
.hint = "config -s",
.func = &do_motor_ctrl_config_cmd,
.argtable = &motor_ctrl_config_args
};
ESP_ERROR_CHECK(esp_console_cmd_register(&motor_ctrl_cfg_cmd));
}
static void register_motor_ctrl_expt(void)
{
motor_ctrl_expt_args.init = arg_dbl0("i", "init", "<duty>", "Initial expectation. Usually between -100~100");
motor_ctrl_expt_args.max = arg_dbl0(NULL, "max", "<duty>", "Max limitation for dynamic expectation");
motor_ctrl_expt_args.min = arg_dbl0(NULL, "min", "<duty>", "Min limitation for dynamic expectation");
motor_ctrl_expt_args.pace = arg_dbl0("p", "pace", "<double>", "The increasing pace of expectation every 50ms");
motor_ctrl_expt_args.mode = arg_str0("m", "mode", "<fixed/tri/rect>",
"Select static or dynamic expectation wave mode. 'fixed' for static, 'tri' for triangle, 'rect' for rectangle");
motor_ctrl_expt_args.end = arg_end(2);
const esp_console_cmd_t motor_ctrl_expt_cmd = {
.command = "expt",
.help = "Set initial value, limitation and wave mode of expectation. Both dynamic and static mode are available",
.hint = "expt -i <duty> -m <fixed/tri/rect> -p <double> --max <duty> --min <duty>",
.func = &do_motor_ctrl_expt_cmd,
.argtable = &motor_ctrl_expt_args
};
ESP_ERROR_CHECK(esp_console_cmd_register(&motor_ctrl_expt_cmd));
}
static void register_motor_ctrl_pid(void)
{
motor_ctrl_pid_args.kp = arg_dbl0("p", "kp", "<double>", "Set Kp value for PID");
motor_ctrl_pid_args.ki = arg_dbl0("i", "ki", "<double>", "Set Ki value for PID");
motor_ctrl_pid_args.kd = arg_dbl0("d", "kd", "<double>", "Set Kd value for PID");
motor_ctrl_pid_args.type = arg_str0("t", "type", "<loc/inc>", "Select locational PID or incremental PID");
motor_ctrl_pid_args.end = arg_end(2);
const esp_console_cmd_t motor_ctrl_pid_cmd = {
.command = "pid",
.help = "Set parameters and type for PID algorithm",
.hint = "pid -p <double> -i <double> -d <double> -t <loc/inc>",
.func = &do_motor_ctrl_pid_cmd,
.argtable = &motor_ctrl_pid_args
};
ESP_ERROR_CHECK(esp_console_cmd_register(&motor_ctrl_pid_cmd));
}
static void register_motor_ctrl_motor(void)
{
motor_ctrl_motor_args.start = arg_int0("u", "start", "<seconds>", "Set running seconds for motor, set '0' to keep motor running");
motor_ctrl_motor_args.stop = arg_lit0("d", "stop", "Stop the motor");
motor_ctrl_motor_args.end = arg_end(2);
const esp_console_cmd_t motor_ctrl_motor_cmd = {
.command = "motor",
.help = "Start or stop the motor",
.hint = "motor -u 10",
.func = &do_motor_ctrl_motor_cmd,
.argtable = &motor_ctrl_motor_args
};
ESP_ERROR_CHECK(esp_console_cmd_register(&motor_ctrl_motor_cmd));
}
void cmd_mcpwm_motor_init(mcpwm_motor_control_t *motor_ctrl)
{
mc = motor_ctrl;
esp_console_repl_t *repl = NULL;
esp_console_repl_config_t repl_config = ESP_CONSOLE_REPL_CONFIG_DEFAULT();
repl_config.prompt = "mcpwm-motor>";
// install console REPL environment
#if CONFIG_ESP_CONSOLE_UART
esp_console_dev_uart_config_t uart_config = ESP_CONSOLE_DEV_UART_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_console_new_repl_uart(&uart_config, &repl_config, &repl));
#elif CONFIG_ESP_CONSOLE_USB_CDC
esp_console_dev_usb_cdc_config_t cdc_config = ESP_CONSOLE_DEV_CDC_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_console_new_repl_usb_cdc(&cdc_config, &repl_config, &repl));
#endif
register_motor_ctrl_config();
register_motor_ctrl_expt();
register_motor_ctrl_pid();
register_motor_ctrl_motor();
printf("\n =================================================================\n");
printf(" | Example of Motor Control |\n");
printf(" | |\n");
printf(" | 1. Try 'help', check all supported commands |\n");
printf(" | 2. Try 'config' to set control period or pwm frequency |\n");
printf(" | 3. Try 'pid' to configure pid paremeters |\n");
printf(" | 4. Try 'expt' to set expectation value and mode |\n");
printf(" | 5. Try 'motor' to start motor in several seconds or stop it |\n");
printf(" | |\n");
printf(" =================================================================\n\n");
printf("Default configuration are shown as follows.\nYou can input 'config -s' to check current status.");
print_current_status();
// start console REPL
ESP_ERROR_CHECK(esp_console_start_repl(repl));
}

327
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.c
View File

@ -17,79 +17,294 @@
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "esp_attr.h"
#include "driver/mcpwm.h"
#include "soc/mcpwm_periph.h"
#include "driver/pcnt.h"
#define GPIO_PWM0A_OUT 15 //Set GPIO 15 as PWM0A
#define GPIO_PWM0B_OUT 16 //Set GPIO 16 as PWM0B
#include "mcpwm_brushed_dc_control_example.h"
#define MOTOR_CTRL_MCPWM_UNIT MCPWM_UNIT_0
#define MOTOR_CTRL_MCPWM_TIMER MCPWM_TIMER_0
/* The global infomation structure */
static mcpwm_motor_control_t motor_ctrl;
SemaphoreHandle_t g_motor_mux;
/**
* @brief Initialize the gpio as mcpwm output
*/
static void mcpwm_example_gpio_initialize(void)
{
printf("initializing mcpwm gpio...\n");
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_PWM0A_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, GPIO_PWM0B_OUT);
mcpwm_gpio_init(MOTOR_CTRL_MCPWM_UNIT, MCPWM0A, GPIO_PWM0A_OUT);
mcpwm_gpio_init(MOTOR_CTRL_MCPWM_UNIT, MCPWM0B, GPIO_PWM0B_OUT);
}
/**
* @brief motor moves in forward direction, with duty cycle = duty %
* @brief set motor moves speed and direction with duty cycle = duty %
*/
static void brushed_motor_forward(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num , float duty_cycle)
void brushed_motor_set_duty(float duty_cycle)
{
mcpwm_set_signal_low(mcpwm_num, timer_num, MCPWM_OPR_B);
mcpwm_set_duty(mcpwm_num, timer_num, MCPWM_OPR_A, duty_cycle);
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_A, MCPWM_DUTY_MODE_0); //call this each time, if operator was previously in low/high state
}
/**
* @brief motor moves in backward direction, with duty cycle = duty %
*/
static void brushed_motor_backward(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num , float duty_cycle)
{
mcpwm_set_signal_low(mcpwm_num, timer_num, MCPWM_OPR_A);
mcpwm_set_duty(mcpwm_num, timer_num, MCPWM_OPR_B, duty_cycle);
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //call this each time, if operator was previously in low/high state
}
/**
* @brief motor stop
*/
static void brushed_motor_stop(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num)
{
mcpwm_set_signal_low(mcpwm_num, timer_num, MCPWM_OPR_A);
mcpwm_set_signal_low(mcpwm_num, timer_num, MCPWM_OPR_B);
}
/**
* @brief Configure MCPWM module for brushed dc motor
*/
static void mcpwm_example_brushed_motor_control(void *arg)
{
//1. mcpwm gpio initialization
mcpwm_example_gpio_initialize();
//2. initial mcpwm configuration
printf("Configuring Initial Parameters of mcpwm...\n");
mcpwm_config_t pwm_config;
pwm_config.frequency = 1000; //frequency = 500Hz,
pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 0
pwm_config.cmpr_b = 0; //duty cycle of PWMxb = 0
pwm_config.counter_mode = MCPWM_UP_COUNTER;
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
while (1) {
brushed_motor_forward(MCPWM_UNIT_0, MCPWM_TIMER_0, 50.0);
vTaskDelay(2000 / portTICK_RATE_MS);
brushed_motor_backward(MCPWM_UNIT_0, MCPWM_TIMER_0, 30.0);
vTaskDelay(2000 / portTICK_RATE_MS);
brushed_motor_stop(MCPWM_UNIT_0, MCPWM_TIMER_0);
vTaskDelay(2000 / portTICK_RATE_MS);
/* motor moves in forward direction, with duty cycle = duty % */
if (duty_cycle > 0) {
mcpwm_set_signal_low(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, MCPWM_OPR_A);
mcpwm_set_duty(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, MCPWM_OPR_B, duty_cycle);
mcpwm_set_duty_type(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //call this each time, if operator was previously in low/high state
}
/* motor moves in backward direction, with duty cycle = -duty % */
else {
mcpwm_set_signal_low(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, MCPWM_OPR_B);
mcpwm_set_duty(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, MCPWM_OPR_A, -duty_cycle);
mcpwm_set_duty_type(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, MCPWM_OPR_A, MCPWM_DUTY_MODE_0); //call this each time, if operator was previously in low/high state
}
}
/**
* @brief start motor
*
* @param mc mcpwm_motor_control_t pointer
*/
void brushed_motor_start(mcpwm_motor_control_t *mc)
{
motor_ctrl_timer_start();
mc->sec_cnt = 0;
mc->start_flag = true;
}
/**
* @brief stop motor
*
* @param mc mcpwm_motor_control_t pointer
*/
void brushed_motor_stop(mcpwm_motor_control_t *mc)
{
mc->expt = 0;
mc->sec_cnt = 0;
mc->start_flag = false;
motor_ctrl_timer_stop();
brushed_motor_set_duty(0);
}
/**
* @brief The callback function of timer interrupt
* @note This callback is called by timer interrupt callback. It need to offer the PCNT pulse in one control period for PID calculation
* @param args the rotary_encoder_t pointer, it is given by timer interrupt callback
* @return
* - int: the PCNT pulse in one control period
*/
static int pcnt_get_pulse_callback(void *args)
{
/* Record the last count value */
static unsigned int last_pulse = 0;
/* Get the encoder from args */
rotary_encoder_t *encoder = (rotary_encoder_t *)args;
/* Get the value current count value */
unsigned int temp = encoder->get_counter_value(encoder);
/* Calculate the pulse count in one control period */
unsigned int ret = temp - last_pulse;
/* Update last count value */
last_pulse = temp;
return (int)ret;
}
/**
* @brief Initialize the PCNT rotaty encoder
*/
static void motor_ctrl_default_init(void)
{
motor_ctrl.cfg.pid_enable = true;
motor_ctrl.pid_param.kp = 0.8;
motor_ctrl.pid_param.ki = 0.0;
motor_ctrl.pid_param.kd = 0.1;
motor_ctrl.pid_param.cal_type = PID_CAL_TYPE_INCREMENTAL;
motor_ctrl.pid_param.max_output = 100;
motor_ctrl.pid_param.min_output = -100;
motor_ctrl.pid_param.max_integral = 1000;
motor_ctrl.pid_param.min_integral = -1000;
motor_ctrl.cfg.expt_init = 30;
motor_ctrl.cfg.expt_mode = MOTOR_CTRL_MODE_TRIANGLE;
motor_ctrl.cfg.expt_max = 50;
motor_ctrl.cfg.expt_min = -50;
motor_ctrl.cfg.expt_pace = 1.0;
motor_ctrl.cfg.pwm_freq = 1000;
motor_ctrl.cfg.running_sec = 10;
motor_ctrl.cfg.ctrl_period = 10;
}
/**
* @brief Initialize the PCNT rotaty encoder
*/
static void motor_ctrl_pcnt_rotary_encoder_init(void)
{
/* Rotary encoder underlying device is represented by a PCNT unit in this example */
uint32_t pcnt_unit = 0;
/* Create rotary encoder instance */
rotary_encoder_config_t config = ROTARY_ENCODER_DEFAULT_CONFIG(
(rotary_encoder_dev_t)pcnt_unit,
GPIO_PCNT_PINA, GPIO_PCNT_PINB);
ESP_ERROR_CHECK(rotary_encoder_new_ec11(&config, &motor_ctrl.encoder));
/* Filter out glitch (1us) */
ESP_ERROR_CHECK(motor_ctrl.encoder->set_glitch_filter(motor_ctrl.encoder, 1));
/* Start encoder */
ESP_ERROR_CHECK(motor_ctrl.encoder->start(motor_ctrl.encoder));
pcnt_counter_clear((pcnt_unit_t)pcnt_unit);
}
/**
* @brief Initialize the MCPWM
*/
static void motor_ctrl_mcpwm_init(void)
{
/* mcpwm gpio initialization */
mcpwm_example_gpio_initialize();
/* initial mcpwm configuration */
printf("Configuring Initial Parameters of mcpwm...\n");
mcpwm_config_t pwm_config;
pwm_config.frequency = motor_ctrl.cfg.pwm_freq; //frequency = 1kHz,
pwm_config.cmpr_a = 0; //initial duty cycle of PWMxA = 0
pwm_config.cmpr_b = 0; //initial duty cycle of PWMxb = 0
pwm_config.counter_mode = MCPWM_UP_COUNTER; //up counting mode
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
mcpwm_init(MOTOR_CTRL_MCPWM_UNIT, MOTOR_CTRL_MCPWM_TIMER, &pwm_config); //Configure PWM0A & PWM0B with above settings
}
/**
* @brief Initialize the timer
*/
static void motor_ctrl_timer_init(void)
{
/* Initialize timer alarm event queue */
motor_ctrl.timer_evt_que = xQueueCreate(10, sizeof(motor_ctrl_timer_info_t));
/* Set PCNT rotary encoder handler and pulse getting callback function */
pulse_info_t pulse_info = {.callback_args = motor_ctrl.encoder,
.get_pulse_callback = pcnt_get_pulse_callback
};
motor_ctrl_new_timer(&motor_ctrl.timer_info, motor_ctrl.timer_evt_que, motor_ctrl.cfg.ctrl_period, pulse_info);
}
/**
* @brief the top initialization function in this example
*/
static void motor_ctrl_init_all(void)
{
/* 1. Set default configurations */
motor_ctrl_default_init();
/* 2.rotary encoder initialization */
motor_ctrl_pcnt_rotary_encoder_init();
/* 3.MCPWM initialization */
motor_ctrl_mcpwm_init();
/* 4.pid_ctrl initialization */
pid_ctrl_config_t pid_config = {
.init_param = motor_ctrl.pid_param,
};
pid_new_control_block(&pid_config, &motor_ctrl.pid);
/* 5.Timer initialization */
motor_ctrl_timer_init();
}
/**
* @brief Motor control thread
*
* @param arg Information pointer transmitted by task creating function
*/
static void mcpwm_brushed_motor_ctrl_thread(void *arg)
{
motor_ctrl_timer_info_t recv_info;
while (1) {
/* Wait for recieving information of timer interrupt from timer event queque */
xQueueReceive(motor_ctrl.timer_evt_que, &recv_info, portMAX_DELAY);
/* Get the pcnt pulse during one control period */
motor_ctrl.pulse_in_one_period = recv_info.pulse_info.pulse_cnt;
if (motor_ctrl.cfg.pid_enable) {
/* Calculate the output by PID algorithm according to the pulse. Pid_output here is the duty of MCPWM */
motor_ctrl.error = motor_ctrl.expt - motor_ctrl.pulse_in_one_period;
pid_compute(motor_ctrl.pid, motor_ctrl.error, &motor_ctrl.pid_output);
} else {
motor_ctrl.pid_output = motor_ctrl.expt;
}
/* Set the MCPWM duty */
brushed_motor_set_duty(motor_ctrl.pid_output);
}
}
/**
* @brief Motor control thread
*
* @param arg Information pointer transmitted by task creating function
*/
static void mcpwm_brushed_motor_expt_thread(void *arg)
{
float cnt = 0;
while (1) {
xSemaphoreTake(g_motor_mux, portMAX_DELAY);
switch (motor_ctrl.cfg.expt_mode) {
/* Static expectation */
case MOTOR_CTRL_MODE_FIXED:
motor_ctrl.expt = motor_ctrl.cfg.expt_init;
break;
/* Dynamic expectation: triangle wave */
case MOTOR_CTRL_MODE_TRIANGLE:
motor_ctrl.expt += motor_ctrl.cfg.expt_pace;
motor_ctrl.cfg.expt_pace = (motor_ctrl.expt > motor_ctrl.cfg.expt_max - 0.0001 ||
motor_ctrl.expt < motor_ctrl.cfg.expt_min + 0.0001) ?
- motor_ctrl.cfg.expt_pace : motor_ctrl.cfg.expt_pace;
break;
/* Dynamic expectation: rectangle wave */
case MOTOR_CTRL_MODE_RECTANGLE:
cnt += motor_ctrl.cfg.expt_pace;
if (cnt > motor_ctrl.cfg.expt_max - 0.0001) {
motor_ctrl.cfg.expt_pace = -motor_ctrl.cfg.expt_pace;
motor_ctrl.expt = motor_ctrl.cfg.expt_min;
}
if (cnt < motor_ctrl.cfg.expt_min - 0.0001) {
motor_ctrl.cfg.expt_pace = -motor_ctrl.cfg.expt_pace;
motor_ctrl.expt = motor_ctrl.cfg.expt_max;
}
break;
default:
motor_ctrl.expt = motor_ctrl.cfg.expt_init;
break;
}
xSemaphoreGive(g_motor_mux);
/* Motor automatic stop judgement */
if (motor_ctrl.start_flag) {
motor_ctrl.sec_cnt++;
/* Show the seconds count */
if ((motor_ctrl.sec_cnt + 1) % 20 == 0) {
printf("%d\n", (motor_ctrl.sec_cnt + 1) / 20);
}
/* Stop motor if time up */
if (motor_ctrl.sec_cnt > 20 * motor_ctrl.cfg.running_sec && motor_ctrl.cfg.running_sec != 0) {
brushed_motor_stop(&motor_ctrl);
printf("\nTime up: motor stoped\n");
}
}
/* Delay 50ms */
vTaskDelay(50 / portTICK_PERIOD_MS);
}
}
/**
* @brief The main entry of this example
*/
void app_main(void)
{
printf("Testing brushed motor...\n");
xTaskCreate(mcpwm_example_brushed_motor_control, "mcpwm_examlpe_brushed_motor_control", 4096, NULL, 5, NULL);
printf("Testing brushed motor with PID...\n");
/* Create semaphore */
g_motor_mux = xSemaphoreCreateMutex();
/* Initialize peripherals and modules */
motor_ctrl_init_all();
/* Initialize the console */
cmd_mcpwm_motor_init(&motor_ctrl);
/* Motor control thread */
xTaskCreate(mcpwm_brushed_motor_ctrl_thread, "mcpwm_brushed_motor_ctrl_thread", 4096, NULL, 3, NULL);
/* Motor expectation wave generate thread */
xTaskCreate(mcpwm_brushed_motor_expt_thread, "mcpwm_brushed_motor_expt_thread", 4096, NULL, 5, NULL);
}

99
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.h Normal file
View File

@ -0,0 +1,99 @@
/* cmd_mcpwm_motor.h
This example code is in the Public Domain (or CC0 licensed, at your option.)
Unless required by applicable law or agreed to in writing, this
software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
CONDITIONS OF ANY KIND, either express or implied.
*/
#pragma once
#include "rotary_encoder.h"
#include "motor_ctrl_timer.h"
#include "pid_ctrl.h"
#ifdef __cplusplus
extern "C" {
#endif
#define GPIO_PWM0A_OUT 15 //Set GPIO 15 as PWM0A
#define GPIO_PWM0B_OUT 16 //Set GPIO 16 as PWM0B
#define GPIO_PCNT_PINA 18 //Set GPIO 18 as phaseA/C1
#define GPIO_PCNT_PINB 19 //Set GPIO 19 as phaseB/C2
typedef enum {
MOTOR_CTRL_MODE_FIXED = 0,
MOTOR_CTRL_MODE_TRIANGLE,
MOTOR_CTRL_MODE_RECTANGLE
} expect_mode_t;
typedef struct {
/* Handles */
rotary_encoder_t *encoder; // PCNT rotary encoder handler
motor_ctrl_timer_info_t *timer_info; // Timer infomation handler
pid_ctrl_block_handle_t pid; // PID algoritm handler
pid_ctrl_parameter_t pid_param; // PID parameters
QueueHandle_t timer_evt_que; // Timer event queue handler
/* Control visualization */
int pulse_in_one_period; // PCNT pulse in one control period
float error; // The error between the expectation(expt) and actual value (pulse_in_one_period)
float expt; // The expectation
float pid_output; // PID algorithm output
/* Status */
unsigned int sec_cnt; // Seconds count
bool start_flag; // Motor start flag
/* Configurations */
struct {
/* PID configuration */
bool pid_enable; // PID enable flag
/* Expectation configuration */
float expt_init; // Initial expectation
float expt_max; // Max expectation in dynamic mode
float expt_min; // Min expectation in dynamic mode
float expt_pace; // The expection pace. It can change expectation wave period
expect_mode_t expt_mode; // Expectation wave mode (MOTOR_CTRL_EXPT_FIXED/MOTOR_CTRL_EXPT_TRIANGLE/MOTOR_CTRL_EXPT_RECTANGLE)
/* Other configurations */
unsigned int ctrl_period; // Control period
unsigned int pwm_freq; // MCPWM output frequency
unsigned int running_sec; // Motor running seconds
} cfg; // Configurations that should be initialized for this example
} mcpwm_motor_control_t;
/**
* @brief Set pwm duty to drive the motor
*
* @param duty_cycle PWM duty cycle (100~-100), the motor will go backward if the duty is set to a negative value
*/
void brushed_motor_set_duty(float duty_cycle);
/**
* @brief start motor
*
* @param mc mcpwm_motor_control_t pointer
*/
void brushed_motor_start(mcpwm_motor_control_t *mc);
/**
* @brief stop motor
*
* @param mc mcpwm_motor_control_t pointer
*/
void brushed_motor_stop(mcpwm_motor_control_t *mc);
/**
* @brief Initialize the motor control console
*
* @param motor_ctrl The top infomation struct of this example
*/
extern void cmd_mcpwm_motor_init(mcpwm_motor_control_t *motor_ctrl);
#ifdef __cplusplus
}
#endif