From 593cf97ae2b9e6de749be553c51162b815ccbaef Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sun, 5 Oct 2025 17:16:04 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E8=AF=95?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/balance_chassis.c | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 58273fa..d03439d 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -218,7 +218,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
 
   case CHASSIS_MODE_RECOVER: {
     float fn;
-    fn = -15.0f;
+    fn = -25.0f;
 
     VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
     VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
@@ -263,6 +263,10 @@ void Chassis_Output(Chassis_t *c) {
   // }
   float out[4] = {
       c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]};
+      out[0] = -1.0f;
+      out[1] = 1.0f;
+      out[2] = 1.0f;
+      out[3] = -1.0f;
   Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
   Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]);
   // for (int i = 0; i < 2; i++) {
@@ -332,8 +336,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
 
   /* 轮毂力矩输出（参考C++版本的减速比） */
-  c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
-  c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
+  // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
+  // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
+  c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f;
+  c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f;
   /* 腿长控制和VMC逆解算（使用PID控制） */
   float virtual_force[2];
   float target_L0[2];