diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 7fb3cf5..e18b673 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -64,7 +64,23 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   PID_Reset(&c->pid.tp[0]);
   PID_Reset(&c->pid.tp[1]);
 
-  c->yaw_control.target_yaw = c->feedback.yaw.rotor_abs_angle;
+  // 双零点自动选择逻辑（使用user_math的CircleError函数）
+  float current_yaw = c->feedback.yaw.rotor_abs_angle;
+  float zero_point_1 = c->param->mech_zero_yaw;
+  float zero_point_2 = zero_point_1 + M_PI; // 第二个零点，相差180度
+  
+  // 使用CircleError计算到两个零点的角度差（范围为M_2PI）
+  float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
+  float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
+  
+  // 选择误差绝对值更小的零点作为目标，并记录是否使用了第二个零点
+  if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
+    c->yaw_control.target_yaw = zero_point_1;
+    c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
+  } else {
+    c->yaw_control.target_yaw = zero_point_2;
+    c->yaw_control.is_reversed = true;  // 使用第二个零点，需要反转前后方向
+  }
 
   // 清空位移
   c->chassis_state.position_x = 0.0f;
@@ -182,6 +198,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   c->yaw_control.target_yaw = 0.0f;
   c->yaw_control.current_yaw = 0.0f;
   c->yaw_control.yaw_force = 0.0f;
+  c->yaw_control.is_reversed = false;
 
   return CHASSIS_OK;
 }
@@ -363,7 +380,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   xhat = c->chassis_state.position_x;
 
   // 目标速度设定（原始期望速度）
-  float desired_velocity = c_cmd->move_vec.vx * 2;
+  float raw_vx = c_cmd->move_vec.vx * 3;
+  
+  // 根据零点选择情况决定是否反转前后方向
+  float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
 
   // 加速度限制处理
   float velocity_change =
@@ -407,26 +427,56 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
 
   /* 构建目标状态 */
   LQR_State_t target_state = {0};
-  target_state.theta = c->param->theta;                  // 目标摆杆角度
+  target_state.theta = 0.00;                  // 目标摆杆角度
   target_state.d_theta = 0.0f;                // 目标摆杆角速度
-  target_state.phi = 11.9601*pow((0.18f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.18f + c_cmd->height * 0.25f),2) + 3.87083*(0.18f + c_cmd->height * 0.25f) + -0.414154;                   // 目标俯仰角
+  target_state.phi = 11.9601*pow((0.16f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.16f + c_cmd->height * 0.25f),2) + 3.87083*(0.16f + c_cmd->height * 0.25f) + -0.4154;                   // 目标俯仰角
   target_state.d_phi = 0.0f;                  // 目标俯仰角速度
-  target_state.x = c->chassis_state.target_x -2.07411f*(0.18f + c_cmd->height * 0.25f) + 0.41182f;
+  target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f;
   target_state.d_x = target_dot_x;            // 目标速度
 
   if (c->mode != CHASSIS_MODE_ROTOR){
-  c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
-  c->yaw_control.target_yaw =
-      c->param->mech_zero_yaw + c_cmd->move_vec.vy * M_PI_2;
-  c->yaw_control.yaw_force =
-      PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
-               c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
-  }else{
     c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
-    c->yaw_control.target_yaw = c->yaw_control.current_yaw + 0.5f;
+    
+    // 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
+    float manual_offset = c_cmd->move_vec.vy * M_PI_2;
+    float base_target_1 = c->param->mech_zero_yaw + manual_offset;
+    float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
+    
+    // 使用CircleError计算到两个目标的角度误差
+    float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
+    float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
+    
+    // 选择误差绝对值更小的目标，并更新反转状态
+    if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
+      c->yaw_control.target_yaw = base_target_1;
+      c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
+    } else {
+      c->yaw_control.target_yaw = base_target_2;
+      c->yaw_control.is_reversed = true;  // 使用第二个零点，需要反转前后方向
+    }
+    
     c->yaw_control.yaw_force =
         PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
                  c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
+  }else{
+    // 小陀螺模式：使用速度环控制
+    c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
+    
+    // 渐增旋转速度，最大角速度约6.9 rad/s（约400度/秒）
+    c->wz_multi += 0.005f;
+    if (c->wz_multi > 1.2f)
+      c->wz_multi = 1.2f;
+    
+    // 目标角速度（rad/s），可根据需要调整最大速度
+    float target_yaw_velocity = c->wz_multi * 1.0f; // 最大约7.2 rad/s
+    
+    // 当前角速度反馈来自IMU陀螺仪
+    float current_yaw_velocity = c->feedback.imu.gyro.z;
+    
+    // 使用PID控制角速度，输出力矩
+    c->yaw_control.yaw_force =
+        PID_Calc(&c->pid.yaw, target_yaw_velocity,
+                 current_yaw_velocity, 0.0f, c->dt);
   }
 
   if (c->vmc_[0].leg.is_ground_contact) {
@@ -434,7 +484,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     LQR_SetTargetState(&c->lqr[0], &target_state);
     LQR_Control(&c->lqr[0]);
   } else {
-    target_state.theta = 0.18f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
+    target_state.theta = 0.16f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
     LQR_SetTargetState(&c->lqr[0], &target_state);
     c->lqr[0].control_output.T = 0.0f;
     c->lqr[0].control_output.Tp =
@@ -446,7 +496,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     LQR_SetTargetState(&c->lqr[1], &target_state);
     LQR_Control(&c->lqr[1]);
   }else {
-    target_state.theta = 0.18f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
+    target_state.theta = 0.16f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
     LQR_SetTargetState(&c->lqr[1], &target_state);
     c->lqr[1].control_output.T = 0.0f;
     c->lqr[1].control_output.Tp =
@@ -466,44 +516,55 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   float leg_d_length[2]; // 腿长变化率
 
   /* 横滚角PID补偿计算 */
-  // 使用PID控制器计算横滚角补偿力，目标横滚角为0
-  float roll_compensation_force =
+  // 使用PID控制器计算横滚角补偿长度，目标横滚角为0
+  float roll_compensation_length =
       PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
                c->feedback.imu.gyro.x, c->dt);
 
