PID算法的代码实现

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