From 0a442e8976f6e7354a757ca16d75c292f950b50b Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sun, 15 Mar 2026 21:27:17 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=89=8A=E5=BC=B1=E5=87=BD?=
 =?UTF-8?q?=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/balance_chassis.c | 20 ++++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 05ba90e..15961e5 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -486,9 +486,22 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       /* 运行LQR和VMC */
       Chassis_LQRControl(c, &climb_cmd);
 
-      /* 削弱LQR的摆力矩Tp，让腿撞到台阶后能自由后摆 */
-      c->lqr[0].control_output.Tp *= c->param->climb.tp_scale;
-      c->lqr[1].control_output.Tp *= c->param->climb.tp_scale;
+      /* 摆力矩Tp随theta非线性缩放：撞到台阶前保持正常，撞到后快速降低 */
+      float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
+      float theta_abs = fabsf(avg_theta);
+      float contact_angle = c->param->climb.theta_retract_threshold * 0.5f;
+      float tp_min = c->param->climb.tp_scale;
+      float dynamic_scale;
+      if (theta_abs < contact_angle) {
+        dynamic_scale = 1.0f;  /* 未撞到，保持正常Tp */
+      } else {
+        float t = (theta_abs - contact_angle) /
+                  (c->param->climb.theta_retract_threshold - contact_angle);
+        t = LIMIT(t, 0.0f, 1.0f);
+        dynamic_scale = 1.0f + (tp_min - 1.0f) * sqrtf(t);  /* sqrt: 撞到后快速衰减 */
+      }
+      c->lqr[0].control_output.Tp *= dynamic_scale;
+      c->lqr[1].control_output.Tp *= dynamic_scale;
 
       Chassis_VMCControl(c, &climb_cmd);
       Chassis_Output(c);
@@ -496,7 +509,6 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       /* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */
       bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
                            (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
-      float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
       if (elapsed_time > 300 && legs_extended &&
           fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
         c->climb.state = CLIMB_RETRACT;