diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/CMakeLists.txt b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/CMakeLists.txt index a4baf8b40c..b9da4a4f67 100644 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/CMakeLists.txt +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/CMakeLists.txt @@ -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) diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/Makefile b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/Makefile index c85d17604e..b7b8924a5a 100644 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/Makefile +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/Makefile @@ -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 diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/README.md b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/README.md index 41ba39935d..c0f056c57a 100644 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/README.md +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/README.md @@ -4,35 +4,49 @@ (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 +Before project configuration and build, be sure to set the correct chip target using `idf.py set-target `. + ### 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 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 quadrature encoder to detect speed Connection : ``` - Power (12V) - ^ - | -+----------------+ +------------+--------------+ +-------------+ -| | | | | | -| GPIO15+------ PWM0A +-+ IN_A +------+ +-------+ OUT_A +------+ Brushed | -| ESP | | H-Bridge | | DC | -| GPIO16+------ PWM0B +-+ IN_B +------+ +-------+ OUT_B +------+ Motor | -| | | | | | -+--------+-------+ +------------+--------------+ +-------------+ - | | - +------------------------------------------------->+ - | - v - GND + 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 @@ -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-]``.) -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 Run the example, you will see the following output log: -``` -... -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 +```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 | + | | + ================================================================= + +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= Enable or disable PID algorithm + -T, --period= Set motor control period + -s, --show Show current configurations + +expt expt -i -m -p --max --min -50 + Set initial value, limitation and wave mode of expectation. Both dynamic and + static mode are available + --max= Max limitation for dynamic expectation + --min= Min limitation for dynamic expectation + -p, --pace= The increasing pace of expectation every 50ms + -i, --init= Initial expectation. Usually between -100~100 + -m, --mode= Select static or dynamic expectation wave mode. 'fixed' for static, 'tri' for triangle, 'rect' for rectangle + +pid pid -p -i -d -t + Set parameters and type for PID algorithm + -p, --kp= Set Kp value for PID + -i, --ki= Set Ki value for PID + -d, --kd= Set Kd value for PID + -t, --type= Select locational PID or incremental PID + +motor motor -u 10 + Start or stop the motor + -u, --start= 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' - 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 -i -d -t ` +* '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 -m -p --max --min ` +* '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 ` +* Command: `motor -d` +* 'u' - start the motor in seconds, if 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. +For any technical queries, please open an [issue](https://github.com/espressif/esp-idf/issues) on GitHub. We will get back to you soon. diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/CMakeLists.txt b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/CMakeLists.txt new file mode 100644 index 0000000000..4f440a7b25 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/CMakeLists.txt @@ -0,0 +1,5 @@ +set(COMPONENT_SRCS "motor_ctrl_timer.c") + +idf_component_register(SRCS "${COMPONENT_SRCS}" + INCLUDE_DIRS . + PRIV_REQUIRES "driver") diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/component.mk b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/component.mk new file mode 100644 index 0000000000..27ad11a7e5 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/component.mk @@ -0,0 +1 @@ +COMPONENT_ADD_INCLUDEDIRS := . diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.c b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.c new file mode 100644 index 0000000000..1a76ad2485 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.c @@ -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 +#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; + } +} diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.h b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.h new file mode 100644 index 0000000000..ca9646fa3d --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/motor_ctrl_timer/motor_ctrl_timer.h @@ -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 diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/CMakeLists.txt b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/CMakeLists.txt new file mode 100644 index 0000000000..7f16dda52e --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/CMakeLists.txt @@ -0,0 +1,5 @@ +set(COMPONENT_SRCS "pid_ctrl.c") + +idf_component_register(SRCS "${COMPONENT_SRCS}" + INCLUDE_DIRS . + PRIV_REQUIRES "driver") diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/component.mk b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/component.mk new file mode 100644 index 0000000000..27ad11a7e5 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/component.mk @@ -0,0 +1 @@ +COMPONENT_ADD_INCLUDEDIRS := . diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.c b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.c new file mode 100644 index 0000000000..7f8d826658 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.c @@ -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 +#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; + } +} diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.h b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.h new file mode 100644 index 0000000000..01d8e59acb --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.h @@ -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 diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/CMakeLists.txt b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/CMakeLists.txt index d0ee6ea1b6..e28b2d51c8 100644 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/CMakeLists.txt +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/CMakeLists.txt @@ -1,2 +1,5 @@ -idf_component_register(SRCS "mcpwm_brushed_dc_example_main.c" - INCLUDE_DIRS ".") +set(COMPONENT_SRCS "mcpwm_brushed_dc_control_example.c" + "cmd_mcpwm_motor.c") + +idf_component_register(SRCS "${COMPONENT_SRCS}" + INCLUDE_DIRS "./") diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/cmd_mcpwm_motor.c b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/cmd_mcpwm_motor.c new file mode 100644 index 0000000000..33401ec2b9 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/cmd_mcpwm_motor.c @@ -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 +#include +#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", "", "Enable or disable PID algorithm"); + motor_ctrl_config_args.period = arg_int0("T", "period", "", "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", "", "Initial expectation. Usually between -100~100"); + motor_ctrl_expt_args.max = arg_dbl0(NULL, "max", "", "Max limitation for dynamic expectation"); + motor_ctrl_expt_args.min = arg_dbl0(NULL, "min", "", "Min limitation for dynamic expectation"); + motor_ctrl_expt_args.pace = arg_dbl0("p", "pace", "", "The increasing pace of expectation every 50ms"); + motor_ctrl_expt_args.mode = arg_str0("m", "mode", "", + "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 -m -p --max --min ", + .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", "", "Set Kp value for PID"); + motor_ctrl_pid_args.ki = arg_dbl0("i", "ki", "", "Set Ki value for PID"); + motor_ctrl_pid_args.kd = arg_dbl0("d", "kd", "", "Set Kd value for PID"); + motor_ctrl_pid_args.type = arg_str0("t", "type", "", "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 -i -d -t ", + .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", "", "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)); +} diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.c b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.c new file mode 100644 index 0000000000..eb3f160157 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.c @@ -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 + +#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); +} diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.h b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.h new file mode 100644 index 0000000000..837c4269f8 --- /dev/null +++ b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.h @@ -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 diff --git a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_example_main.c b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_example_main.c deleted file mode 100644 index 40c4fb0c12..0000000000 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_example_main.c +++ /dev/null @@ -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)); - } -}