#include "PID.h"
signed long v_PIDCalc( PID *pp )
{
signed long error,d_error,dd_error; //相当于公式中的e(k),e(k-1),e(k-2);
error = (signed long)(pp->vi_Ref - pp->vi_FeedBack);//求e(k)
d_error = error - pp->vi_PreError; //求e(k-1)
dd_error = d_error - pp->vi_PreDerror; //求e(k-2)
pp->vi_PreError = error; pp->vi_PreDerror = d_error;//保存这次的e(k)和e(k-1)
/*if( error < VV_DEADLINE && error >0 )pp->vl_PreU+=6;
else if(error > -VV_DEADLINE)pp->vl_PreU-=6;
else
{ }*///死区控制
pp->vl_PreU+= (signed long)( pp -> v_Kp * d_error + pp -> v_Ki * error + pp->v_Kd*dd_error);
//求输出,相当于u(k)=u(k)+Δu(k);
if(error>20|| pp->vl_PreU >= VV_MAX )
{
pp->vl_PreU = VV_MAX;
}
else if( error<-20||pp->vl_PreU <= VV_MIN )
{
pp->vl_PreU = 0;
} //输出限幅处理
return (pp->vl_PreU);
}
评论0