** ===================================================================
** SpeedPID
輸入：speedCount采集車速，AmSpeed 目標車速 ；
輸出 ：SpeedPWMOUT 計算車速 ；
** ===================================================================
*/
int16 SpeedControl(int16 speedCount,int16 AmSpeed,uint8 speedKP,uint8 speedKI,uint8 speedKD)
{
*******************************************************************************************
//speedCount 檢測值
//AmSpeed 目標值
//speedKP P
//speedKI I
//speedKD D
static float Speed1_Err,SumErrSpeed; //靜態變量存儲中間變量
float Speed2_Err,Speed_EC;
float Speed_P_Value,Speed_D_Value ;
static int16 SpeedPWMOUT;
Speed2_Err = Speed1_Err ; //將上一次的偏差保存(上一次的偏差值為0)
Speed1_Err = AmSpeed - speedCount ; // 計算新的偏差值（目標值-傳感器檢測值）
Speed_EC = Speed1_Err - Speed2_Err; // 計算新的偏差變化值 （上次的偏差值-上上次的偏差值）
Speed_P_Value = Speed1_Err * speedKP/10.0 ; // 增量式PID控制計算P調節量
SumErrSpeed += Speed1_Err * speedKI ; //增量式PID控制計算I調節量
Speed_D_Value = Speed_EC * speedKD/100.0 ; // 增量式PID控制計算D調節量
SpeedPWMOUT += (int16)(Speed_P_Value + SumErrSpeed + Speed_D_Value);
//88888888888888888888888888888888 限幅 7777777777777777777777777777777
if(SpeedPWMOUT < SPEED_PWM_MIN )
{
SpeedPWMOUT = SPEED_PWM_MIN ;
}
else if(SpeedPWMOUT > SPEED_PWM_MAX)
{
SpeedPWMOUT = SPEED_PWM_MAX ;
}
if(SpeedPWMOUT<=0)SpeedPWMOUT=0;
//888888888888888888888888888888888 限幅 77777777777777676666777777777
return SpeedPWMOUT ;
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
