|
|
float PID_DATA1(float speed,float snum)//speed 期望速度，snum 實(shí)測(cè)速度
{
float incrementSpeed; //當(dāng)前值
pid.SetSpeed = speed;//設(shè)定的期望速度
pid.err = pid.SetSpeed - snum; //偏差值= 設(shè)定的速度-實(shí)測(cè)的速度
pid.s_error+=pid.err;//偏差積累
incrementSpeed = (pid.Kp + pid.Ki + pid.Kd)*pid.err - ( pid.Kp + 2* pid.Kd)*pid.err_next+pid.Kd*pid.err_last;//計(jì)算
pid.ActualSpeed += incrementSpeed;//實(shí)際輸出值
pid.err_last = pid.err_next;
pid.err_next = pid.err;
if( pid.ActualSpeed>pid.SetSpeedMAX){ pid.ActualSpeed=pid.SetSpeedMAX;}
if( pid.ActualSpeed<-pid.SetSpeedMAX){ pid.ActualSpeed=-pid.SetSpeedMAX;}
return pid.
float PID_DATA2(float speed,float snum)//speed 期望位置，snum 實(shí)測(cè)位置
{
float incrementSpeed; //當(dāng)前值
pid.SetSpeed = speed;//設(shè)定的期望位置
pid.err = pid.SetSpeed - snum; //偏差值= 設(shè)定的位置-實(shí)測(cè)的位置
pid.s_error+=pid.err;//偏差積累
incrementSpeed = (pid.Kp + pid.Ki + pid.Kd)*pid.err - ( pid.Kp + 2* pid.Kd)*pid.err_next+pid.Kd*pid.err_last;//計(jì)算
pid.ActualSpeed = incrementSpeed;//實(shí)際輸出值
pid.err_last = pid.err_next;
pid.err_next = pid.err;
if( pid.ActualSpeed>pid.SetSpeedMAX){ pid.ActualSpeed=pid.SetSpeedMAX;}
if( pid.ActualSpeed<-pid.SetSpeedMAX){ pid.ActualSpeed=-pid.SetSpeedMAX;}
return pid.ActualSpeed;ActualSpeed;
|
|
