新手上路
- 积分
- 25
- 金钱
- 25
- 注册时间
- 2019-7-16
- 在线时间
- 8 小时
|
求大佬帮忙解释哈这一函数,不胜感激
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=0;
//=============遥控左右旋转部分=======================//
if(1==Flag_Left||1==Flag_Right) //这一部分主要是根据旋转前的速度调整速度的起始速度,增加小车的适应性
{
if(++Turn_Count==1)
Encoder_temp=myabs(encoder_left+encoder_right);
Turn_Convert=50/Encoder_temp;
if(Turn_Convert<0.6)Turn_Convert=0.6;
if(Turn_Convert>3)Turn_Convert=3;
}
else
{
Turn_Convert=0.9;
Turn_Count=0;
Encoder_temp=0;
}
if(1==Flag_Left) Turn_Target-=Turn_Convert;
else if(1==Flag_Right) Turn_Target+=Turn_Convert;
else Turn_Target=0;
if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //===转向速度限幅
if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
if(Flag_Qian==1||Flag_Hou==1) Kd=0.5;
else Kd=0; //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
//=============转向PD控制器=======================//
Turn=-Turn_Target*Kp -gyro*Kd; //===结合Z轴陀螺仪进行PD控制
return Turn;
}
|
|