算法模板-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);}