基本算法
本例采用了增量式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;
//講本次速度賦值給上次速度(下次使用)
}
