From 66e23202a77f3290c9da401ac9556e1b9235b0b0 Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sun, 19 May 2024 21:20:48 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=80=9F=E5=BA=A6=E4=BD=8D?=
 =?UTF-8?q?=E7=BD=AE=E9=97=AD=E7=8E=AF=E5=88=86=E7=A6=BB=E5=92=8C=E7=94=B5?=
 =?UTF-8?q?=E6=9C=BA=E7=A6=BB=E7=BA=BF=E4=BF=9D=E6=8A=A4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c          | 30 +++++++++++++++++++++-----
 application/chassis/balance.h          |  8 +++----
 application/chassis/lqr_calc.h         | 24 ++++++++++-----------
 application/chassis/speed_estimation.h | 10 ++++++++-
 4 files changed, 49 insertions(+), 23 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 5b9a3f0..95332e0 100644
--- a/application/chassis/balance.c
+++ b/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; 
diff --git a/application/chassis/balance.h b/application/chassis/balance.h
index c695ea0..c965678 100644
--- a/application/chassis/balance.h
+++ b/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_L 0.1f
-#define CENTER_IMU_H -0.055f
+#define CENTER_IMU_W -0.024f
+#define CENTER_IMU_L 0.09f
+#define CENTER_IMU_H -0.1f
 
 #define VEL_PROCESS_NOISE 10  // 速度过程噪声
 #define VEL_MEASURE_NOISE 2000 // 速度测量噪声
diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h
index dede5b7..40c4458 100644
--- a/application/chassis/lqr_calc.h
+++ b/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,
-                            1.620796,-4.331826,-0.454705,
-                            32.093563,-25.558681,-16.605856,
-                            19.128242,-17.562747,-10.815380,
-                            225.373594,-201.324771,53.236052,
-                            13.575849,-13.013546,3.845604,
-                            76.704297,-72.201666,20.359891,
-                            4.520141,-4.051799,1.189345,
-                            139.418565,-125.750074,34.110069,
-                            88.295852,-79.170124,21.430975,
-                            -163.725633,127.852860,110.004619,
-                            -9.557678,7.476790,4.655220};
+    static float k[12][3] = {64.570926,-78.151551,-15.098827,
+                            1.716153,-4.573289,-0.526836,
+                            25.565621,-20.634320,-17.563574,
+                            11.633020,-11.021911,-13.087704,
+                            212.754066,-196.450645,56.227305,
+                            12.051262,-11.941105,4.523285,
+                            74.149148,-71.296364,23.673363,
+                            4.449069,-4.214434,1.145555,
+                            115.321993,-106.373546,29.626387,
+                            82.459182,-74.915697,20.072512,
+                            -163.793049,130.808796,114.419419,
+                            -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;
diff --git a/application/chassis/speed_estimation.h b/application/chassis/speed_estimation.h
index 1e79951..44f679c 100644
--- a/application/chassis/speed_estimation.h
+++ b/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;
 }
\ No newline at end of file