-  // 限制补偿力范围，防止过度补偿
-  // 如果任一腿离地，限制补偿力为20
+  // 限制补偿长度范围，防止过度补偿
+  // 如果任一腿离地，限制补偿长度
   if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
-    if (roll_compensation_force > 20.0f)
-      roll_compensation_force = 20.0f;
-    if (roll_compensation_force < -20.0f)
-      roll_compensation_force = -20.0f;
+    if (roll_compensation_length > 0.02f)
+      roll_compensation_length = 0.02f;
+    if (roll_compensation_length < -0.02f)
+      roll_compensation_length = -0.02f;
+  } else {
+    if (roll_compensation_length > 0.05f)
+      roll_compensation_length = 0.05f;
+    if (roll_compensation_length < -0.05f)
+      roll_compensation_length = -0.05f;
   }
 
-  // 目标腿长设定（移除横滚角补偿）
+  // 目标腿长设定（包含横滚角补偿）
   switch (c->state) {
   case 0: // 正常状态，根据高度指令调节腿长
-    target_L0[0] = 0.18f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
-    target_L0[1] = 0.18f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
+    target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
+    target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
     break;
   case 1: // 准备阶段，腿长收缩
-    target_L0[0] = 0.18f; // 左腿：收缩到基础腿长
-    target_L0[1] = 0.18f; // 右腿：收缩到基础腿长
+    target_L0[0] = 0.16f + roll_compensation_length; // 左腿：收缩到基础腿长 + 横滚补偿
+    target_L0[1] = 0.16f - roll_compensation_length; // 右腿：收缩到基础腿长 - 横滚补偿
     break;
   case 2: // 起跳阶段，腿长快速伸展
-    target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
-    target_L0[1] = 0.55f; // 右腿：伸展到最大腿长
+    target_L0[0] = 0.55f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
+    target_L0[1] = 0.55f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
     break;
   case 3: // 着地阶段，腿长缓冲
-    target_L0[0] = 0.1f; // 左腿：缓冲腿长
-    target_L0[1] = 0.1f; // 右腿：缓冲腿长
+    target_L0[0] = 0.1f; // 左腿：缓冲腿长（着地时不进行横滚补偿）
+    target_L0[1] = 0.1f; // 右腿：缓冲腿长（着地时不进行横滚补偿）
     break;
   default:
-    target_L0[0] = 0.18f + c_cmd->height * 0.22f;
-    target_L0[1] = 0.18f + c_cmd->height * 0.22f;
+    target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length;
+    target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length;
     break;
   }
 
+  // 腿长限幅：最短0.10，最长0.42m
+  if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
+  if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
+  if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
+  if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
+
   // 获取腿长变化率
   VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
   VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
@@ -526,11 +587,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
                                 c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
 
