基本算法
本例采用了增量式PID算法，算式如下：
PIDOUT = basicValue + Kp*d_error + Ki*error + Kd*dd_error
其中：
PIDOUT：PID輸出控制參數(shù)，為PWM寬度的調(diào)整量
Kp：比例調(diào)節(jié)參數(shù)
Ki：積分調(diào)節(jié)參數(shù)
Kd：微分調(diào)節(jié)參數(shù)
error：誤差量（為期望速度-實(shí)際速度，可以為正數(shù)或負(fù)數(shù)）
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);
//計(jì)算本次誤差(期待速度-當(dāng)前速度)
d_error =
error - pre_error;
//本次誤差與上次誤差之差
dd_error = pre_error - pre_pre_error;
//上次誤差與上上次誤差之差
pre_error =
error;
//將本次誤差賦值給上次誤差(下次計(jì)算用)
pre_pre_error = pre_error;
//將上次誤差賦值給上上次誤差(下次計(jì)算用)
pwm_tmp = pwm_tmp + PID_P
*d_error + PID_I
*error + PID_D
*dd_error;
//計(jì)算pwm寬度調(diào)整量
if(now_speed>
22)
//如果當(dāng)前速度大于22
{
Set_DCMotor(
0,
0);
//不調(diào)整電機(jī)
}
else //否則
{
if(pwm_tmp>=
0)
//如果pwm寬度調(diào)整為正(增加寬度)
{
Set_DCMotor(pwm_tmp,
0);
//增加直流電機(jī)轉(zhuǎn)速
}
else //否則
{
Set_DCMotor(pwm_tmp,
1);
//降低直流電機(jī)轉(zhuǎn)速
}
}
before_speed = now_speed;
//講本次速度賦值給上次速度(下次使用)
}
