typedef struct {
double SetPoint; // 目标值
double Proportion; // 比例系数
double Integral; // 积分系数
double Derivative; // 微分系数
double LastError; // 上一次误差
double PrevError; // 上上次误差
double SumError; // 误差累积
} PID;
double PIDCalc(PID *pp, double NextPoint) {
double Error = pp->SetPoint - NextPoint; // 计算误差
pp->SumError += Error; // 积分项
double dError = pp->LastError - pp->PrevError; // 微分项
pp->PrevError = pp->LastError;
pp->LastError = Error;
// 计算控制量
return (pp->Proportion * Error +
pp->Integral * pp->SumError +
pp->Derivative * dError);
}
// 初始化PID结构体
void PIDInit(PID *pp) {
memset(pp, 0, sizeof(PID));
}
|
