PID算法的代碼實現

舵機控制

在對舵機的控制中，我采用的是位置式PD算法。

將左右電感的偏差值賦給error，再代入公式進行解算，最后傳給舵機輸出子函數，控制舵機正確打角。

void steer_control()  
{
    error=AD_M_Left[0]-AD_M_Right[0];
    
    direction_controlout=dirP*error+dirD*(error-lasterror);
    angle=mid_angle+direction_controlout;
    
    angle=angle >right_angle?right_angle:angle;
    angle=angle< left_angle?left_angle:angle;
    
    PWM_SetSteer((int)angle); 
    lasterror=error;
    
}

另外，如果想使舵機更順滑，可以對偏差error的計算進行處理，如將歸一化后的值開方處理等。

電機控制

在對電機的控制中，我采用的是增量式PID算法。

首先要設定一個目標速度set_speed，再將當前速度與其比較，得出的差值代入公式計算，得到最終需要輸出的電機占空比，從而控制電機轉速。

需要注意的是，電機PID控制作為閉環控制，最好將其放入PIT定時器中斷，同時在中斷中用左右兩個編碼器分別測出實時速度，這樣可以使其控制的更精確。

void PID_computer_newL(int left_speed_in)
{
    ec_left=set_speed-left_speed_in;
    
    pwm_L+=speedL_P*(ec_left)+speedL_I*(ec_left-eb_left)+speedL_D*(ec_left-2*eb_left+ea_left);
    if(pwm_L > 600)  pwm_L=600;  
    if(pwm_L< -600)  pwm_L=-600; 
    
    PWM_SetMotorL((int)pwm_L);  
    
    ea_left=eb_left;
    eb_left=ec_left;
    
}