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

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 ||
+    if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
-        l_driven->daemon->temp_count == 0 ||
-        r_driven->daemon->temp_count == 0)
     {
         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; 

application/chassis/balance.h

@@ -11,16 +11,14 @@
 #define BALANCE_GRAVITY_BIAS 0
 #define ROLL_GRAVITY_BIAS 0
 #define MAX_ACC_REF 0.9f
-#define MAX_DIST_TRACK 1.0f
-#define MAX_VEL_TRACK 0.5f
 
 // 驱动轮质量
 #define WHEEL_MASS 0.58f
 
 // IMU距离中心的距离
-#define CENTER_IMU_W 0
+#define CENTER_IMU_W -0.024f
-#define CENTER_IMU_L 0.1f
+#define CENTER_IMU_L 0.09f
-#define CENTER_IMU_H -0.055f
+#define CENTER_IMU_H -0.1f
 
 #define VEL_PROCESS_NOISE 10  // 速度过程噪声
 #define VEL_MEASURE_NOISE 2000 // 速度测量噪声

application/chassis/lqr_calc.h

@@ -9,18 +9,18 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    static float k[12][3] = {62.680622,-74.772126,-13.135672,
+    static float k[12][3] = {64.570926,-78.151551,-15.098827,
-                            1.620796,-4.331826,-0.454705,
+                            1.716153,-4.573289,-0.526836,
-                            32.093563,-25.558681,-16.605856,
+                            25.565621,-20.634320,-17.563574,
-                            19.128242,-17.562747,-10.815380,
+                            11.633020,-11.021911,-13.087704,
-                            225.373594,-201.324771,53.236052,
+                            212.754066,-196.450645,56.227305,
-                            13.575849,-13.013546,3.845604,
+                            12.051262,-11.941105,4.523285,
-                            76.704297,-72.201666,20.359891,
+                            74.149148,-71.296364,23.673363,
-                            4.520141,-4.051799,1.189345,
+                            4.449069,-4.214434,1.145555,
-                            139.418565,-125.750074,34.110069,
+                            115.321993,-106.373546,29.626387,
-                            88.295852,-79.170124,21.430975,
+                            82.459182,-74.915697,20.072512,
-                            -163.725633,127.852860,110.004619,
+                            -163.793049,130.808796,114.419419,
-                            -9.557678,7.476790,4.655220};
+                            -13.396998,11.357026,4.288949};
     float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;

application/chassis/speed_estimation.h

@@ -59,5 +59,13 @@ void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS
     cp->vel_cov = (1 - k) * vel_cov;                        // 后验协方差
 
     VAL_LIMIT(cp->vel_cov, 0.01, 100);       // 协方差限幅
-    cp->dist = cp->dist + cp->vel * delta_t;
+
+    // 速度和位置分离，有速度输入时不进行位置闭环
+    if(abs(cp->target_v) < 0.001)
+    {
+        cp->target_dist = 0;
+        cp->dist += cp->vel * delta_t;
+    }
+    else
+        cp->target_dist = cp->dist = 0;
 }