typedef struct {
int16_t Kp; // 比例增益
int16_t Ki; // 积分增益
int16_t Kd; // 微分增益
int16_t integral; // 积分累加
int16_t last_error; // 上一次误差
int16_t output_max; // 输出最大值
int16_t output_min; // 输出最小值
} PIDController;
int16_t PID_Compute(PIDController *pid, int16_t setpoint, int16_t measured_value) {
int16_t error = setpoint - measured_value; // 误差计算
int16_t proportional = pid->Kp * error; // 比例项
// 积分项（防止积分饱和）
pid->integral += pid->Ki * error;
if (pid->integral > pid->output_max) {
pid->integral = pid->output_max;
} else if (pid->integral < pid->output_min) {
pid->integral = pid->output_min;
}
// 微分项
int16_t derivative = pid->Kd * (error - pid->last_error);
// 计算输出
int16_t output = proportional + pid->integral + derivative;
// 输出限制
if (output > pid->output_max) {
output = pid->output_max;
} else if (output < pid->output_min) {
output = pid->output_min;
}
// 更新上一次误差
pid->last_error = error;
return output;
}
|
楼主
