基本演算法

本例採用了增量式PID演算法，算式如下： 
PIDOUT = basicValue + Kp*d_error + Ki*error + Kd*dd_error

其中： 
PIDOUT：PID輸出控制引數，為PWM寬度的調整量 
Kp：比例調節引數 
Ki：積分調節引數 
Kd：微分調節引數 
error：誤差量（為期望速度-實際速度，可以為正數或負數） 
d_error：本次誤差量與上次誤差量之差（error-pre_error） 
dd_error：上次誤差量與上上次誤差量之差（pre_error-pre_pre_error）

控制程式

void SpeedAdjust()
{
    long d_error,dd_error,error
 
;        //宣告變數
    error = (int)(speed_ept - now_speed);           //計算本次誤差(期待速度-當前速度)
    d_error = error - pre_error;        //本次誤差與上次誤差之差
    dd_error = pre_error - pre_pre_error;       //上次誤差與上上次誤差之差
    pre_error = error;          //將本次誤差賦值給上次誤差(下次計算用)
    pre_pre_error = pre_error;      //將上次誤差賦值給上上次誤差(下次計算用)
 

    pwm_tmp = pwm_tmp + PID_P*d_error + PID_I*error + PID_D*dd_error;       //計算pwm寬度調整量
    if(now_speed>22)        //如果當前速度大於22
    {
        Set_DCMotor(0,0);       //不調整電機
    }
    else        //否則
    {
        if(pwm_tmp>=0)      //如果pwm寬度調整為正(增加寬度)
        {
            Set_DCMotor(pwm_tmp,0);     //增加直流電機轉速
 

        }
        else        //否則
        {
            Set_DCMotor(pwm_tmp,1);     //降低直流電機轉速
        }
    }
    before_speed = now_speed;       //講本次速度賦值給上次速度(下次使用)
}

轉自：http://blog.csdn.net/lczdk/article/details/54924825