添加速度位置闭环分离和电机离线保护

application/chassis/balance.c

@@ -186,14 +186,36 @@ static void EnableAllMotor() /* 打开所有电机 */
         LKMotorEnable(driven[i]);
 }
 
+// 检查关节电机是否离线
+static uint8_t JointMotorIsLost()
+{
+    for (uint8_t i = 0; i < JOINT_CNT; i++)
+    {
+        if(joint[i]->motor_daemon->temp_count == 0)
+            return 1;
+    }
+
+    return 0;
+}
+
+// 检查驱动轮电机是否离线
+static uint8_t DrivenMotorIsLost()
+{
+    for(uint8_t i = 0; i < DRIVEN_CNT; i++)
+    {
+        if(driven[i]->daemon->temp_count == 0)
+            return 1;
+    }
+
+    return 0;
+}
+
 /* 切换底盘遥控器控制和云台双板控制 */
 static void ControlSwitch()
 { 
     // 根据裁判系统底盘输出电压设定底盘状态
     float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001;
-    if (chassis_vol < 15.0f ||
-        l_driven->daemon->temp_count == 0 ||
-        r_driven->daemon->temp_count == 0)
+    if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
     {
         chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
         return;
@@ -310,8 +332,6 @@ static void WokingStateSet()
 
     // 加速度限幅,防止键盘控制摔倒
     chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
-    // 模型距离参考输入
-    chassis.target_dist += chassis.target_v * del_t;
 
     // 角度输入
     chassis.target_yaw = chassis_cmd_recv.offset_angle;