新增pid优化处理（效果较差）
This commit is contained in:
parent
f60f444fe7
commit
7b804f2f63
|
|
@ -1,6 +1,18 @@
|
|||
{
|
||||
"C_Cpp.errorSquiggles": "disabled",
|
||||
"files.associations": {
|
||||
"cmath": "c"
|
||||
"cmath": "c",
|
||||
"pid_hd.h": "c",
|
||||
"array": "c",
|
||||
"*.tcc": "c",
|
||||
"string": "c",
|
||||
"vector": "c",
|
||||
"exception": "c",
|
||||
"functional": "c",
|
||||
"istream": "c",
|
||||
"limits": "c",
|
||||
"ostream": "c",
|
||||
"sstream": "c",
|
||||
"streambuf": "c"
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -250,6 +250,11 @@
|
|||
<WinNumber>1</WinNumber>
|
||||
<ItemText>mode_pwmp_hd->process_state,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>18</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>\\controller_v2\../User/application/mode/mode_pwmp_hd.c\mode_pwmp_hd->control</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<MemoryWindow1>
|
||||
<Mm>
|
||||
|
|
|
|||
|
|
@ -175,8 +175,8 @@ static float32 get_pwmp_hd_control_kp(void)
|
|||
{
|
||||
if (mode_pwmp_hd->pwmp_save->storage.kp < 2)
|
||||
{
|
||||
// return mode_pwmp_hd->pwmp_save->storage.kp * 8;
|
||||
return mode_pwmp_hd->pwmp_save->storage.kp * 15;
|
||||
return mode_pwmp_hd->pwmp_save->storage.kp * 8;
|
||||
// return mode_pwmp_hd->pwmp_save->storage.kp * 15;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -245,11 +245,11 @@ static float32_t convert_target_to_controlvalue()
|
|||
{
|
||||
if (mode_pwmp_hd->action == INFLATE)
|
||||
{
|
||||
control_value = mode_pwmp_hd->valvepos.valvepos_in[i - 1];
|
||||
control_value = mode_pwmp_hd->pwmp_save->storage.valvepos_in[i - 1];
|
||||
}
|
||||
else if (mode_pwmp_hd->action == DEFLATE)
|
||||
{
|
||||
control_value = mode_pwmp_hd->valvepos.valvepos_de[i - 1];
|
||||
control_value = mode_pwmp_hd->pwmp_save->storage.valvepos_de[i - 1];
|
||||
}
|
||||
}
|
||||
else
|
||||
|
|
@ -257,12 +257,12 @@ static float32_t convert_target_to_controlvalue()
|
|||
if (mode_pwmp_hd->action == INFLATE)
|
||||
{
|
||||
tmp = ((mode_pwmp_hd->control.ctrl_target - (i * 10)) / (10));
|
||||
control_value = tmp * mode_pwmp_hd->valvepos.valvepos_in_k[i - 1] + mode_pwmp_hd->valvepos.valvepos_in_b[i - 1];
|
||||
control_value = tmp * mode_pwmp_hd->pwmp_save->storage.valvepos_in_k[i - 1] + mode_pwmp_hd->pwmp_save->storage.valvepos_in_b[i - 1];
|
||||
}
|
||||
else if (mode_pwmp_hd->action == DEFLATE)
|
||||
{
|
||||
tmp = ((mode_pwmp_hd->control.ctrl_target - (i * 10)) / (10));
|
||||
control_value = tmp * mode_pwmp_hd->valvepos.valvepos_de_k[i - 1] + mode_pwmp_hd->valvepos.valvepos_de_b[i - 1];
|
||||
control_value = tmp * mode_pwmp_hd->pwmp_save->storage.valvepos_de_k[i - 1] + mode_pwmp_hd->pwmp_save->storage.valvepos_de_b[i - 1];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -368,11 +368,11 @@ static execute_rsp_hd_t *execute_dac_plan1()
|
|||
{
|
||||
#if 1
|
||||
/* 一阶段加权系数*/
|
||||
static float32 index_min_first = 0.3;
|
||||
static float32 index_max_first = 0.4;
|
||||
static float32 index_min_first = 0.2;
|
||||
static float32 index_max_first = 0.2;
|
||||
/*二阶段加权系数*/
|
||||
static float32 index_min_second = 0.2;
|
||||
static float32 index_max_second = 0.3;
|
||||
static float32 index_min_second = 0.1;
|
||||
static float32 index_max_second = 0.1;
|
||||
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
|
||||
{
|
||||
if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
|
||||
|
|
@ -397,20 +397,24 @@ static execute_rsp_hd_t *execute_dac_plan1()
|
|||
{ // 对PID输出结果进行归一化处理
|
||||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
|
||||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
|
||||
mode_pwmp_hd->pid_mode = INAUTO_C;
|
||||
}
|
||||
else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
|
||||
{ // 对PID输出结果进行归一化处理
|
||||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
|
||||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
|
||||
mode_pwmp_hd->pid_mode = INAUTO_C;
|
||||
}
|
||||
else if (fabs(mode_pwmp_hd->control.real_error) < 1 && fabs(mode_pwmp_hd->control.real_error) > 0.7)
|
||||
{
|
||||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
|
||||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
|
||||
mode_pwmp_hd->pid_mode = INAUTO_C;
|
||||
}
|
||||
else
|
||||
{
|
||||
out = mode_pwmp_hd->valvepos_stable;
|
||||
mode_pwmp_hd->pid_mode = OFFAUTO_C;
|
||||
}
|
||||
}
|
||||
#else
|
||||
|
|
@ -648,7 +652,7 @@ static void pwmp_adjust_hd_rough_position0(uint8_t *state, mode_pwmp_hd_adjust_s
|
|||
mode_pwmp_hd_adjust->arr_current = 200;
|
||||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
|
||||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||||
FSM_WAIT(*state); // 设置等待状态
|
||||
}
|
||||
|
|
@ -668,7 +672,8 @@ static void pwmp_adjust_hd_rough_position0(uint8_t *state, mode_pwmp_hd_adjust_s
|
|||
else
|
||||
{
|
||||
/*得到一个粗略的，小于启动量的值*/
|
||||
mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last > 1000 ? mode_pwmp_hd_adjust->arr_last * 0.97 : mode_pwmp_hd_adjust->arr_last;
|
||||
// mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last > 1000 ? mode_pwmp_hd_adjust->arr_last * 0.97 : mode_pwmp_hd_adjust->arr_last;
|
||||
mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last;
|
||||
/*判断位置反馈磁条是正装还是反装*/
|
||||
mode_pwmp_hd_adjust->adc_0_100_flag = mode_pwmp_hd_adjust->adc_record_0 < current_adc ? TRUE : FALSE;
|
||||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1;
|
||||
|
|
@ -703,7 +708,7 @@ static void pwmp_adjust_hd_rough_position0(uint8_t *state, mode_pwmp_hd_adjust_s
|
|||
return;
|
||||
}
|
||||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
|
||||
*state = PWMP_HD_ADJUST_ROUGH_POSITION0;
|
||||
FSM_WAIT(*state); // 设置等待状态
|
||||
}
|
||||
|
|
@ -1056,7 +1061,7 @@ static void pwmp_adjust_hd_accurate_position100(uint8_t *state, mode_pwmp_hd_adj
|
|||
/*若快到顶时等待10s，否则等待1s*/
|
||||
if (adc_diff < 20)
|
||||
{
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 100;
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -1419,24 +1424,24 @@ static void pwmp_adjust_hd_pid_calculate(uint8_t *state, mode_pwmp_hd_adjust_sta
|
|||
switch (pid_autotune_hd->autotune_way)
|
||||
{
|
||||
case PID_AUTOTUNE_WAY_ZN:
|
||||
if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) <= 0.2)
|
||||
if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) <= 0.2f)
|
||||
{
|
||||
pid_autotune_hd->p_auto_hd = (pid_autotune_hd->t_hd / (0.85 * pid_autotune_hd->l_hd * pid_autotune_hd->k_hd));
|
||||
pid_autotune_hd->p_auto_hd = (pid_autotune_hd->t_hd / (0.85f * pid_autotune_hd->l_hd * pid_autotune_hd->k_hd));
|
||||
pid_autotune_hd->ti_auto_hd = 2 * pid_autotune_hd->l_hd;
|
||||
pid_autotune_hd->td_auto_hd = 0.5 * pid_autotune_hd->l_hd;
|
||||
pid_autotune_hd->td_auto_hd = 0.5f * pid_autotune_hd->l_hd;
|
||||
}
|
||||
else if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) > 0.2)
|
||||
else if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) > 0.2f)
|
||||
{
|
||||
pid_autotune_hd->p_auto_hd = ((1 / pid_autotune_hd->k_hd) * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) + 0.6) / (2.6 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) - 0.08);
|
||||
pid_autotune_hd->ti_auto_hd = 0.18 * pid_autotune_hd->t_hd + 0.19 * pid_autotune_hd->l_hd;
|
||||
pid_autotune_hd->td_auto_hd = 0.25 * pid_autotune_hd->ti_auto_hd;
|
||||
pid_autotune_hd->p_auto_hd = ((1 / pid_autotune_hd->k_hd) * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) + 0.6f) / (2.6f * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) - 0.08f);
|
||||
pid_autotune_hd->ti_auto_hd = 0.18f * pid_autotune_hd->t_hd + 0.19f * pid_autotune_hd->l_hd;
|
||||
pid_autotune_hd->td_auto_hd = 0.25f * pid_autotune_hd->ti_auto_hd;
|
||||
}
|
||||
pid_autotune_hd->i_auto_hd = pid_autotune_hd->p_auto_hd / pid_autotune_hd->ti_auto_hd;
|
||||
pid_autotune_hd->d_auto_hd = (pid_autotune_hd->td_auto_hd * pid_autotune_hd->p_auto_hd) / pid_autotune_hd->t_hd;
|
||||
break;
|
||||
case PID_AUTOTUNE_WAY_CC:
|
||||
/*Kp、Ti、Td的求取*/
|
||||
pid_autotune_hd->p_auto_hd = ((pid_autotune_hd->t_hd / (pid_autotune_hd->k_hd * pid_autotune_hd->l_hd)) * (4 / 3 + (pid_autotune_hd->l_hd / (pid_autotune_hd->t_hd * 4))));
|
||||
pid_autotune_hd->p_auto_hd = ((pid_autotune_hd->t_hd / (pid_autotune_hd->k_hd * pid_autotune_hd->l_hd)) * ((float32)(4 / 3) + (pid_autotune_hd->l_hd / (pid_autotune_hd->t_hd * 4))));
|
||||
pid_autotune_hd->ti_auto_hd = pid_autotune_hd->l_hd * ((32 + 6 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd)) / (13 + 8 * (pid_autotune_hd->l_hd / pid_autotune_hd->l_hd)));
|
||||
pid_autotune_hd->td_auto_hd = (4 * pid_autotune_hd->l_hd) / (11 + 2 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd));
|
||||
/*Ki、Kd的求取*/
|
||||
|
|
@ -1509,7 +1514,7 @@ static void pwmp_hd_adjust_step_by_10_to_100(uint8_t *state, mode_pwmp_hd_adjust
|
|||
/*当误差大于0.5%时重置等待时间*/
|
||||
if (fabs(mode_pwmp_hd_adjust->real_error) > 1)
|
||||
{
|
||||
// 此处延时三秒钟
|
||||
// 此处延时两秒钟
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||||
}
|
||||
else
|
||||
|
|
@ -1525,6 +1530,7 @@ static void pwmp_hd_adjust_step_by_10_to_100(uint8_t *state, mode_pwmp_hd_adjust
|
|||
else
|
||||
{
|
||||
*state = next_state;
|
||||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||||
}
|
||||
mode_pwmp_hd_adjust->ctrl_target += 10;
|
||||
}
|
||||
|
|
@ -1538,6 +1544,11 @@ static void pwmp_hd_adjust_step_by_10_to_0(uint8_t *state, mode_pwmp_hd_adjust_s
|
|||
execute_rsp_hd_t *execute_res;
|
||||
if (!FSM_IS_WAIT(*state))
|
||||
{
|
||||
if (mode_pwmp_hd_adjust->wait_count-- > 0)
|
||||
{
|
||||
// 等待放气完毕
|
||||
return;
|
||||
}
|
||||
mode_pwmp_hd_adjust->ctrl_target = 90;
|
||||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||||
FSM_WAIT(*state); // 设置等待状态
|
||||
|
|
@ -1598,15 +1609,15 @@ static void pwmp_hd_adjust_valvepos_calculate(uint8_t *state, mode_pwmp_hd_adjus
|
|||
*state = next_state;
|
||||
for (unsigned int i = 0; i < 9; i++)
|
||||
{
|
||||
mode_pwmp_hd->valvepos.valvepos_in[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||||
mode_pwmp_hd->valvepos.valvepos_de[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||||
mode_pwmp_hd->pwmp_save->storage.valvepos_in[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||||
mode_pwmp_hd->pwmp_save->storage.valvepos_de[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||||
}
|
||||
for (unsigned int i = 0; i < 8; i++)
|
||||
{
|
||||
mode_pwmp_hd->valvepos.valvepos_in_k[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i + 1] - mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||||
mode_pwmp_hd->valvepos.valvepos_de_k[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i + 1] - mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||||
mode_pwmp_hd->valvepos.valvepos_in_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||||
mode_pwmp_hd->valvepos.valvepos_de_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||||
mode_pwmp_hd->pwmp_save->storage.valvepos_in_k[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i + 1] - mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||||
mode_pwmp_hd->pwmp_save->storage.valvepos_de_k[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i + 1] - mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||||
mode_pwmp_hd->pwmp_save->storage.valvepos_in_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||||
mode_pwmp_hd->pwmp_save->storage.valvepos_de_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||||
}
|
||||
mode_pwmp_hd->params_save_cb();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,13 +30,22 @@ typedef enum
|
|||
|
||||
typedef enum
|
||||
{
|
||||
INFLATE=0,
|
||||
INFLATE = 0,
|
||||
DEFLATE,
|
||||
}mode_pwmp_hd_action_e; // 运动状态（充气/排气）
|
||||
} mode_pwmp_hd_action_e; // 运动状态（充气/排气）
|
||||
|
||||
/*需要存储的变量*/
|
||||
typedef struct
|
||||
{
|
||||
/*充气过程：各百分之十阀位的控制信号值*/
|
||||
uint16_t valvepos_in[9]; // 充气过程中9个不同阀位的控制信号值
|
||||
/*排气过程：各百分之十阀位的控制信号值*/
|
||||
uint16_t valvepos_de[9]; // 排气过程中9个不同阀位的控制信号值
|
||||
|
||||
float32_t valvepos_in_k[8];
|
||||
float32_t valvepos_de_k[8];
|
||||
float32_t valvepos_in_b[8];
|
||||
float32_t valvepos_de_b[8];
|
||||
uint8_t tuned_flag; // 整定标志 0：未整定，1：整定中，2：整定成功，3：整定失败
|
||||
uint16_t startup_value; // 阀位启动值
|
||||
uint16_t arr_diff; // 控制区间
|
||||
|
|
@ -82,27 +91,21 @@ typedef struct
|
|||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data[128];
|
||||
//
|
||||
uint8_t data[256];
|
||||
// 自定义数据结构
|
||||
mode_pwmp_hd_storage_data_t storage;
|
||||
} mode_pwmp_hd_params_u;
|
||||
|
||||
typedef struct
|
||||
typedef enum
|
||||
{
|
||||
/*充气过程：各百分之十阀位的控制信号值*/
|
||||
uint16_t valvepos_in[9]; // 充气过程中9个不同阀位的控制信号值
|
||||
/*排气过程：各百分之十阀位的控制信号值*/
|
||||
uint16_t valvepos_de[9]; // 排气过程中9个不同阀位的控制信号值
|
||||
|
||||
float32_t valvepos_in_k[8];
|
||||
float32_t valvepos_de_k[8];
|
||||
float32_t valvepos_in_b[8];
|
||||
float32_t valvepos_de_b[8];
|
||||
} mode_pwmp_hd_valvepos_t;
|
||||
INAUTO_C,
|
||||
OFFAUTO_C,
|
||||
} mode_pwmp_hd_pid_mode_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
mode_pwmp_hd_valvepos_t valvepos; // 各稳定阀位控制信号值
|
||||
mode_pwmp_hd_pid_mode_e pid_mode; // PID控制模式
|
||||
mode_pwmp_hd_action_e action; // 运动状态（充气/排气）
|
||||
|
||||
uint8_t lcd_adjust_state; // LCD自整定流程状态
|
||||
|
|
@ -254,7 +257,7 @@ typedef struct
|
|||
|
||||
float32 ctrl_target; // 控制目标值
|
||||
float32 real_error; // 实际误差
|
||||
float32 ctrl_feedback;// 反馈值
|
||||
float32 ctrl_feedback; // 反馈值
|
||||
|
||||
/*输出值*/
|
||||
uint16_t arr_default; // 默认计数器(推动计数值)
|
||||
|
|
|
|||
|
|
@ -2,13 +2,40 @@
|
|||
#include <math.h>
|
||||
#include "sys.h"
|
||||
#include "app.h"
|
||||
#include "mode_pwmp_hd.h"
|
||||
|
||||
extern execute_rsp_hd_t rsp; // 执行结果
|
||||
|
||||
/*PID控制器重启后的初始化*/
|
||||
|
||||
static void initialize(struct PID_HD *self)
|
||||
{
|
||||
self->pri_u.position.pre_feed_back = mode_pwmp_hd->control.ctrl_feedback;
|
||||
self->pri_u.position.ki_error = (float32)((rsp.dac - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin) * (1 / (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin)) * 100);
|
||||
if (self->pri_u.position.ki_error > 100)
|
||||
{
|
||||
self->pri_u.position.ki_error = 100;
|
||||
}
|
||||
else if (self->pri_u.position.ki_error < 0)
|
||||
{
|
||||
self->pri_u.position.ki_error = 0;
|
||||
}
|
||||
}
|
||||
/*模式设置*/
|
||||
static void _set_mode(struct PID_HD *self, pid_mode_e mode)
|
||||
{
|
||||
self->pri_u.position.pid_mode = mode;
|
||||
if (mode != INAUTO)
|
||||
{
|
||||
initialize(&_pid.pid_u.hd);
|
||||
}
|
||||
}
|
||||
#if INCOMPLETE_DIFFEREN_HD == 1 // 积分分离
|
||||
/*计算微分项，使用追随误差微分项*/
|
||||
static float32 td_derivative(struct PID_HD *self, float32 current_err, float32 pre_err, float32 dt)
|
||||
{
|
||||
pid_hd_position_t *pri = &self->pri_u.position;
|
||||
float32 derivative = (current_err - pre_err) / dt; // 计算积分项
|
||||
float32 derivative = (current_err - pre_err) / dt; // 计算微分项
|
||||
derivative = pri->td_alpha * derivative + (1 - pri->td_alpha) * pri->td_beta * pri->pre_derivative; // 追随误差微分器平滑输出
|
||||
pri->pre_derivative = derivative; // 更新上一次误差
|
||||
return derivative;
|
||||
|
|
@ -21,12 +48,13 @@ static void _set_ctrl_prm_position(struct PID_HD *self, float32 kp, float32 ki,
|
|||
pid_hd_position_t *pri = &self->pri_u.position;
|
||||
osel_memset((uint8_t *)pri, 0, sizeof(pid_hd_position_t));
|
||||
|
||||
/*设置模式*/
|
||||
self->pri_u.position.pid_mode = INAUTO;
|
||||
/*观测传进来的Kp、Ki、Kd*/
|
||||
|
||||
pri->kp = kp;
|
||||
pri->ki = ki;
|
||||
pri->kd = kd;
|
||||
pri->ki_limit = 10; // 积分分离界限
|
||||
pri->ki_limit = 20; // 积分分离界限
|
||||
pri->err_dead = 0.5; // 控制死区范围
|
||||
#if INCOMPLETE_DIFFEREN_HD == 1
|
||||
/*不完全微分系数*/
|
||||
|
|
@ -45,6 +73,12 @@ static void _set_out_prm_position(struct PID_HD *self, float32 maximum, float32
|
|||
/*杭电：位置式PID控制算法*/
|
||||
static float32 _pid_position(struct PID_HD *self, float32 err)
|
||||
{
|
||||
/*模式设置*/
|
||||
self->pri_u.position.pid_mode = (pid_mode_e)mode_pwmp_hd->pid_mode;
|
||||
_set_mode(self, self->pri_u.position.pid_mode);
|
||||
/*判断是否在自动模式*/
|
||||
if (self->pri_u.position.pid_mode != INAUTO)
|
||||
return self->pri_u.position.pre_out;
|
||||
/*计算控制的运行时间*/
|
||||
// sys_millis_reset();
|
||||
self->pri_u.position.control_time = sys_millis();
|
||||
|
|
@ -55,9 +89,8 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
|
|||
{
|
||||
err = 0;
|
||||
}
|
||||
|
||||
float32 x[3];
|
||||
self->pri_u.position.err = err;
|
||||
self->pri_u.position.feed_back = mode_pwmp_hd->control.ctrl_feedback;
|
||||
|
||||
/*抗积分饱和*/
|
||||
#if INTEGRAL_SEPARATION == 1 // 积分分离
|
||||
|
|
@ -71,7 +104,7 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
|
|||
{
|
||||
if (self->pri_u.position.err < 0) // 若偏差为负值，执行负偏差的累加
|
||||
{
|
||||
self->pri_u.position.ki_error += self->pri_u.position.err;
|
||||
self->pri_u.position.ki_error += self->pri_u.position.ki * self->pri_u.position.err;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -85,7 +118,7 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
|
|||
{
|
||||
if (self->pri_u.position.err > 0) // 若偏差为正值，执行正偏差的累加
|
||||
{
|
||||
self->pri_u.position.ki_error += self->pri_u.position.err;
|
||||
self->pri_u.position.ki_error += self->pri_u.position.ki * self->pri_u.position.err;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -97,7 +130,7 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
|
|||
}
|
||||
else
|
||||
{
|
||||
self->pri_u.position.ki_error += self->pri_u.position.err;
|
||||
self->pri_u.position.ki_error += self->pri_u.position.ki * self->pri_u.position.err;
|
||||
}
|
||||
}
|
||||
#else /*无积分分离操作*/
|
||||
|
|
@ -111,32 +144,42 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
|
|||
}
|
||||
#endif
|
||||
|
||||
if (self->pri_u.position.ki_error > 100)
|
||||
{
|
||||
self->pri_u.position.ki_error = 100;
|
||||
}
|
||||
else if (self->pri_u.position.ki_error < 0)
|
||||
{
|
||||
self->pri_u.position.ki_error = 0;
|
||||
}
|
||||
|
||||
/*输出*/
|
||||
if (fabs(err) < self->pri_u.position.err_dead)
|
||||
{
|
||||
/*输出上一次的值*/
|
||||
// self->pri_u.position.out = self->pri_u.position.pre_out;
|
||||
x[0] = self->pri_u.position.err;
|
||||
x[1] = self->pri_u.position.ki_error;
|
||||
self->pri_u.position.out = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
|
||||
self->pri_u.position.x[0] = self->pri_u.position.err;
|
||||
self->pri_u.position.x[1] = self->pri_u.position.ki_error;
|
||||
self->pri_u.position.out = self->pri_u.position.kp * self->pri_u.position.x[0] + self->pri_u.position.x[1] + self->pri_u.position.kd * self->pri_u.position.x[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
x[0] = self->pri_u.position.err;
|
||||
x[1] = self->pri_u.position.ki_error;
|
||||
self->pri_u.position.x[0] = self->pri_u.position.err;
|
||||
self->pri_u.position.x[1] = self->pri_u.position.ki_error;
|
||||
|
||||
#if INCOMPLETE_DIFFEREN_HD == 1
|
||||
/*不完全微分项，为了解决普通PID为微分环节容易振荡的问题*/
|
||||
self->pri_u.position.tmp_time = sys_millis();
|
||||
self->pri_u.position.control_time -= self->pri_u.position.tmp_time;
|
||||
self->pri_u.position.control_time /= 1000.0; // 将单位转换为秒
|
||||
x[2] = td_derivative(&_pid.pid_u.hd, err, self->pri_u.position.pre_error, self->pri_u.position.control_time);
|
||||
// x[2] = td_derivative(&_pid.pid_u.hd, err, self->pri_u.position.pre_error, self->pri_u.position.control_time);
|
||||
self->pri_u.position.x[2] = -(td_derivative(&_pid.pid_u.hd, self->pri_u.position.feed_back, self->pri_u.position.pre_feed_back, self->pri_u.position.control_time));
|
||||
#else
|
||||
// 普通的微分环节
|
||||
x[2] = self->pri_u.position.err - self->pri_u.position.pre_error;
|
||||
self->pri_u.position.x[2] = self->pri_u.position.err - self->pri_u.position.pre_error;
|
||||
#endif
|
||||
|
||||
self->pri_u.position.out = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
|
||||
self->pri_u.position.out = self->pri_u.position.kp * self->pri_u.position.x[0] + self->pri_u.position.x[1] + self->pri_u.position.kd * self->pri_u.position.x[2];
|
||||
}
|
||||
|
||||
/*输出限幅*/
|
||||
|
|
@ -153,6 +196,8 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
|
|||
self->pri_u.position.pre_error = self->pri_u.position.err; /*上一次误差值*/
|
||||
// 更新输出历史
|
||||
self->pri_u.position.pre_out = self->pri_u.position.out; /*上一次输出值*/
|
||||
// 更新反馈历史
|
||||
self->pri_u.position.pre_feed_back = self->pri_u.position.feed_back; /*上一次反馈值*/
|
||||
|
||||
return self->pri_u.position.out;
|
||||
}
|
||||
|
|
@ -163,4 +208,5 @@ void pid_hd_constructor(struct PID_HD *self)
|
|||
self->set_ctrl_prm_position = _set_ctrl_prm_position;
|
||||
self->set_out_prm_position = _set_out_prm_position;
|
||||
self->pid_position = _pid_position;
|
||||
self->set_mode = _set_mode;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,8 +5,16 @@
|
|||
#define INTEGRAL_SEPARATION 1 // 积分分离
|
||||
#define INCOMPLETE_DIFFEREN_HD 1 // 不完全微分
|
||||
|
||||
typedef enum
|
||||
{
|
||||
INAUTO,
|
||||
OFFAUTO,
|
||||
} pid_mode_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
pid_mode_e pid_mode;
|
||||
float32 x[3];
|
||||
float32 ref;
|
||||
float32 feed_back;
|
||||
float32 pre_feed_back;
|
||||
|
|
@ -45,6 +53,8 @@ typedef struct PID_HD
|
|||
void (*set_ctrl_prm_position)(struct PID_HD *self, float32 kp, float32 ki, float32 kd);
|
||||
/* 设置输出范围 */
|
||||
void (*set_out_prm_position)(struct PID_HD *self, float32 maximum, float32 minimum);
|
||||
/*设置模式*/
|
||||
void (*set_mode)(struct PID_HD *self, pid_mode_e mode);
|
||||
|
||||
/* 控制接口 */
|
||||
float32 (*pid_position)(struct PID_HD *self, float32 err);
|
||||
|
|
|
|||
Reference in New Issue