模糊pid算法控制速度

typedef struct
{
 int et;//当前状态的误差
 int pre_et;//前一状态的误差
 int for_et;
 uchar output;
} PID;

 

 

uchar pid(uint set_speed,uint real_speed,PID my)
{
 char  data ec;
 uchar idata kp,ki,kd;
 uchar out;
 int temp;
 my.et=(int)(set_speed-real_speed)>>3;//Deviation
 if(my.et>3)
 {
  out=30; 
 }
 else
 { 
  if(my.et<-3)
  {
   out=0;
  }
  else
  {
   ec=(my.et-my.pre_et)>>2;
   kp=-ec-my.et;//Fuzzy p,i,d
   ki=ec+my.et;
   kd=my.et+abs(ec);
   temp=my.output+(my.et*(kp+ki+kd)+my.pre_et*(-kp-(kd<<1))+my.for_et*kd)<<3;//pid adjust
   if(temp<1)
    out=1;
   if(temp>29)
    out=29;
   if(temp>0&&temp<29)
    out=temp;
  }
 }
 my.for_et=my.pre_et;//prepare for the next
 my.pre_et=my.et;
 my.output=out;
 return(out);
}