void Angle_Calculate()
{
int16_t AngleControlOut_P,AngleControlOut_D;
angle_Accel = (A_Z-AZ_ZERO);
angle_Accel*=0.0039f;
angle_Accel = asin(angle_Accel);
angle_Accel*=500;
Gyro_Now = (T_Y-TY_OFFSET) * TY_Ratio;
第 26 页 共 47 页
Erjie_Lvbo(angle_Accel,Gyro_Now);
if(FilterSwitch==0) myfilter=QingHua_AngleCalaulate;
else if(FilterSwitch==1)myfilter=Yijie_Lvbo;
else if(FilterSwitch==2) myfilter=Erjie_Lvbo;
else if(FilterSwitch==3) myfilter=Kalman_Filter;
if(FilterSwitch==3)
{
myfilter(angle_Accel,Gyro_Now);
ZL.error=angle_now_kal-(Balance_Point);
}
else
{
myfilter(angle_Accel,Gyro_Now);
ZL.error=angle_now-(Balance_Point);
}
AngleControlOut_P=-(int16_t)(ZL.P*ZL.error);
AngleControlOut_D=-(int16_t)(ZL.D*Gyro_Now);
if(AngleControlOut_D>500) AngleControlOut_D=500;//对微分项限幅
else if(AngleControlOut_D<-500) AngleControlOut_D=-500;
AngleControlOut=AngleControlOut_P AngleControlOut_D;
if(AngleControlOut>800) AngleControlOut=800;
else if(AngleControlOut<-800) AngleControlOut=-800;
}
3.4.4 速度控制
直立车模的速度控制方法与普通车模的速度控制方法不同， 因为普通车模的电机只需要控制转动， 而直立车模的电机输出是平衡控制、 速度控制、 方向控制三部分之间的叠加。
因为直立系统的首要任务是保持直立， 当车模倾角发生改变， 车模就会在直立控制的作
用下向倾斜方向运动。 速度控制就是利用了直立系统的这种特性。
改变车模倾角有两种方法：
（1） 改变车模直立控制的目标角度， 直立控制 PD 算法中， 有一个目标倾角， 当目
标倾角不是真正的平衡位置倾角时， 车模会一直运动。 通过编码器测量转速， 形成
速度反馈回路， 就可以使用 PID 算法进行速度调节。
（2） 给电机施加干扰输出。 在直立过程中， 如果增加或者减少电机输出， 车模就很难维持平衡， 在直立控制的作用下， 车模就会运行。 通过编码器测量转速， 形成速度反馈回路， 就可以使用 PID 算法进行速度调节。
学长设计的系统中，使用第二种方法对速度进行控制， 因为第二种方法对直立的影响较小，可以提高系统的稳定性， 但是速度控制的灵敏度没有第一种高。
void Speed_Calculate()
{
SP.goal=(t1-t1_mid)/10;
if(SP.goal>50) SP.goal=50;
else if(SP.goal<-30) SP.goal=-30;
SpeedControlOut_Old=SpeedControlOut;
SP.error=(SP.goal)-(Speed_L Speed_R)/2;
SP.error_sum =SP.error;
if(SP.error_sum>200) SP.error_sum=200;//积分限幅
else if(SP.error_sum<-200) SP.error_sum=-200;
SpeedControlOut=-(int16_t)((SP.P)*SP.error SP.I*SP.error_sum);
if(SpeedControlOut>300) SpeedControlOut=300;//速度环输出限幅
else if(SpeedControlOut<-300) SpeedControlOut=-300;
}
3.4.5 方向控制
直立车模的转向是通过两轮的差速来完成的， 转向控制使用 PD 算法， 小车不具备自主识别路径的功能， 因此 PD 算法的输入参数使用遥控器完成。 为了改善转向的动态特性， 避免振荡， 对 PD 算法做了修改， 微分项的因子不再使用传统的 e(t) -e(t-1) ，而是使用陀螺仪的 X 轴， 因为陀螺仪输出灵敏度更高。
void Direction_Calculate(int16_t bias)
{
DIR.last_error=DIR.error;
DIR.error=bias/6-dmp_yaw;
DIR.error_sum =DIR.error;
if(DIR.error_sum>2000) DIR.error_sum=2000;
else if(DIR.error_sum<-2000) DIR.error_sum=-2000;
DirectionControlOut_Old=DirectionControlOut;
DirectionControlOut = DIR.P*DIR.error
DIR.I*DIR.error_sum
DIR.D*(T_X-TX_OFFSET);
DirectionControlOut_Old=DirectionControlOut;
DirectionControlOut=bias*0.15f;
if(DirectionControlOut>300) DirectionControlOut=300;
else if(DirectionControlOut<-300) DirectionControlOut=-300;
}
完整代码可进群免费领取。
嵌入式物联网的学习之路非常漫长，不少人因为学习路线不对或者学习内容不够专业而错失高薪offer。不过别担心，我为大家整理了一份150多G的学习资源，基本上涵盖了嵌入式物联网学习的所有内容。点击下方链接，0元领取学习资源，让你的学习之路更加顺畅！记得点赞、关注、收藏、转发哦！
点击这里找小助理0元领取：
