From 41e13aee44e4c2c289c797d20330d95af874efbf Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Wed, 3 Apr 2024 22:34:36 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=A2=9E=E7=9B=8AK?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c  |  4 ++--
 application/chassis/balance.h  |  2 ++
 application/chassis/lqr_calc.h | 24 ++++++++++++------------
 3 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 291de3b..1b43b38 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -199,7 +199,7 @@ static void ControlSwitch()
             chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
             chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
             chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial;
-            chassis_cmd_recv.offset_angle -= 0.00001 * (float)rc_data[TEMP].rc.rocker_r_;
+            chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
         }
     }
     else
@@ -359,7 +359,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
     r_side.target_len -= roll_compensate_pid.Output;
 
     static float gravity_comp = 57.63;
-    static float roll_extra_comp_p = 300;
+    static float roll_extra_comp_p = 400;
     float roll_comp = roll_extra_comp_p * chassis.roll;
     l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp;
     r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp;
diff --git a/application/chassis/balance.h b/application/chassis/balance.h
index 16f58f4..3c8e3e6 100644
--- a/application/chassis/balance.h
+++ b/application/chassis/balance.h
@@ -9,6 +9,8 @@
 #define BALANCE_GRAVITY_BIAS 0
 #define ROLL_GRAVITY_BIAS 0
 #define MAX_ACC_REF 0.5f
+#define MAX_DIST_TRACK 1.0f
+#define MAX_VEL_TRACK 0.5f
 
 // IMU距离中心的距离
 #define CENTER_IMU_W 0
diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h
index 394d1cb..6521f14 100644
--- a/application/chassis/lqr_calc.h
+++ b/application/chassis/lqr_calc.h
@@ -9,18 +9,18 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    float k[12][3] = {85.365277,-164.978797,-4.643325,
-                    -7.817260,-26.353895,0.955791,
-                    43.461413,-37.225135,-12.040913,
-                    34.470271,-39.045724,-7.813613,
-                    171.639863,-172.946723,59.632812,
-                    11.281864,-11.926075,4.239721,
-                    -19.517972,1.032904,27.896023,
-                    -10.715152,11.663766,2.651705,
-                    96.039929,-99.227575,36.121143,
-                    56.371947,-60.347262,25.166639,
-                    -213.707672,181.799382,93.194337,
-                    -14.287060,12.217074,3.285836};
+    float k[12][3] = {155.616146,-200.327241,-0.278791,
+                        -12.564474,-24.018026,0.674594,
+                        145.503085,-87.742553,-5.916048,
+                        97.588867,-70.361944,-4.007305,
+                        303.894243,-240.356086,68.074439,
+                        18.743007,-15.738160,4.718314,
+                        -170.799127,75.225969,18.998160,
+                        -29.312006,20.892957,1.529644,
+                        122.553208,-114.023608,38.147691,
+                        63.561616,-64.916409,25.861226,
+                        -657.233777,402.487049,66.281303,
+                        -40.107302,25.117848,1.705173};
     float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;