TuxMonkey/tronone-h7-scaffold
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/tronone-h7-scaffold synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

add ffc controller

Browse Source
This commit is contained in:
tuxmonkey
2026-02-12 00:59:13 +08:00
parent a025289597
commit d27b8e8473
2 changed files with 69 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
User_Code/module/algorithm/controller/pid/pid.c
View File

@@ -4,7 +4,7 @@
* @author modified by TuxMonkey
* @brief PID控制器定义
* @version beta
* @date 2022-11-01
* @date 2026-02-12
*
* @copyright Copyright (c) 2025 DLMU.CONE EC all rights reserved
*/
@@ -80,6 +80,45 @@ static void f_Output_Filter(PIDInstance *pid)
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 前馈控制计算
static void f_FeedForward_Control(PIDInstance *pid)
{
// 更新设定值历史
pid->FFC_Set_History[2] = pid->FFC_Set_History[1]; // LLAST = LAST
pid->FFC_Set_History[1] = pid->FFC_Set_History[0]; // LAST = NOW
pid->FFC_Set_History[0] = pid->Ref; // NOW = 当前设定值
// 低通滤波处理
float denominator = pid->FFC_LPF_RC + pid->dt;
if (denominator > 0.000001f) {
pid->FFC_Set_History[0] = pid->FFC_Set_History[0] * pid->dt / denominator +
pid->FFC_Set_History[0] * pid->FFC_LPF_RC / denominator;
}
// 前馈计算: P + D + A
float p_term = pid->FFC_Kp * pid->FFC_Set_History[0]; // 比例项
float d_term = 0.0f;
float a_term = 0.0f;
if (pid->dt > 0.000001f) { // 防止除以零
// 微分项: 速度前馈
d_term = pid->FFC_Kd * (pid->FFC_Set_History[0] - pid->FFC_Set_History[1]) / pid->dt;
// 加速度项: 加速度前馈
a_term = pid->FFC_Ka * (pid->FFC_Set_History[0] - 2 * pid->FFC_Set_History[1] + pid->FFC_Set_History[2]) / (pid->dt * pid->dt);
}
pid->FFC_Output = p_term + d_term + a_term;
// 前馈输出限幅
if (pid->FFC_Output > pid->MaxOut) {
pid->FFC_Output = pid->MaxOut;
}
if (pid->FFC_Output < -(pid->MaxOut)) {
pid->FFC_Output = -(pid->MaxOut);
}
}
// 输出限幅
static void f_Output_Limit(PIDInstance *pid)
{
@@ -184,12 +223,23 @@ float PIDCalculate(PIDInstance *pid, float measure, float ref)
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 前馈控制
if (pid->Improve & PID_FeedForward)
f_FeedForward_Control(pid);
// 输出滤波
if (pid->Improve & PID_OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
// 前馈输出叠加到总输出
if (pid->Improve & PID_FeedForward) {
pid->Output += pid->FFC_Output;
// 再次限幅确保总输出不超过限制
f_Output_Limit(pid);
}
}
else // 进入死区, 则清空积分和输出
{
@@ -205,4 +255,4 @@ float PIDCalculate(PIDInstance *pid, float measure, float ref)
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}
}

18
User_Code/module/algorithm/controller/pid/pid.h
View File

@@ -37,6 +37,7 @@ typedef enum
PID_ChangingIntegrationRate = 0b00100000, // 变积分
PID_DerivativeFilter = 0b01000000, // 微分滤波
PID_ErrorHandle = 0b10000000, // 误差处理
PID_FeedForward = 0b100000000, // 前馈控制
} PID_Improvement_e;
/* PID 报错类型枚举*/
@@ -71,6 +72,15 @@ typedef struct
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
float Derivative_LPF_RC; // 微分滤波器系数
//-----------------------------------
// Feed Forward Control (FFC) parameters
float FFC_Kp; // 前馈比例系数
float FFC_Kd; // 前馈微分系数
float FFC_Ka; // 前馈加速度系数
float FFC_LPF_RC; // 前馈低通滤波器系数
float FFC_Output; // 前馈输出值
float FFC_Set_History[3]; // 前馈设定值历史 [NOW, LAST, LLAST]
//-----------------------------------
// for calculating
float Measure;
@@ -113,6 +123,12 @@ typedef struct // config parameter
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
// Feed Forward Control (FFC) parameters
float FFC_Kp; // 前馈比例系数
float FFC_Kd; // 前馈微分系数
float FFC_Ka; // 前馈加速度系数
float FFC_LPF_RC; // 前馈低通滤波器系数
} PID_Init_Config_s;
/**
@@ -133,4 +149,4 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif
#endif