wxsmart
/
controller-hd
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

新增pid优化处理（效果较差）

This commit is contained in:
gaoyuhang 2024-07-03 14:52:33 +08:00
parent f60f444fe7
commit 7b804f2f63
6 changed files with 152 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.vscode/settings.json vendored
View File

@ -1,6 +1,18 @@
{ {
"C_Cpp.errorSquiggles": "disabled", "C_Cpp.errorSquiggles": "disabled",
"files.associations": { "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"
} }
} }

5
MDK-ARM/controller-v2.uvoptx
View File

@ -250,6 +250,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>mode_pwmp_hd-&gt;process_state,0x0A</ItemText> <ItemText>mode_pwmp_hd-&gt;process_state,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>\\controller_v2\../User/application/mode/mode_pwmp_hd.c\mode_pwmp_hd-&gt;control</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<MemoryWindow1> <MemoryWindow1>
<Mm> <Mm>

69
User/application/mode/mode_pwmp_hd.c
View File

@ -175,8 +175,8 @@ static float32 get_pwmp_hd_control_kp(void)
{ {
if (mode_pwmp_hd->pwmp_save->storage.kp < 2) 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 * 8;
return mode_pwmp_hd->pwmp_save->storage.kp * 15; // return mode_pwmp_hd->pwmp_save->storage.kp * 15;
} }
else else
{ {
@ -245,11 +245,11 @@ static float32_t convert_target_to_controlvalue()
{ {
if (mode_pwmp_hd->action == INFLATE) 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) 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 else
@ -257,12 +257,12 @@ static float32_t convert_target_to_controlvalue()
if (mode_pwmp_hd->action == INFLATE) if (mode_pwmp_hd->action == INFLATE)
{ {
tmp = ((mode_pwmp_hd->control.ctrl_target - (i * 10)) / (10)); 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) else if (mode_pwmp_hd->action == DEFLATE)
{ {
tmp = ((mode_pwmp_hd->control.ctrl_target - (i * 10)) / (10)); 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 #if 1
/* 一阶段加权系数*/ /* 一阶段加权系数*/
static float32 index_min_first = 0.3; static float32 index_min_first = 0.2;
static float32 index_max_first = 0.4; static float32 index_max_first = 0.2;
/*二阶段加权系数*/ /*二阶段加权系数*/
static float32 index_min_second = 0.2; static float32 index_min_second = 0.1;
static float32 index_max_second = 0.3; static float32 index_max_second = 0.1;
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST) if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
{ {
if (fabs(mode_pwmp_hd->control.real_error) >= 5.7) if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
@ -397,20 +397,24 @@ static execute_rsp_hd_t *execute_dac_plan1()
{ // 对PID输出结果进行归一化处理 { // 对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_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; 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) else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
{ // 对PID输出结果进行归一化处理 { // 对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_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; 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) 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_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
out += 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 else
{ {
out = mode_pwmp_hd->valvepos_stable; out = mode_pwmp_hd->valvepos_stable;
mode_pwmp_hd->pid_mode = OFFAUTO_C;
} }
} }
#else #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_current = 200;
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current; mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
pdctrl_out(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; mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
FSM_WAIT(*state); // 设置等待状态 FSM_WAIT(*state); // 设置等待状态
} }
@ -668,7 +672,8 @@ static void pwmp_adjust_hd_rough_position0(uint8_t *state, mode_pwmp_hd_adjust_s
else 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->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; 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; return;
} }
pdctrl_out(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 * 30; mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
*state = PWMP_HD_ADJUST_ROUGH_POSITION0; *state = PWMP_HD_ADJUST_ROUGH_POSITION0;
FSM_WAIT(*state); // 设置等待状态 FSM_WAIT(*state); // 设置等待状态
} }
@ -1056,7 +1061,7 @@ static void pwmp_adjust_hd_accurate_position100(uint8_t *state, mode_pwmp_hd_adj
/*若快到顶时等待10s，否则等待1s*/ /*若快到顶时等待10s，否则等待1s*/
if (adc_diff < 20) 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 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) switch (pid_autotune_hd->autotune_way)
{ {
case PID_AUTOTUNE_WAY_ZN: 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->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->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.18 * pid_autotune_hd->t_hd + 0.19 * pid_autotune_hd->l_hd; 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.25 * pid_autotune_hd->ti_auto_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->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; pid_autotune_hd->d_auto_hd = (pid_autotune_hd->td_auto_hd * pid_autotune_hd->p_auto_hd) / pid_autotune_hd->t_hd;
break; break;
case PID_AUTOTUNE_WAY_CC: case PID_AUTOTUNE_WAY_CC:
/*Kp、Ti、Td的求取*/ /*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->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)); 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的求取*/ /*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%时重置等待时间*/ /*当误差大于0.5%时重置等待时间*/
if (fabs(mode_pwmp_hd_adjust->real_error) > 1) if (fabs(mode_pwmp_hd_adjust->real_error) > 1)
{ {
// 此处延时三秒钟 // 此处延时两秒钟
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 * 30;
} }
else else
@ -1525,6 +1530,7 @@ static void pwmp_hd_adjust_step_by_10_to_100(uint8_t *state, mode_pwmp_hd_adjust
else else
{ {
*state = next_state; *state = next_state;
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
} }
mode_pwmp_hd_adjust->ctrl_target += 10; 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; execute_rsp_hd_t *execute_res;
if (!FSM_IS_WAIT(*state)) 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->ctrl_target = 90;
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state; mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
FSM_WAIT(*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; *state = next_state;
for (unsigned int i = 0; i < 9; i++) 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->pwmp_save->storage.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_de[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
} }
for (unsigned int i = 0; i < 8; 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->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->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->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->valvepos.valvepos_in_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i]; mode_pwmp_hd->pwmp_save->storage.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_de_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
} }
mode_pwmp_hd->params_save_cb(); mode_pwmp_hd->params_save_cb();
} }

41
User/application/mode/mode_pwmp_hd.h
View File

@ -30,13 +30,22 @@ typedef enum
typedef enum typedef enum
{ {
INFLATE=0, INFLATE = 0,
DEFLATE, DEFLATE,
}mode_pwmp_hd_action_e; // 运动状态（充气/排气） } mode_pwmp_hd_action_e; // 运动状态（充气/排气）
/*需要存储的变量*/ /*需要存储的变量*/
typedef struct 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：整定失败 uint8_t tuned_flag; // 整定标志 0：未整定，1：整定中，2：整定成功，3：整定失败
uint16_t startup_value; // 阀位启动值 uint16_t startup_value; // 阀位启动值
uint16_t arr_diff; // 控制区间 uint16_t arr_diff; // 控制区间
@ -82,28 +91,22 @@ typedef struct
typedef union typedef union
{ {
uint8_t data[128]; //
uint8_t data[256];
// 自定义数据结构 // 自定义数据结构
mode_pwmp_hd_storage_data_t storage; mode_pwmp_hd_storage_data_t storage;
} mode_pwmp_hd_params_u; } mode_pwmp_hd_params_u;
typedef struct typedef enum
{ {
/*充气过程：各百分之十阀位的控制信号值*/ INAUTO_C,
uint16_t valvepos_in[9]; // 充气过程中9个不同阀位的控制信号值 OFFAUTO_C,
/*排气过程：各百分之十阀位的控制信号值*/ } mode_pwmp_hd_pid_mode_e;
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;
typedef struct typedef struct
{ {
mode_pwmp_hd_valvepos_t valvepos; // 各稳定阀位控制信号值 mode_pwmp_hd_pid_mode_e pid_mode; // PID控制模式
mode_pwmp_hd_action_e action; // 运动状态（充气/排气） mode_pwmp_hd_action_e action; // 运动状态（充气/排气）
uint8_t lcd_adjust_state; // LCD自整定流程状态 uint8_t lcd_adjust_state; // LCD自整定流程状态
float32 duty_percent; // 占空比 float32 duty_percent; // 占空比
@ -252,9 +255,9 @@ typedef struct
uint32_t current_0; // 0位置电流大小 uint32_t current_0; // 0位置电流大小
uint32_t current_100; // 100位置电流大小 uint32_t current_100; // 100位置电流大小
float32 ctrl_target; // 控制目标值 float32 ctrl_target; // 控制目标值
float32 real_error; // 实际误差 float32 real_error; // 实际误差
float32 ctrl_feedback;// 反馈值 float32 ctrl_feedback; // 反馈值
/*输出值*/ /*输出值*/
uint16_t arr_default; // 默认计数器(推动计数值) uint16_t arr_default; // 默认计数器(推动计数值)

78
User/lib/control/custom/pid_hd.c
View File

@ -2,13 +2,40 @@
#include <math.h> #include <math.h>
#include "sys.h" #include "sys.h"
#include "app.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 // 积分分离 #if INCOMPLETE_DIFFEREN_HD == 1 // 积分分离
/*计算微分项，使用追随误差微分项*/ /*计算微分项，使用追随误差微分项*/
static float32 td_derivative(struct PID_HD *self, float32 current_err, float32 pre_err, float32 dt) 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; 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; // 追随误差微分器平滑输出 derivative = pri->td_alpha * derivative + (1 - pri->td_alpha) * pri->td_beta * pri->pre_derivative; // 追随误差微分器平滑输出
pri->pre_derivative = derivative; // 更新上一次误差 pri->pre_derivative = derivative; // 更新上一次误差
return 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; pid_hd_position_t *pri = &self->pri_u.position;
osel_memset((uint8_t *)pri, 0, sizeof(pid_hd_position_t)); osel_memset((uint8_t *)pri, 0, sizeof(pid_hd_position_t));
/*设置模式*/
self->pri_u.position.pid_mode = INAUTO;
/*观测传进来的Kp、Ki、Kd*/ /*观测传进来的Kp、Ki、Kd*/
pri->kp = kp; pri->kp = kp;
pri->ki = ki; pri->ki = ki;
pri->kd = kd; pri->kd = kd;
pri->ki_limit = 10; // 积分分离界限 pri->ki_limit = 20; // 积分分离界限
pri->err_dead = 0.5; // 控制死区范围 pri->err_dead = 0.5; // 控制死区范围
#if INCOMPLETE_DIFFEREN_HD == 1 #if INCOMPLETE_DIFFEREN_HD == 1
/*不完全微分系数*/ /*不完全微分系数*/
@ -45,6 +73,12 @@ static void _set_out_prm_position(struct PID_HD *self, float32 maximum, float32
/*杭电：位置式PID控制算法*/ /*杭电：位置式PID控制算法*/
static float32 _pid_position(struct PID_HD *self, float32 err) 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(); // sys_millis_reset();
self->pri_u.position.control_time = sys_millis(); self->pri_u.position.control_time = sys_millis();
@ -55,9 +89,8 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
{ {
err = 0; err = 0;
} }
float32 x[3];
self->pri_u.position.err = err; self->pri_u.position.err = err;
self->pri_u.position.feed_back = mode_pwmp_hd->control.ctrl_feedback;
/*抗积分饱和*/ /*抗积分饱和*/
#if INTEGRAL_SEPARATION == 1 // 积分分离 #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) // 若偏差为负值，执行负偏差的累加 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) // 若偏差为正值，执行正偏差的累加 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 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 /*无积分分离操作*/ #else /*无积分分离操作*/
@ -111,32 +144,42 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
} }
#endif #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) if (fabs(err) < self->pri_u.position.err_dead)
{ {
/*输出上一次的值*/ /*输出上一次的值*/
// self->pri_u.position.out = self->pri_u.position.pre_out; // self->pri_u.position.out = self->pri_u.position.pre_out;
x[0] = self->pri_u.position.err; self->pri_u.position.x[0] = self->pri_u.position.err;
x[1] = self->pri_u.position.ki_error; self->pri_u.position.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.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 else
{ {
x[0] = self->pri_u.position.err; self->pri_u.position.x[0] = self->pri_u.position.err;
x[1] = self->pri_u.position.ki_error; self->pri_u.position.x[1] = self->pri_u.position.ki_error;
#if INCOMPLETE_DIFFEREN_HD == 1 #if INCOMPLETE_DIFFEREN_HD == 1
/*不完全微分项，为了解决普通PID为微分环节容易振荡的问题*/ /*不完全微分项，为了解决普通PID为微分环节容易振荡的问题*/
self->pri_u.position.tmp_time = sys_millis(); 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 -= self->pri_u.position.tmp_time;
self->pri_u.position.control_time /= 1000.0; // 将单位转换为秒 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 #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 #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_error = self->pri_u.position.err; /*上一次误差值*/
// 更新输出历史 // 更新输出历史
self->pri_u.position.pre_out = self->pri_u.position.out; /*上一次输出值*/ 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; 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_ctrl_prm_position = _set_ctrl_prm_position;
self->set_out_prm_position = _set_out_prm_position; self->set_out_prm_position = _set_out_prm_position;
self->pid_position = _pid_position; self->pid_position = _pid_position;
self->set_mode = _set_mode;
} }

10
User/lib/control/custom/pid_hd.h
View File

@ -5,8 +5,16 @@
#define INTEGRAL_SEPARATION 1 // 积分分离 #define INTEGRAL_SEPARATION 1 // 积分分离
#define INCOMPLETE_DIFFEREN_HD 1 // 不完全微分 #define INCOMPLETE_DIFFEREN_HD 1 // 不完全微分
typedef enum
{
INAUTO,
OFFAUTO,
} pid_mode_e;
typedef struct typedef struct
{ {
pid_mode_e pid_mode;
float32 x[3];
float32 ref; float32 ref;
float32 feed_back; float32 feed_back;
float32 pre_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_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_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); float32 (*pid_position)(struct PID_HD *self, float32 err);