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