-    // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 -
-    // 横滚角补偿力（左腿减少支撑力）
+    // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
     virtual_force[0] =
         (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
-        pid_output + 55.0f + roll_compensation_force;
+        pid_output + 60.0f;
 
     // VMC逆解算（包含摆角补偿）
     if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
@@ -551,11 +611,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
                                 c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
 
-    // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 +
-    // 横滚角补偿力（右腿增加支撑力）
+    // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
     virtual_force[1] =
         (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
-        pid_output + 60.0f - roll_compensation_force;
+        pid_output + 60.0f;
 
     // VMC逆解算（包含摆角补偿）
     if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index 6e2cf3a..98f694e 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -147,6 +147,7 @@ typedef struct {
     float target_yaw;     /* 目标yaw角度 */
     float current_yaw;    /* 当前yaw角度 */
     float yaw_force;      /* yaw轴控制力矩 */
+    bool is_reversed;     /* 是否使用反转的零点（180度零点），影响前后方向 */
   } yaw_control;
 
   float wz_multi; /* 小陀螺模式旋转方向 */
diff --git a/User/module/config.c b/User/module/config.c
index 58c697a..b729bfc 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -179,23 +179,23 @@ Config_RobotParam_t robot_config = {
         },
 
         .roll={
-            .k=30.0f,
-            .p=20.0f,           /* 横滚角比例系数 */
-            .i=1.0f,           /* 横滚角积分系数 */
-            .d=0.5f,           /* 横滚角微分系数 */
-            .i_limit=20.0f,     /* 积分限幅 */
-            .out_limit=100.0f,  /* 输出限幅，腿长差值限制 */
+            .k=1.0f,
+            .p=1.0f,           /* 横滚角比例系数 */
+            .i=0.01f,           /* 横滚角积分系数 */
+            .d=0.01f,           /* 横滚角微分系数 */
+            .i_limit=0.2f,     /* 积分限幅 */
+            .out_limit=1.0f,  /* 输出限幅，腿长差值限制 */
             .d_cutoff_freq=30.0f, /* 微分截止频率 */
             .range=-1.0f,      /* 不使用循环误差处理 */
         },
 
         .leg_length={
-            .k = 50.0f,
-            .p = 10.0f,
-            .i = 0.0f,
-            .d = 1.0f,
+            .k = 25.0f,
+            .p = 20.0f,
+            .i = 0.01f,
+            .d = 2.0f,
             .i_limit = 0.0f,
-            .out_limit = 100.0f,
+            .out_limit = 200.0f,
             .d_cutoff_freq = -1.0f,
             .range = -1.0f,
         },
@@ -255,18 +255,18 @@ Config_RobotParam_t robot_config = {
         },
         
 .lqr_gains = {
-    .k11_coeff = { -1.365991212772591e+02f, 1.817682520209751e+02f, -1.090145744768390e+02f, -1.357841047348700e+00f }, // theta
-    .k12_coeff = { 1.066191238841660e+01f, -9.003325414315036e+00f, -6.468059178062407e+00f, -3.576912197025609e-02f },  // d_theta
-    .k13_coeff = { -4.401223711894286e+01f, 5.240645764730768e+01f, -2.259350423458009e+01f, -3.125930762749351e-01f },  // x
-    .k14_coeff = { -3.589096446318186e+01f, 4.504832892816042e+01f, -2.180583719935267e+01f, -6.131521769198509e-01f },  // d_x
-    .k15_coeff = { 7.755509171684017e+00f, 9.703025198473474e+00f, -1.617988750531686e+01f, 6.969677381136859e+00f }, // phi
-    .k16_coeff = { -1.054369868423547e+00f, 3.078146872733054e+00f, -2.719931242949781e+00f, 1.137139318462496e+00f },  // d_phi
-    .k21_coeff = { 4.187447897772694e+02f, -3.681828885583142e+02f, 8.516335489092428e+01f, 1.307642410705911e+01f }, // theta
-    .k22_coeff = { 2.733059508651422e+01f, -2.951697374399402e+01f, 1.083275594166739e+01f, 1.186192378498821e+00f },  // d_theta
-    .k23_coeff = { 2.607118229545885e+01f, 5.493744611623077e+00f, -2.426883873185716e+01f, 1.096947115837726e+01f },  // x
-    .k24_coeff = { 1.079657629365681e+01f, 1.807070716568389e+01f, -2.737138856658193e+01f, 1.153864525902007e+01f },  // d_x
-    .k25_coeff = { 1.861482062842389e+02f, -2.152844878785277e+02f, 9.051129761064519e+01f, -1.063399918986778e+00f }, // phi
-    .k26_coeff = { 2.777098508680760e+01f, -3.223625515767142e+01f, 1.376100864562188e+01f, -6.731370513814361e-01f },  // d_phi
+    .k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
+    .k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f },  // d_theta
+    .k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f },  // x
+    .k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f },  // d_x
+    .k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
+    .k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f },  // d_phi
+    .k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
+    .k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f },  // d_theta
+    .k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f },  // x
+    .k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f },  // d_x
+    .k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
+    .k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f },  // d_phi
 },
 
         .theta = 0.0f,
diff --git a/User/task/rc.c b/User/task/rc.c
index e362885..2f32b0b 100644
--- a/User/task/rc.c
+++ b/User/task/rc.c
@@ -61,11 +61,12 @@ void Task_rc(void *argument) {
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
       break;
     case DR16_SW_MID:
-      cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
-      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
+      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
       break;
     case DR16_SW_DOWN:
-      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
+      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
       // cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
       break;
     default:
diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m
index 692d42b..144034d 100644
--- a/utils/Simulation-master/balance/series_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
     R1=0.072;                         %驱动轮半径
     L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
     LM1=leg_length/2;                 %摆杆重心到其转轴距离
-    l1=0.01;                          %机体质心距离转轴距离
+    l1=-0.01;                          %机体质心距离转轴距离
     mw1=0.60;                         %驱动轮质量
     mp1=1.8;                         %杆质量
-    M1=12.0;                          %机体质量
+    M1=14.0;                          %机体质量
     Iw1=mw1*R1^2;                     %驱动轮转动惯量
     Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
     IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
@@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
     B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
     B=double(B);
     
-    Q=diag([1500 100 5000 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[300 0;0 50];                %T Tp
+    Q=diag([1000 1 8000 100 20000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[280 0;0 30];                %T Tp
     
     K=lqr(A,B,Q,R);