PID模板

typedef struct
{
float Kp,Ki,Kd;
float Target;
float Current;
float Error[3];
float DeadZone;
float Output;
float Increment;
float OutputUpper;
float OutputLower;
} PID_TypeDef;

void PID_Calculate(PID_TypeDef *Data)
{
float Inc, P, I, D, Err;

Err = Data->Target - Data->Current;

Data->Error[0] = Data->Error[1];
Data->Error[1] = Data->Error[2];
Data->Error[2] = (Abs_float(Err) > Data->DeadZone) * Err;

P = Data->Kp*(Data->Error[2]-Data->Error[1]);
I = Data->Ki*(Data->Error[2]);
D = Data->Kd*(Data->Error[2]-2*Data->Error[1]+Data->Error[0]);
Inc = (P+I+D);

Data->Increment = Inc;

Data->Output += Inc;
Data->Output = Min_float(Data->Output, Data->OutputUpper);
Data->Output = Max_float(Data->Output, Data->OutputLower);
}

void PID_Clear(PID_TypeDef *Data)
{
Data->Current = 0;
Data->Target = 0;

Data->Error[0] = 0;
Data->Error[1] = 0;
Data->Error[2] = 0;

Data->Increment = 0;
Data->Output = 0;
}

巴特西

PID模板

最新文章

热门文章