From 12be1e96de79ac8f374a2f070ab0daeab5c86e54 Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 14:29:29 +0800
Subject: [PATCH] =?UTF-8?q?=E6=88=91=E8=8D=89=E5=8F=AF=E4=BB=A5=E4=BA=86?=
 =?UTF-8?q?=EF=BC=8C=E4=BD=86=E6=98=AF=E6=9C=89=E7=82=B9=E4=B8=8D=E5=B9=B3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index cbba77b..097df29 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -31,7 +31,8 @@
 #define STOOL_PITCH_K 24.0f
 #define STOOL_PITCH_W_K 6.8f
 #define STOOL_VEL_K 3.2f
-#define STOOL_STOP_DIST_K 1.2f
+#define STOOL_STOP_DIST_K 0.0f
+#define STOOL_PITCH_REF -0.05f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
 #define STOOL_RC_DEADBAND 40
@@ -430,7 +431,7 @@ static void ParamAssemble()
 {
     // 机体参数,视为平面刚体
     chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
-    chassis.pitch_w = Chassis_IMU_data->Gyro[0];
+    chassis.pitch_w = Chassis_IMU_data->Gyro[1];
     chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
     chassis.wz = Chassis_IMU_data->Gyro[2];
     chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
@@ -463,7 +464,8 @@ static void StoolModeBalanceControl(void)
         stool_stop_dist = stool_dist;
 
     float stop_dist_error = stool_dist - stool_stop_dist;
-    float wheel_torque = -STOOL_PITCH_K * chassis.pitch -
+    float pitch_error = chassis.pitch - STOOL_PITCH_REF;
+    float wheel_torque = -STOOL_PITCH_K * pitch_error -
                          STOOL_PITCH_W_K * chassis.pitch_w +
                          STOOL_VEL_K * (stool_vel - chassis.target_v) +
                          STOOL_STOP_DIST_K * stop_dist_error;