mirror of
https://github.com/espressif/esp-idf
synced 2025-03-10 09:39:10 -04:00
mcpwm: DC motor PID control example
This commit is contained in:
laokaiyao
morris
parent
768af636a6
commit
b6c5a6ee8b
@ -2,5 +2,8 @@
|
|||||||
# in this exact order for cmake to work correctly
|
# in this exact order for cmake to work correctly
|
||||||
cmake_minimum_required(VERSION 3.5)
|
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)
|
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
|
||||||
project(mcpwm_brushed_dc_control)
|
project(mcpwm_brushed_dc_control)
|
||||||
|
|||||||
@ -5,4 +5,7 @@
|
|||||||
|
|
||||||
PROJECT_NAME := mcpwm_brushed_dc_control
|
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
|
include $(IDF_PATH)/make/project.mk
|
||||||
|
|||||||
@ -4,35 +4,49 @@
|
|||||||
|
|
||||||
(See the README.md file in the upper level 'examples' directory for more information about examples.)
|
(See the README.md file in the upper level 'examples' directory for more information about examples.)
|
||||||
|
|
||||||
This example mainly illustrates how to drive a brushed DC motor by generating two specific PWM signals. We used [L298N](https://www.st.com/content/st_com/en/products/motor-drivers/brushed-dc-motor-drivers/l298.html) as the H-bridge driver to provide the needed voltage and current for brushed DC motor.
|
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
|
## 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
|
### Hardware Required
|
||||||
|
|
||||||
* A development board with any Espressif SoC which features MCPWM peripheral (e.g., ESP32-DevKitC, ESP-WROVER-KIT, etc.)
|
* 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 USB cable for Power supply and programming
|
||||||
* A separate 12V power for brushed DC (the voltage depends on the motor model used in the example)
|
* 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 brushed DC motor, e.g. [25GA370](http://www.tronsunmotor.com/data/upload/file/201807/e03b98802b5c5390d6570939def525ba.pdf)
|
||||||
|
* A quadrature encoder to detect speed
|
||||||
|
|
||||||
Connection :
|
Connection :
|
||||||
```
|
```
|
||||||
Power (12V)
|
Power(12V)
|
||||||
^
|
|
|
||||||
|
|
v
|
||||||
+----------------+ +------------+--------------+ +-------------+
|
+----------------+ +--------------------+
|
||||||
| | | | | |
|
| | | H-Bridge |
|
||||||
| GPIO15+------ PWM0A +-+ IN_A +------+ +-------+ OUT_A +------+ Brushed |
|
| GND +------------>| | +--------------+
|
||||||
| ESP | | H-Bridge | | DC |
|
| | | | | |
|
||||||
| GPIO16+------ PWM0B +-+ IN_B +------+ +-------+ OUT_B +------+ Motor |
|
| GPIO15 +----PWM0A--->| IN_A OUT_A +----->| Brushed |
|
||||||
| | | | | |
|
| | | | | DC |
|
||||||
+--------+-------+ +------------+--------------+ +-------------+
|
| GPIO16 +----PWM0B--->| IN_A OUT_B +----->| Motor |
|
||||||
| |
|
| | | | | |
|
||||||
+------------------------------------------------->+
|
| ESP | +--------------------+ | |
|
||||||
|
|
| | +------+-------+
|
||||||
v
|
| | |
|
||||||
GND
|
| | +--------------------+ |
|
||||||
|
| 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
|
### Build and Flash
|
||||||
|
|
||||||
@ -40,29 +54,193 @@ Run `idf.py -p PORT flash monitor` to build, flash and monitor the project.
|
|||||||
|
|
||||||
(To exit the serial monitor, type ``Ctrl-]``.)
|
(To exit the serial monitor, type ``Ctrl-]``.)
|
||||||
|
|
||||||
See the [Getting Started Guide](https://docs.espressif.com/projects/esp-idf/en/latest/get-started/index.html) for full steps to configure and use ESP-IDF to build projects.
|
See the [Getting Started Guide](https://idf.espressif.com/) for full steps to configure and use ESP-IDF to build projects.
|
||||||
|
|
||||||
|
|
||||||
## Example Output
|
## Example Output
|
||||||
|
|
||||||
Run the example, you will see the following output log:
|
Run the example, you will see the following output log:
|
||||||
|
|
||||||
```
|
```bash
|
||||||
...
|
|
||||||
I (0) cpu_start: Starting scheduler on APP CPU.
|
|
||||||
I (350) example: running forward
|
|
||||||
I (2350) example: running backward
|
|
||||||
I (4350) example: stop
|
|
||||||
I (6350) example: running forward
|
|
||||||
I (8350) example: running backward
|
|
||||||
I (10350) example: stop
|
|
||||||
...
|
...
|
||||||
|
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 |
|
||||||
|
| |
|
||||||
|
=================================================================
|
||||||
|
|
||||||
|
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>
|
||||||
```
|
```
|
||||||
|
|
||||||
Motor first moves forward, then backward and then stops for 2 seconds each, continuously.
|
### 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
|
## Troubleshooting
|
||||||
|
|
||||||
* Make sure your ESP board and H-bridge module have been connected to the same GND panel.
|
* 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.
|
For any technical queries, please open an [issue](https://github.com/espressif/esp-idf/issues) on GitHub. We will get back to you soon.
|
||||||
|
|||||||
@ -0,0 +1,5 @@
|
|||||||
|
set(COMPONENT_SRCS "motor_ctrl_timer.c")
|
||||||
|
|
||||||
|
idf_component_register(SRCS "${COMPONENT_SRCS}"
|
||||||
|
INCLUDE_DIRS .
|
||||||
|
PRIV_REQUIRES "driver")
|
||||||
@ -0,0 +1 @@
|
|||||||
|
COMPONENT_ADD_INCLUDEDIRS := .
|
||||||
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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
|
||||||
@ -0,0 +1,5 @@
|
|||||||
|
set(COMPONENT_SRCS "pid_ctrl.c")
|
||||||
|
|
||||||
|
idf_component_register(SRCS "${COMPONENT_SRCS}"
|
||||||
|
INCLUDE_DIRS .
|
||||||
|
PRIV_REQUIRES "driver")
|
||||||
@ -0,0 +1 @@
|
|||||||
|
COMPONENT_ADD_INCLUDEDIRS := .
|
||||||
@ -0,0 +1,123 @@
|
|||||||
|
/* General Purpose PID example
|
||||||
|
|
||||||
|
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 "esp_check.h"
|
||||||
|
#include "pid_ctrl.h"
|
||||||
|
|
||||||
|
static const char *TAG = "pid_ctrl";
|
||||||
|
|
||||||
|
static float pid_calc_location(pid_ctrl_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 = pid->integral_err > pid->max_integral_limit ? pid->max_integral_limit : pid->integral_err;
|
||||||
|
pid->integral_err = pid->integral_err < pid->min_integral_limit ? pid->min_integral_limit : pid->integral_err;
|
||||||
|
|
||||||
|
/* 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 = output > pid->max_output_limit ? pid->max_output_limit : output;
|
||||||
|
output = output < pid->min_output_limit ? pid->min_output_limit : output;
|
||||||
|
|
||||||
|
/* Update previous error */
|
||||||
|
pid->previous_err1 = error;
|
||||||
|
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
static float pid_calc_increment(pid_ctrl_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 = output > pid->max_output_limit ? pid->max_output_limit : output;
|
||||||
|
output = output < pid->min_output_limit ? pid->min_output_limit : 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_integral_clear(pid_ctrl_t *pid)
|
||||||
|
{
|
||||||
|
esp_err_t ret;
|
||||||
|
/* Check the input pointer */
|
||||||
|
ESP_GOTO_ON_FALSE(pid, ESP_ERR_INVALID_ARG, err, TAG, "Input a NULL pointer\n");
|
||||||
|
|
||||||
|
/* Clear the integral error in pid structure */
|
||||||
|
pid->integral_err = 0;
|
||||||
|
err:
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
esp_err_t pid_init(float Kp, float Ki, float Kd, pid_calculate_type_e type, pid_ctrl_t **pid)
|
||||||
|
{
|
||||||
|
esp_err_t ret = ESP_OK;
|
||||||
|
/* Check the input pointer */
|
||||||
|
ESP_GOTO_ON_FALSE(pid, ESP_ERR_INVALID_ARG, err, TAG, "Input a NULL pointer\n");
|
||||||
|
|
||||||
|
*pid = calloc(1, sizeof(pid_ctrl_t));
|
||||||
|
ESP_GOTO_ON_FALSE((*pid), ESP_ERR_NO_MEM, err, TAG, "There is no memory for PID structure\n");
|
||||||
|
|
||||||
|
/* Set the PID parameters */
|
||||||
|
(*pid)->Kp = Kp;
|
||||||
|
(*pid)->Ki = Ki;
|
||||||
|
(*pid)->Kd = Kd;
|
||||||
|
|
||||||
|
/* Select the PID work mode */
|
||||||
|
(*pid)->type = type;
|
||||||
|
|
||||||
|
/* Initialize the pid history variables */
|
||||||
|
(*pid)->previous_err1 = 0;
|
||||||
|
(*pid)->previous_err2 = 0;
|
||||||
|
(*pid)->integral_err = 0;
|
||||||
|
(*pid)->last_output = 0;
|
||||||
|
|
||||||
|
/* Set pid default limitation */
|
||||||
|
(*pid)->max_output_limit = 100;
|
||||||
|
(*pid)->min_output_limit = -100;
|
||||||
|
(*pid)->max_integral_limit = 1000;
|
||||||
|
(*pid)->max_integral_limit = -1000;
|
||||||
|
|
||||||
|
/* Set the calculate function according to the PID type */
|
||||||
|
(*pid)->calculate_func =
|
||||||
|
(*pid)->type == PID_INCREMENT ? pid_calc_increment : pid_calc_location;
|
||||||
|
|
||||||
|
err:
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pid_deinit(pid_ctrl_t **pid)
|
||||||
|
{
|
||||||
|
if((*pid) != NULL)
|
||||||
|
{
|
||||||
|
free(*pid);
|
||||||
|
*pid = NULL;
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,110 @@
|
|||||||
|
/* General Purpose PID example
|
||||||
|
|
||||||
|
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 "esp_err.h"
|
||||||
|
|
||||||
|
#define LOCATION_PID_DEFAULT_CONFIG(p, i, d, max_o, min_o, max_i, min_i) \
|
||||||
|
{ \
|
||||||
|
.Kp = p, \
|
||||||
|
.Ki = i, \
|
||||||
|
.Kd = d, \
|
||||||
|
.max_output_limit = max_o, \
|
||||||
|
.min_output_limit = min_o, \
|
||||||
|
.max_integral_limit = max_i, \
|
||||||
|
.min_integral_limit = min_i, \
|
||||||
|
.type = PID_LOCATION \
|
||||||
|
}
|
||||||
|
|
||||||
|
#define INCREMENT_PID_DEFAULT_CONFIG(p, i, d, max_o, min_o, max_i, min_i, init_out) \
|
||||||
|
{ \
|
||||||
|
.Kp = p, \
|
||||||
|
.Ki = i, \
|
||||||
|
.Kd = d, \
|
||||||
|
.max_output_limit = max_o, \
|
||||||
|
.min_output_limit = min_o, \
|
||||||
|
.max_integral_limit = max_i, \
|
||||||
|
.min_integral_limit = min_i, \
|
||||||
|
.last_output = init_out, \
|
||||||
|
.type = PID_INCREMENT \
|
||||||
|
}
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
PID_INCREMENT = 0,
|
||||||
|
PID_LOCATION
|
||||||
|
} pid_calculate_type_e;
|
||||||
|
|
||||||
|
typedef struct pid_ctrl pid_ctrl_t;
|
||||||
|
|
||||||
|
struct pid_ctrl{
|
||||||
|
/* pid parameters (Set by user) */
|
||||||
|
float Kp; // PID Kp value
|
||||||
|
float Ki; // PID Ki value
|
||||||
|
float Kd; // PID Kd value
|
||||||
|
|
||||||
|
/* history variables */
|
||||||
|
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
|
||||||
|
|
||||||
|
/* limitation */
|
||||||
|
float max_output_limit; // PID maximum output limitation
|
||||||
|
float min_output_limit; // PID minimum output limitation
|
||||||
|
float max_integral_limit; // PID maximum integral value limitation
|
||||||
|
float min_integral_limit; // PID minimum integral value limitation
|
||||||
|
|
||||||
|
/* PID calculation function type (Set by user) */
|
||||||
|
pid_calculate_type_e type; // PID calculation type
|
||||||
|
|
||||||
|
/* PID calculation function */
|
||||||
|
float (*calculate_func)(pid_ctrl_t *pid, float error); // PID calculation function pointer
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief PID initialization
|
||||||
|
*
|
||||||
|
* @param Kp PID kp value
|
||||||
|
* @param Ki PID ki value
|
||||||
|
* @param Kd PID kd value
|
||||||
|
* @param type PID calculation type
|
||||||
|
* @param pid Secondary poiter of pid control struct
|
||||||
|
* @return
|
||||||
|
* - ESP_OK: PID initialized successfully
|
||||||
|
* - ESP_ERR_INVALID_ARG: The secondary pointer is NULL
|
||||||
|
* - ESP_ERR_NO_MEM: There is no memory for PID structure
|
||||||
|
*/
|
||||||
|
esp_err_t pid_init(float Kp, float Ki, float Kd, pid_calculate_type_e type, pid_ctrl_t **pid);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief PID deinitialization
|
||||||
|
*
|
||||||
|
* @param pid Secondary poiter of pid control struct
|
||||||
|
*/
|
||||||
|
void pid_deinit(pid_ctrl_t **pid);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief
|
||||||
|
*
|
||||||
|
* @param pid The pointer of pid control struct
|
||||||
|
* @return
|
||||||
|
* - ESP_OK: PID integral value cleared successfully
|
||||||
|
* - ESP_ERR_INVALID_ARG: The pointer is NULL
|
||||||
|
*/
|
||||||
|
esp_err_t pid_integral_clear(pid_ctrl_t *pid);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
@ -1,2 +1,5 @@
|
|||||||
idf_component_register(SRCS "mcpwm_brushed_dc_example_main.c"
|
set(COMPONENT_SRCS "mcpwm_brushed_dc_control_example.c"
|
||||||
INCLUDE_DIRS ".")
|
"cmd_mcpwm_motor.c")
|
||||||
|
|
||||||
|
idf_component_register(SRCS "${COMPONENT_SRCS}"
|
||||||
|
INCLUDE_DIRS "./")
|
||||||
|
|||||||
@ -0,0 +1,306 @@
|
|||||||
|
/* 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->type == PID_LOCATION) ? "Location" : "Increment",
|
||||||
|
mc->pid->Kp, mc->pid->Ki, mc->pid->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)
|
||||||
|
{
|
||||||
|
MOTOR_CTRL_CMD_CHECK(motor_ctrl_pid_args);
|
||||||
|
xSemaphoreTake(g_motor_mux, portMAX_DELAY);
|
||||||
|
if (motor_ctrl_pid_args.kp->count) {
|
||||||
|
mc->pid->Kp = motor_ctrl_pid_args.kp->dval[0];
|
||||||
|
printf("pid: kp = %.3f\n", mc->pid->Kp);
|
||||||
|
}
|
||||||
|
if (motor_ctrl_pid_args.ki->count) {
|
||||||
|
mc->pid->Ki = motor_ctrl_pid_args.ki->dval[0];
|
||||||
|
printf("pid: ki = %.3f\n", mc->pid->Ki);
|
||||||
|
}
|
||||||
|
if (motor_ctrl_pid_args.kd->count) {
|
||||||
|
mc->pid->Kd = motor_ctrl_pid_args.kd->dval[0];
|
||||||
|
printf("pid: kd = %.3f\n", mc->pid->Kd);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (motor_ctrl_pid_args.type->count) {
|
||||||
|
if (!strcmp(*motor_ctrl_pid_args.type->sval, "loc")) {
|
||||||
|
mc->pid->type = PID_LOCATION;
|
||||||
|
printf("pid: type = location\n");
|
||||||
|
} else if (!strcmp(*motor_ctrl_pid_args.type->sval, "inc")) {
|
||||||
|
mc->pid->type = PID_INCREMENT;
|
||||||
|
printf("pid: type = increment\n");
|
||||||
|
} else {
|
||||||
|
mc->pid->type = PID_INCREMENT;
|
||||||
|
printf("pid: type = increment\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
xSemaphoreGive(g_motor_mux);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
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));
|
||||||
|
}
|
||||||
@ -0,0 +1,307 @@
|
|||||||
|
/* brushed dc motor control example
|
||||||
|
|
||||||
|
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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This example will show you how to use MCPWM module to control brushed dc motor.
|
||||||
|
* This code is tested with L298 motor driver.
|
||||||
|
* User may need to make changes according to the motor driver they use.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#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"
|
||||||
|
|
||||||
|
#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(MOTOR_CTRL_MCPWM_UNIT, MCPWM0A, GPIO_PWM0A_OUT);
|
||||||
|
mcpwm_gpio_init(MOTOR_CTRL_MCPWM_UNIT, MCPWM0B, GPIO_PWM0B_OUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief set motor moves speed and direction with duty cycle = duty %
|
||||||
|
*/
|
||||||
|
void brushed_motor_set_duty(float duty_cycle)
|
||||||
|
{
|
||||||
|
/* 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.cfg.pid_init_kp = 0.8;
|
||||||
|
motor_ctrl.cfg.pid_init_ki = 0.0;
|
||||||
|
motor_ctrl.cfg.pid_init_kd = 0.1;
|
||||||
|
motor_ctrl.cfg.pid_init_type = PID_INCREMENT;
|
||||||
|
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_init(motor_ctrl.cfg.pid_init_kp,
|
||||||
|
motor_ctrl.cfg.pid_init_ki,
|
||||||
|
motor_ctrl.cfg.pid_init_kd,
|
||||||
|
motor_ctrl.cfg.pid_init_type,
|
||||||
|
&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;
|
||||||
|
motor_ctrl.pid_output = motor_ctrl.pid->calculate_func(motor_ctrl.pid, motor_ctrl.error);
|
||||||
|
} 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 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);
|
||||||
|
}
|
||||||
@ -0,0 +1,102 @@
|
|||||||
|
/* 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_t *pid; // PID algoritm handler
|
||||||
|
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
|
||||||
|
float pid_init_kp; // PID initial kp value
|
||||||
|
float pid_init_ki; // PID initial ki value
|
||||||
|
float pid_init_kd; // PID initial kd value
|
||||||
|
pid_calculate_type_e pid_init_type; // PID initial calcalation type (PID_INCREMENT/PID_LOCATION)
|
||||||
|
|
||||||
|
/* 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
|
||||||
@ -1,73 +0,0 @@
|
|||||||
/* Brushed DC motor control example
|
|
||||||
|
|
||||||
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 "freertos/FreeRTOS.h"
|
|
||||||
#include "freertos/task.h"
|
|
||||||
#include "esp_log.h"
|
|
||||||
#include "driver/mcpwm.h"
|
|
||||||
|
|
||||||
static const char *TAG = "example";
|
|
||||||
|
|
||||||
#define GPIO_PWM0A_OUT (15) //Set GPIO 15 as PWM0A
|
|
||||||
#define GPIO_PWM0B_OUT (16) //Set GPIO 16 as PWM0B
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief motor moves in forward direction, with duty cycle = duty %
|
|
||||||
*/
|
|
||||||
static void brushed_motor_forward(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num, float duty_cycle)
|
|
||||||
{
|
|
||||||
ESP_LOGI(TAG, "running forward");
|
|
||||||
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_B, MCPWM_HAL_GENERATOR_MODE_FORCE_LOW);
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @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)
|
|
||||||
{
|
|
||||||
ESP_LOGI(TAG, "running backward");
|
|
||||||
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_A, MCPWM_HAL_GENERATOR_MODE_FORCE_LOW);
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void brushed_motor_stop(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num)
|
|
||||||
{
|
|
||||||
ESP_LOGI(TAG, "stop");
|
|
||||||
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_A, MCPWM_HAL_GENERATOR_MODE_FORCE_LOW);
|
|
||||||
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_B, MCPWM_HAL_GENERATOR_MODE_FORCE_LOW);
|
|
||||||
}
|
|
||||||
|
|
||||||
void app_main(void)
|
|
||||||
{
|
|
||||||
// Initialize GPIO
|
|
||||||
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_PWM0A_OUT);
|
|
||||||
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, GPIO_PWM0B_OUT);
|
|
||||||
|
|
||||||
// MCPWM configuration
|
|
||||||
mcpwm_config_t pwm_config = {
|
|
||||||
.frequency = 1000,
|
|
||||||
.cmpr_a = 0,
|
|
||||||
.cmpr_b = 0,
|
|
||||||
.counter_mode = MCPWM_UP_COUNTER,
|
|
||||||
.duty_mode = MCPWM_DUTY_MODE_0,
|
|
||||||
};
|
|
||||||
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
brushed_motor_forward(MCPWM_UNIT_0, MCPWM_TIMER_0, 50.0);
|
|
||||||
vTaskDelay(pdMS_TO_TICKS(2000));
|
|
||||||
brushed_motor_backward(MCPWM_UNIT_0, MCPWM_TIMER_0, 30.0);
|
|
||||||
vTaskDelay(pdMS_TO_TICKS(2000));
|
|
||||||
brushed_motor_stop(MCPWM_UNIT_0, MCPWM_TIMER_0);
|
|
||||||
vTaskDelay(pdMS_TO_TICKS(2000));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Loading…
x
Reference in New Issue
Block a user