struct PID_Value
{
float Target;
float FeedBack;
float ErrNow,ErrLast;
float K_P,K_I,K_D;
float OUT_P,OUT_I,OUT_D;
float OUT_I_MAX,OUT_I_MIN;
};
float Function_PID(struct PID_Value *PID)
{
float Out;
PID->ErrNow = PID->Target - PID->FeedBack;
PID->OUT_P = PID->K_P * PID->ErrNow;
PID->OUT_I += PID->K_I * PID->ErrNow;
if(PID->OUT_I > PID->OUT_I_MAX)
{
PID->OUT_I = PID->OUT_I_MAX;
}
else if(PID->OUT_I < PID->OUT_I_MIN)
{
PID->OUT_I = PID->OUT_I_MIN;
}
PID->OUT_D = PID->K_D * (PID->ErrNow-PID->ErrLast);
PID->ErrLast = PID->ErrNow;
Out = PID->OUT_P + PID->OUT_I + PID->OUT_D;
if(Out > PID->OUT_I_MAX)
{
return PID->OUT_I_MAX;
}
else if(Out < PID->OUT_I_MIN)
{
return PID->OUT_I_MIN;
}
else
{
return Out;
}
}
| 
楼主
标题置顶
标题高亮
