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);
}