69 lines
1.9 KiB
C
69 lines
1.9 KiB
C
#include "bsp_pid.h"
|
||
|
||
/**
|
||
* @brief 初始化 PID 控制器参数
|
||
* @param pid 指向 PID 结构体的指针
|
||
* @param kp 比例增益
|
||
* @param ki 积分增益
|
||
* @param kd 微分增益
|
||
* @param out_max 输出最大限幅
|
||
* @param int_max 积分最大限幅 (防止积分饱和)
|
||
*/
|
||
void pid_init(PID_TypeDef *pid, float kp, float ki, float kd, float out_max, float int_max) {
|
||
pid->target = 0.0f;
|
||
pid->actual = 0.0f;
|
||
pid->err = 0.0f;
|
||
pid->err_last = 0.0f;
|
||
pid->Kp = kp;
|
||
pid->Ki = ki;
|
||
pid->Kd = kd;
|
||
pid->integral = 0.0f;
|
||
pid->integral_max = int_max;
|
||
pid->output = 0.0f;
|
||
pid->output_max = out_max;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算 PID 闭环输出
|
||
* @param pid 指向 PID 结构体的指针
|
||
* @param target 目标 RPM
|
||
* @param actual 实际反馈 RPM
|
||
* @return float 计算得出的 PWM 输出值
|
||
*/
|
||
float pid_calculate(PID_TypeDef *pid, float target, float actual) {
|
||
pid->target = target;
|
||
pid->actual = actual;
|
||
pid->err = target - actual;
|
||
|
||
/* 积分累加 */
|
||
pid->integral += pid->err;
|
||
|
||
/* 积分限幅 (Anti-Windup) */
|
||
if (pid->integral > pid->integral_max) pid->integral = pid->integral_max;
|
||
if (pid->integral < -pid->integral_max) pid->integral = -pid->integral_max;
|
||
|
||
/* PID 公式：Output = Kp*err + Ki*integral + Kd*(err - err_last) */
|
||
pid->output = (pid->Kp * pid->err) +
|
||
(pid->Ki * pid->integral) +
|
||
(pid->Kd * (pid->err - pid->err_last));
|
||
|
||
/* 记录误差用于下次计算 */
|
||
pid->err_last = pid->err;
|
||
|
||
/* 输出限幅 */
|
||
if (pid->output > pid->output_max) pid->output = pid->output_max;
|
||
if (pid->output < -pid->output_max) pid->output = -pid->output_max;
|
||
|
||
return pid->output;
|
||
}
|
||
|
||
/**
|
||
* @brief 重置 PID 内部状态 (通常在电机停止时调用)
|
||
*/
|
||
void pid_reset(PID_TypeDef *pid) {
|
||
pid->integral = 0.0f;
|
||
pid->err = 0.0f;
|
||
pid->err_last = 0.0f;
|
||
pid->output = 0.0f;
|
||
}
|