动态限位

目的分析：

由于以前的限位使用的数据从imu中获取，为固定限位，而机器人的真实限位应该以底盘所在平面为pitch轴0度所在，无法满足机器人在斜坡上时的抬头低头。为此在原来限位的基础上增加了动态限位，可以使得机器人更加适应斜坡地形。

算法思路：

此版动态限位的大体思想是通过imu和电机编码器数据的融合得到真实的限位值。由于电机编码器在相同位置的读数是固定的，因此当底盘水平，云台水平时，每辆车pitch轴电机会得到一个固定的编码器的值，记为。根据电机安装的方向不同，云台朝上和朝下转动的时候电机编码器读数的增大减小也不确定。这里假设云台向上时电机编码器的读数增大。分别记imu获得的角度和编码器读取的角度为，pitch相对底盘坐标系的限位为，相对地面坐标系的限位为

当底盘水平时，如果云台向上运动，有。

而当底盘和地面有一个夹角时，假设此时云台依旧与地面平行，即，则以底盘坐标系来看，云台实际上是一个低头的状态，低头的夹角和地盘与地面的夹角相同，因此编码器的角度应该为

移项得到

则云台相对于地面的限位应该为

整理得到

而当云台不与底盘平行时，注意此时电机编码器的角度也随之改变。

此时需要引入imu的读数

他的作用在于补偿编码器读数和水平时编码器读数的差值，使得我们能够用上上面得到的限幅公式，有

云台与底盘的实际夹角为

于是可以得到此时的限幅为

对于步兵来说，电机安装为正，公式与上面推导相同。

对于英雄来说，电机安装为负，云台向上时电机编码器读数减小，则公式应该为

代码介绍：

动态限位的核心为Gimbal_get_DeltaAtti。

void Gimbal_get_DeltaAtti(void)
{
      static rt_tick_t Atti_NowTick,Atti_LastTick;
	  static int Now_Delta_Atti = 0;
	  static float Delta_Atti_Float = 0;
	  static float Atti_DMPitch = 0, TempDelta_Atti = 0;
		Atti_DMPitch = uint_to_float(DMPitch.dji.lastangle, -PI, PI, 16) / PI * 180 + PITCH_ENCOD_ERROR;
#if defined CORE_USING_HERO	
	  TempDelta_Atti = Atti_DMPitch + GimbalFB.FB_This.Pitch;
#elif defined CORE_USING_INFANTRY
	  TempDelta_Atti = GimbalFB.FB_This.Pitch - Atti_DMPitch;
#endif
		Delta_Atti_Float = UTILS_LP_FAST(Delta_Atti_Float, TempDelta_Atti, 0.1f);
	  Now_Delta_Atti =(int)Delta_Atti_Float / 2;
	  Now_Delta_Atti = Now_Delta_Atti * 2;
	  Atti_NowTick = rt_tick_get();
	
	  if(Now_Delta_Atti != Delta_Atti)
		{
				if(Atti_NowTick - Atti_LastTick > 100)
				{
						Delta_Atti = Now_Delta_Atti;
				}
		}
		else
		{
				Atti_LastTick = Atti_NowTick;
		}
}

这个函数的目的是获取给限幅增加的值，里面PITCH_ENCOD_ERROR是零位置的编码器读数取反的结果，根据不同兵种来选择对应的公式。

但是由于imu和编码器本身的数据波动，如果直接套用公式会导致限幅在小范围内振动，使得云台在限幅时高频抖动。为此我做了处理。

1.进行了低通滤波，消除高频振动。

2.通过对2取整的方式，将波形由小幅高频振动变化转化为大幅低频阶梯状变化。值得注意的是，为了防止限位在2的倍数附近导致高频抖动的情况发生，我对于Delta_Atti进行了有关时间的处理。当Delta_Atti发生变化且变化大于100ms时，才表明电机的位置确实发生了变化，此时才给Delta_Atti赋新值。

每次更新时，记录TickAtti为设定值获取的次数。当刚刚启动时，云台imu有读数但是电机并没有上电，此时不能更新设定值，否则会出现电机无法转动的情况。当TickAtti大于一定值时，云台已经完全启动成功，此时再进行赋值。