56 lines
1.3 KiB
C
56 lines
1.3 KiB
C
#include "pid.h"
|
|
#include <string.h>
|
|
|
|
void PIDController_Init(PIDController_t *pid)
|
|
{
|
|
if (!pid) return;
|
|
pid->Kp = 0.0f;
|
|
pid->Ki = 0.0f;
|
|
pid->Kd = 0.0f;
|
|
pid->integral = 0.0f;
|
|
pid->prev_error = 0.0f;
|
|
pid->output_max = 204800.0f; // 电机最大速度 (0.01 DPS)
|
|
}
|
|
|
|
float PIDController_Compute(PIDController_t *pid, float target, float actual)
|
|
{
|
|
if (!pid) return 0.0f;
|
|
|
|
float error = target - actual;
|
|
|
|
// 比例项
|
|
float p_term = pid->Kp * error;
|
|
|
|
// 积分项 (带抗饱和)
|
|
pid->integral += error;
|
|
if (pid->integral > pid->output_max) {
|
|
pid->integral = pid->output_max;
|
|
} else if (pid->integral < -pid->output_max) {
|
|
pid->integral = -pid->output_max;
|
|
}
|
|
float i_term = pid->Ki * pid->integral;
|
|
|
|
// 微分项
|
|
float derivative = error - pid->prev_error;
|
|
float d_term = pid->Kd * derivative;
|
|
pid->prev_error = error;
|
|
|
|
float output = p_term + i_term + d_term;
|
|
|
|
// 输出限幅
|
|
if (output > pid->output_max) {
|
|
output = pid->output_max;
|
|
} else if (output < -pid->output_max) {
|
|
output = -pid->output_max;
|
|
}
|
|
|
|
return output;
|
|
}
|
|
|
|
void PIDController_Reset(PIDController_t *pid)
|
|
{
|
|
if (!pid) return;
|
|
pid->integral = 0.0f;
|
|
pid->prev_error = 0.0f;
|
|
}
|