From f0fab687adfe65b38773f561ea92c32c422b170c Mon Sep 17 00:00:00 2001 From: morris Date: Tue, 10 Aug 2021 10:41:13 +0800 Subject: [PATCH] pid_ctrl: abstract into example common component --- .../common_components/pid_ctrl/CMakeLists.txt | 4 + .../common_components/pid_ctrl/component.mk | 2 + .../pid_ctrl/include/pid_ctrl.h | 100 +++++++++++++ .../common_components/pid_ctrl/src/pid_ctrl.c | 140 ++++++++++++++++++ .../components/pid_ctrl/CMakeLists.txt | 5 - .../components/pid_ctrl/component.mk | 1 - .../components/pid_ctrl/pid_ctrl.c | 123 --------------- .../components/pid_ctrl/pid_ctrl.h | 110 -------------- .../main/cmd_mcpwm_motor.c | 38 ++--- .../main/mcpwm_brushed_dc_control_example.c | 23 +-- .../main/mcpwm_brushed_dc_control_example.h | 7 +- 11 files changed, 281 insertions(+), 272 deletions(-) create mode 100644 examples/common_components/pid_ctrl/CMakeLists.txt create mode 100644 examples/common_components/pid_ctrl/component.mk create mode 100644 examples/common_components/pid_ctrl/include/pid_ctrl.h create mode 100644 examples/common_components/pid_ctrl/src/pid_ctrl.c delete mode 100644 examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/CMakeLists.txt delete mode 100644 examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/component.mk delete mode 100644 examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.c delete mode 100644 examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.h diff --git a/examples/common_components/pid_ctrl/CMakeLists.txt b/examples/common_components/pid_ctrl/CMakeLists.txt new file mode 100644 index 0000000000..544820d5d7 --- /dev/null +++ b/examples/common_components/pid_ctrl/CMakeLists.txt @@ -0,0 +1,4 @@ +set(srcs "src/pid_ctrl.c") + +idf_component_register(SRCS "${srcs}" + INCLUDE_DIRS "include") diff --git a/examples/common_components/pid_ctrl/component.mk b/examples/common_components/pid_ctrl/component.mk new file mode 100644 index 0000000000..33f4c6039d --- /dev/null +++ b/examples/common_components/pid_ctrl/component.mk @@ -0,0 +1,2 @@ +COMPONENT_ADD_INCLUDEDIRS := include +COMPONENT_SRCDIRS := src diff --git a/examples/common_components/pid_ctrl/include/pid_ctrl.h b/examples/common_components/pid_ctrl/include/pid_ctrl.h new file mode 100644 index 0000000000..5aa26df7b6 --- /dev/null +++ b/examples/common_components/pid_ctrl/include/pid_ctrl.h @@ -0,0 +1,100 @@ +/* + * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#include "esp_err.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief PID calculation type + * + */ +typedef enum { + PID_CAL_TYPE_INCREMENTAL, /*!< Incremental PID control */ + PID_CAL_TYPE_POSITIONAL, /*!< Positional PID control */ +} pid_calculate_type_t; + +/** + * @brief Type of PID control block handle + * + */ +typedef struct pid_ctrl_block_t *pid_ctrl_block_handle_t; + +/** + * @brief PID control parameters + * + */ +typedef struct { + float kp; // PID Kp parameter + float ki; // PID Ki parameter + float kd; // PID Kd parameter + float max_output; // PID maximum output limitation + float min_output; // PID minimum output limitation + float max_integral; // PID maximum integral value limitation + float min_integral; // PID minimum integral value limitation + pid_calculate_type_t cal_type; // PID calculation type +} pid_ctrl_parameter_t; + +/** + * @brief PID control configuration + * + */ +typedef struct { + pid_ctrl_parameter_t init_param; // Initial parameters +} pid_ctrl_config_t; + +/** + * @brief Create a new PID control session, returns the handle of control block + * + * @param[in] config PID control configuration + * @param[out] ret_pid Returned PID control block handle + * @return + * - ESP_OK: Created PID control block successfully + * - ESP_ERR_INVALID_ARG: Created PID control block failed because of invalid argument + * - ESP_ERR_NO_MEM: Created PID control block failed because out of memory + */ +esp_err_t pid_new_control_block(const pid_ctrl_config_t *config, pid_ctrl_block_handle_t *ret_pid); + +/** + * @brief Delete the PID control block + * + * @param[in] pid PID control block handle, created by `pid_new_control_block()` + * @return + * - ESP_OK: Delete PID control block successfully + * - ESP_ERR_INVALID_ARG: Delete PID control block failed because of invalid argument + */ +esp_err_t pid_del_control_block(pid_ctrl_block_handle_t pid); + +/** + * @brief Update PID parameters + * + * @param[in] pid PID control block handle, created by `pid_new_control_block()` + * @param[in] params PID parameters + * @return + * - ESP_OK: Update PID parameters successfully + * - ESP_ERR_INVALID_ARG: Update PID parameters failed because of invalid argument + */ +esp_err_t pid_update_parameters(pid_ctrl_block_handle_t pid, const pid_ctrl_parameter_t *params); + +/** + * @brief Input error and get PID control result + * + * @param[in] pid PID control block handle, created by `pid_new_control_block()` + * @param[in] input_error error data that feed to the PID controller + * @param[out] ret_result result after PID calculation + * @return + * - ESP_OK: Run a PID compute successfully + * - ESP_ERR_INVALID_ARG: Run a PID compute failed because of invalid argument + */ +esp_err_t pid_compute(pid_ctrl_block_handle_t pid, float input_error, float *ret_result); + +#ifdef __cplusplus +} +#endif diff --git a/examples/common_components/pid_ctrl/src/pid_ctrl.c b/examples/common_components/pid_ctrl/src/pid_ctrl.c new file mode 100644 index 0000000000..4b51177b41 --- /dev/null +++ b/examples/common_components/pid_ctrl/src/pid_ctrl.c @@ -0,0 +1,140 @@ +/* + * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "esp_check.h" +#include "esp_log.h" +#include "pid_ctrl.h" + +static const char *TAG = "pid_ctrl"; + +typedef struct pid_ctrl_block_t pid_ctrl_block_t; +typedef float (*pid_cal_func_t)(pid_ctrl_block_t *pid, float error); + +struct pid_ctrl_block_t { + float Kp; // PID Kp value + float Ki; // PID Ki value + float Kd; // PID Kd value + float previous_err1; // e(k-1) + float previous_err2; // e(k-2) + float integral_err; // Sum of error + float last_output; // PID output in last control period + float max_output; // PID maximum output limitation + float min_output; // PID minimum output limitation + float max_integral; // PID maximum integral value limitation + float min_integral; // PID minimum integral value limitation + pid_cal_func_t calculate_func; // calculation function, depends on actual PID type set by user +}; + +static float pid_calc_positional(pid_ctrl_block_t *pid, float error) +{ + float output = 0; + /* Add current error to the integral error */ + pid->integral_err += error; + /* If the integral error is out of the range, it will be limited */ + pid->integral_err = MIN(pid->integral_err, pid->max_integral); + pid->integral_err = MAX(pid->integral_err, pid->min_integral); + + /* Calculate the pid control value by location formula */ + /* u(k) = e(k)*Kp + (e(k)-e(k-1))*Kd + integral*Ki */ + output = error * pid->Kp + + (error - pid->previous_err1) * pid->Kd + + pid->integral_err * pid->Ki; + + /* If the output is out of the range, it will be limited */ + output = MIN(output, pid->max_output); + output = MAX(output, pid->min_output); + + /* Update previous error */ + pid->previous_err1 = error; + + return output; +} + +static float pid_calc_incremental(pid_ctrl_block_t *pid, float error) +{ + float output = 0; + + /* Calculate the pid control value by increment formula */ + /* du(k) = (e(k)-e(k-1))*Kp + (e(k)-2*e(k-1)+e(k-2))*Kd + e(k)*Ki */ + /* u(k) = du(k) + u(k-1) */ + output = (error - pid->previous_err1) * pid->Kp + + (error - 2 * pid->previous_err1 + pid->previous_err2) * pid->Kd + + error * pid->Ki + + pid->last_output; + + /* If the output is beyond the range, it will be limited */ + output = MIN(output, pid->max_output); + output = MAX(output, pid->min_output); + + /* Update previous error */ + pid->previous_err2 = pid->previous_err1; + pid->previous_err1 = error; + + /* Update last output */ + pid->last_output = output; + + return output; +} + +esp_err_t pid_new_control_block(const pid_ctrl_config_t *config, pid_ctrl_block_handle_t *ret_pid) +{ + esp_err_t ret = ESP_OK; + pid_ctrl_block_t *pid = NULL; + /* Check the input pointer */ + ESP_GOTO_ON_FALSE(config && ret_pid, ESP_ERR_INVALID_ARG, err, TAG, "invalid argument"); + + pid = calloc(1, sizeof(pid_ctrl_block_t)); + ESP_GOTO_ON_FALSE(pid, ESP_ERR_NO_MEM, err, TAG, "no mem for PID control block"); + ESP_GOTO_ON_ERROR(pid_update_parameters(pid, &config->init_param), err, TAG, "init PID parameters failed"); + *ret_pid = pid; + return ret; + +err: + if (pid) { + free(pid); + } + return ret; +} + +esp_err_t pid_del_control_block(pid_ctrl_block_handle_t pid) +{ + ESP_RETURN_ON_FALSE(pid, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + free(pid); + return ESP_OK; +} + +esp_err_t pid_compute(pid_ctrl_block_handle_t pid, float input_error, float *ret_result) +{ + ESP_RETURN_ON_FALSE(pid && ret_result, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + *ret_result = pid->calculate_func(pid, input_error); + return ESP_OK; +} + +esp_err_t pid_update_parameters(pid_ctrl_block_handle_t pid, const pid_ctrl_parameter_t *params) +{ + ESP_RETURN_ON_FALSE(pid && params, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + pid->Kp = params->kp; + pid->Ki = params->ki; + pid->Kd = params->kd; + pid->max_output = params->max_output; + pid->min_output = params->min_output; + pid->max_integral = params->max_integral; + pid->min_integral = params->min_integral; + /* Set the calculate function according to the PID type */ + switch (params->cal_type) { + case PID_CAL_TYPE_INCREMENTAL: + pid->calculate_func = pid_calc_incremental; + break; + case PID_CAL_TYPE_POSITIONAL: + pid->calculate_func = pid_calc_positional; + break; + default: + ESP_RETURN_ON_FALSE(false, ESP_ERR_INVALID_ARG, TAG, "invalid PID calculation type:%d", params->cal_type); + } + return ESP_OK; +} 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 deleted file mode 100644 index 7f16dda52e..0000000000 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -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 deleted file mode 100644 index 27ad11a7e5..0000000000 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/component.mk +++ /dev/null @@ -1 +0,0 @@ -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 deleted file mode 100644 index 7f8d826658..0000000000 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.c +++ /dev/null @@ -1,123 +0,0 @@ -/* 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 deleted file mode 100644 index 01d8e59acb..0000000000 --- a/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/components/pid_ctrl/pid_ctrl.h +++ /dev/null @@ -1,110 +0,0 @@ -/* 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/cmd_mcpwm_motor.c b/examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/cmd_mcpwm_motor.c index 33401ec2b9..b25727f9f2 100644 --- 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 @@ -63,8 +63,8 @@ static void print_current_status(void) 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); + (mc->pid_param.cal_type == PID_CAL_TYPE_POSITIONAL) ? "Location" : "Increment", + mc->pid_param.kp, mc->pid_param.ki, mc->pid_param.kd); printf(" Expectation - %s\n init = %.3f\tmax = %.3f\tmin = %.3f\tpace = %.3f\n\n", mc->cfg.expt_mode ? (mc->cfg.expt_mode == MOTOR_CTRL_MODE_TRIANGLE ? "Triangle" : "Rectangle") : "Fixed", mc->cfg.expt_init, mc->cfg.expt_max, mc->cfg.expt_min, mc->cfg.expt_pace); @@ -143,35 +143,37 @@ static int do_motor_ctrl_expt_cmd(int argc, char **argv) static int do_motor_ctrl_pid_cmd(int argc, char **argv) { + int ret = 0; MOTOR_CTRL_CMD_CHECK(motor_ctrl_pid_args); xSemaphoreTake(g_motor_mux, portMAX_DELAY); if (motor_ctrl_pid_args.kp->count) { - mc->pid->Kp = motor_ctrl_pid_args.kp->dval[0]; - printf("pid: kp = %.3f\n", mc->pid->Kp); + mc->pid_param.kp = motor_ctrl_pid_args.kp->dval[0]; + printf("pid: kp = %.3f\n", mc->pid_param.kp); } if (motor_ctrl_pid_args.ki->count) { - mc->pid->Ki = motor_ctrl_pid_args.ki->dval[0]; - printf("pid: ki = %.3f\n", mc->pid->Ki); + mc->pid_param.ki = motor_ctrl_pid_args.ki->dval[0]; + printf("pid: ki = %.3f\n", mc->pid_param.ki); } if (motor_ctrl_pid_args.kd->count) { - mc->pid->Kd = motor_ctrl_pid_args.kd->dval[0]; - printf("pid: kd = %.3f\n", mc->pid->Kd); + mc->pid_param.kd = motor_ctrl_pid_args.kd->dval[0]; + printf("pid: kd = %.3f\n", mc->pid_param.kd); } if (motor_ctrl_pid_args.type->count) { - if (!strcmp(*motor_ctrl_pid_args.type->sval, "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"); + if (!strcmp(motor_ctrl_pid_args.type->sval[0], "loc")) { + mc->pid_param.cal_type = PID_CAL_TYPE_POSITIONAL; + printf("pid: type = positional\n"); + } else if (!strcmp(motor_ctrl_pid_args.type->sval[0], "inc")) { + mc->pid_param.cal_type = PID_CAL_TYPE_INCREMENTAL; + printf("pid: type = incremental\n"); } else { - mc->pid->type = PID_INCREMENT; - printf("pid: type = increment\n"); + printf("Invalid pid type:%s\n", motor_ctrl_pid_args.type->sval[0]); + ret = 1; } } + pid_update_parameters(mc->pid, &mc->pid_param); xSemaphoreGive(g_motor_mux); - return 0; + return ret; } static int do_motor_ctrl_motor_cmd(int argc, char **argv) @@ -183,7 +185,7 @@ static int do_motor_ctrl_motor_cmd(int argc, char **argv) // 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 in %d seconds\n", mc->cfg.running_sec) : printf("motor: motor starts to run, input 'motor -d' to stop it\n"); } 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 index eb3f160157..e7409122d6 100644 --- 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 @@ -118,10 +118,14 @@ static int pcnt_get_pulse_callback(void *args) 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.pid_param.kp = 0.8; + motor_ctrl.pid_param.ki = 0.0; + motor_ctrl.pid_param.kd = 0.1; + motor_ctrl.pid_param.cal_type = PID_CAL_TYPE_INCREMENTAL; + motor_ctrl.pid_param.max_output = 100; + motor_ctrl.pid_param.min_output = -100; + motor_ctrl.pid_param.max_integral = 1000; + motor_ctrl.pid_param.min_integral = -1000; motor_ctrl.cfg.expt_init = 30; motor_ctrl.cfg.expt_mode = MOTOR_CTRL_MODE_TRIANGLE; motor_ctrl.cfg.expt_max = 50; @@ -195,11 +199,10 @@ static void motor_ctrl_init_all(void) /* 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); + pid_ctrl_config_t pid_config = { + .init_param = motor_ctrl.pid_param, + }; + pid_new_control_block(&pid_config, &motor_ctrl.pid); /* 5.Timer initialization */ motor_ctrl_timer_init(); } @@ -220,7 +223,7 @@ static void mcpwm_brushed_motor_ctrl_thread(void *arg) 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); + pid_compute(motor_ctrl.pid, motor_ctrl.error, &motor_ctrl.pid_output); } else { motor_ctrl.pid_output = motor_ctrl.expt; } 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 index 837c4269f8..ea2cdaae32 100644 --- 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 @@ -32,7 +32,8 @@ 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 + pid_ctrl_block_handle_t pid; // PID algoritm handler + pid_ctrl_parameter_t pid_param; // PID parameters QueueHandle_t timer_evt_que; // Timer event queue handler /* Control visualization */ @@ -49,10 +50,6 @@ typedef struct { 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