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

补充：软件开发 , C++ ,

上一个：HackerRank Algorithms-Search-Pairs 数组的固定差对数
下一个：hdu 2440 Watch out the Animal(凸包+费马点)