From 8fc322fd517938a82929b1e4ded098381046540f Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sun, 28 Apr 2024 22:25:29 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E6=80=A5=E5=88=B9=E6=83=85?=
 =?UTF-8?q?=E5=86=B5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 21 +++++++++------------
 application/chassis/balance.h |  2 +-
 2 files changed, 10 insertions(+), 13 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 00278db..caf46ea 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -109,7 +109,7 @@ void BalanceInit()
     // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
         .Kp = 600,
-        .Kd = 200,
+        .Kd = 100,
         .Ki = 0,
         .MaxOut = 20,
         .DeadBand = 0.0001f,
@@ -121,8 +121,8 @@ void BalanceInit()
 
     // roll轴补偿
     PID_Init_Config_s roll_compensate_pid_conf = {
-        .Kp = 0.0006f,
-        .Kd = 0.00005f,
+        .Kp = 0.0008f,
+        .Kd = 0.0001f,
         .Ki = 0.0f,
         .MaxOut = 0.04,
         .DeadBand = 0.001f,
@@ -137,7 +137,7 @@ void BalanceInit()
         .Kp = 5,
         .Kd = 0,
         .Ki = 0.0f,
-        .MaxOut = 4,
+        .MaxOut = 3,
         .DeadBand = 0.001f,
         .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
         .Derivative_LPF_RC = 0.05,
@@ -158,7 +158,7 @@ void BalanceInit()
     // 抗劈叉
     PID_Init_Config_s anti_crash_pid_conf = {
         .Kp = 8,
-        .Kd = 2.5,
+        .Kd = 2,
         .Ki = 0.0,
         .MaxOut = 10,
         .DeadBand = 0.001f,
@@ -197,7 +197,7 @@ static void ControlSwitch()
         else
         {
             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.vx = 0.004 * (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.000005 * (float)rc_data[TEMP].rc.rocker_r_;
         }
@@ -291,10 +291,7 @@ static void WokingStateSet()
     VAL_LIMIT(r_side.target_len, 0.12, 0.25);
 
     // 加速度限幅,防止键盘控制摔倒
-    if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF)
-        chassis.target_v = chassis_cmd_recv.vx;
-    else
-        chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
+    chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
     // 模型距离参考输入
     chassis.target_dist += chassis.target_v * del_t;
 
@@ -355,7 +352,7 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
     r_side.T_wheel += steer_v_pid.Output;
 
     // 抗劈叉
-    static float swerving_speed_ff, ff_coef = 0;
+    static float swerving_speed_ff, ff_coef = 3;
     swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
     PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
     l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
@@ -369,7 +366,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
     l_side.target_len += roll_compensate_pid.Output;
     r_side.target_len -= roll_compensate_pid.Output;
 
-    static float gravity_comp = 60;
+    static float gravity_comp = 80;
     static float roll_extra_comp_p = 300;
     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;
diff --git a/application/chassis/balance.h b/application/chassis/balance.h
index 204764c..f54e9a2 100644
--- a/application/chassis/balance.h
+++ b/application/chassis/balance.h
@@ -8,7 +8,7 @@
 #define LIMIT_LINK_RAD 0.220039368      // 初始限位角度,见ParamAssemble
 #define BALANCE_GRAVITY_BIAS 0
 #define ROLL_GRAVITY_BIAS 0
-#define MAX_ACC_REF 0.5f
+#define MAX_ACC_REF 0.8f
 #define MAX_DIST_TRACK 1.0f
 #define MAX_VEL_TRACK 0.5f