pid_ctrl: abstract into example common component

This commit is contained in:
morris
2021-08-10 10:41:13 +08:00
parent b6c5a6ee8b
commit f0fab687ad
11 changed files with 281 additions and 272 deletions

View File

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

View File

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

View File

@@ -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 <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;
}
}

View File

@@ -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

View File

@@ -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");
}

View File

@@ -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;
}

View File

@@ -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