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