diff --git a/.vscode/settings.json b/.vscode/settings.json
index 12604bf..b98514f 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -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"
}
}
diff --git a/MDK-ARM/controller-v2.uvoptx b/MDK-ARM/controller-v2.uvoptx
index fb8fbe7..4a97cf5 100644
--- a/MDK-ARM/controller-v2.uvoptx
+++ b/MDK-ARM/controller-v2.uvoptx
@@ -250,6 +250,11 @@
1
mode_pwmp_hd->process_state,0x0A
+
+ 18
+ 1
+ \\controller_v2\../User/application/mode/mode_pwmp_hd.c\mode_pwmp_hd->control
+
diff --git a/User/application/mode/mode_pwmp_hd.c b/User/application/mode/mode_pwmp_hd.c
index f35b6cd..fe1ca80 100644
--- a/User/application/mode/mode_pwmp_hd.c
+++ b/User/application/mode/mode_pwmp_hd.c
@@ -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();
}
diff --git a/User/application/mode/mode_pwmp_hd.h b/User/application/mode/mode_pwmp_hd.h
index f829f8e..528eaeb 100644
--- a/User/application/mode/mode_pwmp_hd.h
+++ b/User/application/mode/mode_pwmp_hd.h
@@ -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,28 +91,22 @@ 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_action_e action; // 运动状态（充气/排气）
+ mode_pwmp_hd_pid_mode_e pid_mode; // PID控制模式
+ mode_pwmp_hd_action_e action; // 运动状态（充气/排气）
uint8_t lcd_adjust_state; // LCD自整定流程状态
float32 duty_percent; // 占空比
@@ -252,9 +255,9 @@ typedef struct
uint32_t current_0; // 0位置电流大小
uint32_t current_100; // 100位置电流大小
- float32 ctrl_target; // 控制目标值
- float32 real_error; // 实际误差
- float32 ctrl_feedback;// 反馈值
+ float32 ctrl_target; // 控制目标值
+ float32 real_error; // 实际误差
+ float32 ctrl_feedback; // 反馈值
/*输出值*/
uint16_t arr_default; // 默认计数器(推动计数值)
diff --git a/User/lib/control/custom/pid_hd.c b/User/lib/control/custom/pid_hd.c
index fb4a709..f3d0336 100644
--- a/User/lib/control/custom/pid_hd.c
+++ b/User/lib/control/custom/pid_hd.c
@@ -2,13 +2,40 @@
#include
#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;
}
diff --git a/User/lib/control/custom/pid_hd.h b/User/lib/control/custom/pid_hd.h
index 2fa96f0..fa52895 100644
--- a/User/lib/control/custom/pid_hd.h
+++ b/User/lib/control/custom/pid_hd.h
@@ -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);