算法模板-PID

int Error[3]={0,0,0};   int Pwm;   int Kp,Ki,Kd;   int speed[2];   int base_speed;      void Mot_PID(int now_cnt, int target_cnt)   {       Error[2]=Error[1];       Error[1]=Error[0];       Error[0] = (target_cnt - now_cnt)>>3;                  Pwm = Pwm + Kp*(Error[0]-Error[1]) + Ki*Error[0] + Kd*(Error[0]-2*Error[1]+Error[2]);                      if (Pwm>0)       {           if (Error[0] > 11) Pwm=1000;           if (Pwm>1000) Pwm=1000;           PWMDTY23=Pwm;           PWMDTY45=0;          }       else       {                   Pwm = -Pwm;          // if (Error[0]<-11) Pwm=1000;   //bang-bang           PWMDTY23=0;           PWMDTY45=Pwm;       }               }      //*****************電機控制******************   void Mot_Ctrl(int center_n){       int y;              y=speed[0]*10+(speed[1]-speed[0])*center_n/4; //get target speed       Mot_PID(y);   }