diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 3bffb46..f353ee5 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -1,527 +1,445 @@
+#include "device/motor_lz.h"
 #define _USE_MATH_DEFINES
-#include "module/balance_chassis.h"
-#include "bsp/time.h"
 #include "bsp/can.h"
-#include "component/user_math.h"
+#include "bsp/time.h"
 #include "component/kinematics.h"
 #include "component/limiter.h"
+#include "component/user_math.h"
+#include "module/balance_chassis.h"
 #include <math.h>
-#include <string.h>
 #include <stddef.h>
+#include <string.h>
 
 /**
  * @brief 使能所有电机
  * @param c 底盘结构体指针
- * @return 
+ * @return
  */
 static int8_t Chassis_MotorEnable(Chassis_t *c) {
-    if (c == NULL) return CHASSIS_ERR_NULL;
-    
-    for (int i = 0; i < 4; i++) {
-        MOTOR_LZ_Enable(&c->param->joint_motors[i]);
-    }
-
-    BSP_TIME_Delay_us(200);
-
-    for (int i = 0; i < 2; i++) {
-        MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
-    }
-    
-    return CHASSIS_OK;
+  if (c == NULL)
+    return CHASSIS_ERR_NULL;
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_Enable(&c->param->joint_motors[i]);
+  }
+  BSP_TIME_Delay_us(200);
+  for (int i = 0; i < 2; i++) {
+    MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
+  }
+  return CHASSIS_OK;
 }
 
-
-// /**
-//  * @brief 关闭所有电机
-//  * @param c 底盘结构体指针
-//  * @return 
-//  */
-// static int8_t Chassis_MotorDisable(Chassis_t *c){
-//     if (c == NULL) return CHASSIS_ERR_NULL;
-    
-//     for (int i = 0; i < 2; i++) {
-//         MOTOR_LK_MotorOff(&c->param->wheel_motors[i]);
-//     }
-//     for (int i = 0; i < 4; i++) {
-//         MOTOR_LZ_Disable(&c->param->joint_motors[i], true);
-//     }
-    
-//     return CHASSIS_OK;
-// }
+static int8_t Chassis_MotorRelax(Chassis_t *c) {
+  if (c == NULL)
+    return CHASSIS_ERR_NULL;
+  for (int i = 0; i < 2; i++) {
+    MOTOR_LK_Relax(&c->param->wheel_motors[i]);
+  }
+  BSP_TIME_Delay_us(200);
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_Relax(&c->param->joint_motors[i]);
+  }
+  return CHASSIS_OK;
+}
 
 static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
-  if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
-  if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
+  if (c == NULL)
+    return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
+  if (mode == c->mode)
+    return CHASSIS_OK; /* 模式未改变直接返回 */
 
+  Chassis_MotorEnable(c);
 
-    Chassis_MotorEnable(c);
+  PID_Reset(&c->pid.leg_length[0]);
+  PID_Reset(&c->pid.leg_length[1]);
+  PID_Reset(&c->pid.yaw);
+  PID_Reset(&c->pid.roll);
+  PID_Reset(&c->pid.tp[0]);
+  PID_Reset(&c->pid.tp[1]);
 
-    PID_Reset(&c->pid.leg_length[0]);
-    PID_Reset(&c->pid.leg_length[1]);
-    PID_Reset(&c->pid.yaw);
-    PID_Reset(&c->pid.roll);
-    PID_Reset(&c->pid.tp[0]);
-    PID_Reset(&c->pid.tp[1]);
-
-    c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
-
-    //清空位移
-    c->chassis_state.position_x = 0.0f;
-    c->chassis_state.velocity_x = 0.0f;
-    c->chassis_state.last_velocity_x = 0.0f;
-    c->chassis_state.target_x = 0.0f;
+  c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
 
+  // 清空位移
+  c->chassis_state.position_x = 0.0f;
+  c->chassis_state.velocity_x = 0.0f;
+  c->chassis_state.last_velocity_x = 0.0f;
+  c->chassis_state.target_x = 0.0f;
 
   LQR_Reset(&c->lqr[0]);
   LQR_Reset(&c->lqr[1]);
   c->mode = mode;
-  c->state = 0;  // 重置状态，确保每次切换模式时都重新初始化
+  c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
 
   return CHASSIS_OK;
 }
 
 /* 更新机体状态估计 */
 static void Chassis_UpdateChassisState(Chassis_t *c) {
-    if (c == NULL) return;
-    
-    // 从轮子编码器估计机体速度 (参考C++代码)
-    float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
-    float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒)
-    
-    // 将dps转换为rad/s
-    float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s
-    float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
-    
-    float wheel_radius = 0.072f;
-    
-    float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
-    float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
-    
-    // 机体x方向速度 (轮子中心速度)
-    c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
-    c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
-    
-    // 积分得到位置
-    c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
+  if (c == NULL)
+    return;
+
+  // 从轮子编码器估计机体速度 (参考C++代码)
+  float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
+  float right_wheel_speed_dps =
+      c->feedback.wheel[1].rotor_speed; // dps (度每秒)
+
+  // 将dps转换为rad/s
+  float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f;   // rad/s
+  float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
+
+  float wheel_radius = 0.072f;
+
+  float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
+  float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
+
+  // 机体x方向速度 (轮子中心速度)
+  c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
+  c->chassis_state.velocity_x =
+      (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
+
+  // 积分得到位置
+  c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
 }
 
+int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
+  if (c == NULL || param == NULL || target_freq <= 0.0f) {
+    return -1; // 参数错误
+  }
+  c->param = param;
 
-int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
-    if (c == NULL || param == NULL || target_freq <= 0.0f) {
-        return -1; // 参数错误
+  /*初始化can*/
+  BSP_CAN_Init();
+
+  /*初始化和注册所有电机*/
+  MOTOR_LZ_Init();
+
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_Register(&c->param->joint_motors[i]);
+  }
+  for (int i = 0; i < 2; i++) {
+    MOTOR_LK_Register(&c->param->wheel_motors[i]);
+  }
+  MOTOR_DM_Register(&c->param->yaw_motor);
+
+  /*初始化VMC控制器*/
+  VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
+  VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
+
+  /*初始化pid*/
+  PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq,
+           &param->leg_length);
+  PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq,
+           &param->leg_length);
+  PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
+  PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
+  PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
+  PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
+
+  /*初始化LQR控制器*/
+  LQR_Init(&c->lqr[0], &param->lqr_gains);
+  LQR_Init(&c->lqr[1], &param->lqr_gains);
+
+  /*初始化机体状态*/
+  c->chassis_state.position_x = 0.0f;
+  c->chassis_state.velocity_x = 0.0f;
+  c->chassis_state.last_velocity_x = 0.0f;
+  c->chassis_state.target_x = 0.0f;
+
+  /*初始化yaw控制状态*/
+  c->yaw_control.target_yaw = 0.0f;
+  c->yaw_control.current_yaw = 0.0f;
+  c->yaw_control.yaw_force = 0.0f;
+
+  return CHASSIS_OK;
+}
+
+int8_t Chassis_UpdateFeedback(Chassis_t *c) {
+  if (c == NULL) {
+    return -1; // 参数错误
+  }
+  /*更新电机反馈*/
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_Update(&c->param->joint_motors[i]);
+  }
+  MOTOR_LK_Update(&c->param->wheel_motors[0]);
+  MOTOR_LK_Update(&c->param->wheel_motors[1]);
+
+  /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
+  // 更新关节电机反馈并检查离线状态
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
+    if (joint_motor != NULL) {
+      if (joint_motor->motor.feedback.rotor_abs_angle > M_PI) {
+        joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
+      }
+      joint_motor->motor.feedback.rotor_abs_angle =
+          -joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整
+
+      c->feedback.joint[i] = joint_motor->motor.feedback;
     }
-    c->param = param;
-    
-    /*初始化can*/
-    BSP_CAN_Init();
+  }
 
-    /*初始化和注册所有电机*/
-    MOTOR_LZ_Init();
-
-    for (int i = 0; i < 4; i++) {
-      MOTOR_LZ_Register(&c->param->joint_motors[i]);
+  // 更新轮子电机反馈并检查离线状态
+  for (int i = 0; i < 2; i++) {
+    MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
+    if (wheel_motor != NULL) {
+      c->feedback.wheel[i] = wheel_motor->motor.feedback;
     }
-    for (int i = 0; i < 2; i++) {
-        MOTOR_LK_Register(&c->param->wheel_motors[i]);
-    }
-    MOTOR_DM_Register(&c->param->yaw_motor);
+  }
 
+  /* 更新云台电机反馈数据（yaw轴） */
+  MOTOR_DM_Update(&(c->param->yaw_motor));
+  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
+  if (dm_motor != NULL) {
+    c->feedback.yaw = dm_motor->motor.feedback;
+  }
+
+  // 更新机体状态估计
+  Chassis_UpdateChassisState(c);
+
+  return 0;
+}
+
+int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
+  if (c == NULL) {
+    return -1; // 参数错误
+  }
+  c->feedback.imu = imu;
+  return 0;
+}
+
+int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+  if (c == NULL || c_cmd == NULL) {
+    return CHASSIS_ERR_NULL; // 参数错误
+  }
+  c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) /
+          1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
+  c->lask_wakeup = BSP_TIME_Get_us();
+
+  /*设置底盘模式*/
+  if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
+    return CHASSIS_ERR_MODE; // 设置模式失败
+  }
+
+  VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
+                   c->feedback.joint[1].rotor_abs_angle,
+                   c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+  VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle,
+                   c->feedback.joint[2].rotor_abs_angle,
+                   c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+
+  /*根据底盘模式执行不同的控制逻辑*/
+  switch (c->mode) {
+  case CHASSIS_MODE_RELAX:
+    // 放松模式，电机不输出
+    Chassis_MotorRelax(c);
+
+    Chassis_LQRControl(c, c_cmd);
+
+    break;
+
+  case CHASSIS_MODE_RECOVER: {
+    float fn;
+    fn = -25.0f;
+
+    VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
+    VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
+                        &c->output.joint[1].torque);
+    VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
+    VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
+                        &c->output.joint[2].torque);
     // Chassis_MotorEnable(c);
-    
-    /*初始化VMC控制器*/
-    VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
-    VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
+    c->output.wheel[0] = 0.0f;
+    c->output.wheel[1] = 0.0f;
+    Chassis_Output(c); // 统一输出
 
-    /*初始化pid*/
-    PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
-    PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
-    PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
-    PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
-    PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
-    PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
+  } break;
 
-    /*初始化LQR控制器*/
-    LQR_Init(&c->lqr[0], &param->lqr_gains);
-    LQR_Init(&c->lqr[1], &param->lqr_gains);
-    
-    /*初始化机体状态*/
-    c->chassis_state.position_x = 0.0f;
-    c->chassis_state.velocity_x = 0.0f;
-    c->chassis_state.last_velocity_x = 0.0f;
-    c->chassis_state.target_x = 0.0f;
-    
-    /*初始化yaw控制状态*/
-    c->yaw_control.target_yaw = 0.0f;
-    c->yaw_control.current_yaw = 0.0f;
-    c->yaw_control.yaw_force = 0.0f;
 
-    return CHASSIS_OK;
-}
+  case CHASSIS_MODE_WHELL_LEG_BALANCE:
+    // 执行LQR控制（包含PID腿长控制）
+    Chassis_LQRControl(c, c_cmd);
 
-int8_t Chassis_UpdateFeedback(Chassis_t *c){
-    if (c == NULL) {
-        return -1; // 参数错误
-    }
-    /*更新电机反馈*/
-    for (int i = 0; i < 4; i++) {
-        MOTOR_LZ_Update(&c->param->joint_motors[i]);
-    }
-    MOTOR_LK_Update(&c->param->wheel_motors[0]);
-    MOTOR_LK_Update(&c->param->wheel_motors[1]);
-    
-    /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
-    // 更新关节电机反馈并检查离线状态
-    for (int i = 0; i < 4; i++) {
-        MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
-        if (joint_motor != NULL) {
-            if (joint_motor->motor.feedback.rotor_abs_angle > M_PI ){
-                joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
-            } 
-            joint_motor->motor.feedback.rotor_abs_angle = - joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整
+    Chassis_Output(c); // 统一输出
+    break;
 
-            c->feedback.joint[i] = joint_motor->motor.feedback;
-        } 
-    }
-    
-    // 更新轮子电机反馈并检查离线状态
-    for (int i = 0; i < 2; i++) {
-        MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
-        if (wheel_motor != NULL) {
-            c->feedback.wheel[i] = wheel_motor->motor.feedback;
-        }
-    }
+  default:
+    return CHASSIS_ERR_MODE;
+  }
 
-    /* 更新云台电机反馈数据（yaw轴） */
-    MOTOR_DM_Update(&(c->param->yaw_motor));
-    MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
-    if (dm_motor != NULL) {
-        c->feedback.yaw = dm_motor->motor.feedback;
-    }
-
-    // 更新机体状态估计
-    Chassis_UpdateChassisState(c);
-    
-    return 0;
-}
-
-int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){
-    if (c == NULL) {
-        return -1; // 参数错误
-    }
-    c->feedback.imu = imu;
-    // c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit;
-    return 0;
-}
-
-int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
-    if (c == NULL || c_cmd == NULL) {
-        return CHASSIS_ERR_NULL; // 参数错误
-    }
-    c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
-    c->lask_wakeup = BSP_TIME_Get_us();
-
-    /*设置底盘模式*/
-    if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
-        return CHASSIS_ERR_MODE; // 设置模式失败
-    }
-
-    /*根据底盘模式执行不同的控制逻辑*/
-    switch (c->mode) {
-        case CHASSIS_MODE_RELAX:
-            // 放松模式，电机不输出
-
-            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
-            MOTOR_LZ_Relax(&c->param->joint_motors[0]);
-            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
-            MOTOR_LZ_Relax(&c->param->joint_motors[1]);
-            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
-            MOTOR_LZ_Relax(&c->param->joint_motors[2]);
-            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
-            MOTOR_LZ_Relax(&c->param->joint_motors[3]);
-            BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成
-            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
-            MOTOR_LK_Relax(&c->param->wheel_motors[0]);
-            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
-            MOTOR_LK_Relax(&c->param->wheel_motors[1]);
-
-            // 更新VMC正解算用于状态估计
-            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-
-            Chassis_LQRControl(c, c_cmd);
-
-            break;
-
-        case CHASSIS_MODE_RECOVER:
-        {
-            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            
-            float fn;
-            fn = -25.0f;
-
-            VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
-            VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
-            VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
-            VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
-            // Chassis_MotorEnable(c);
-            c->output.wheel[0] = 0.0f;
-            c->output.wheel[1] = 0.0f;
-            Chassis_Output(c); // 统一输出
-        }
-        break;
-            
-        case CHASSIS_MODE_WHELL_BALANCE:
-            // 更新VMC正解算用于状态估计
-            for (int i = 0; i < 4; i++) {
-                c->output.joint[i].torque = 0.0f;
-            }
-            for (int i = 0; i < 2; i++) {
-                c->output.wheel[i] = 0.0f;
-            }
-
-            // 更新VMC正解算用于状态估计
-            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-
-            // VMC_InverseSolve(&c->vmc_[1], fn, tp);
-            // VMC_GetJointTorques(&c->vmc_[1], &t1, &t2);
-            
-            // c->output.joint[3].torque = t1;
-            // c->output.joint[2].torque = t2;
-
-            Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
-            // c->output.wheel[0] = 0.2f;
-            // c->output.wheel[1] = 0.2f;
-            Chassis_Output(c); // 统一输出
-            break;
-            
-        case CHASSIS_MODE_WHELL_LEG_BALANCE:
-            // 轮腿平衡模式，使用LQR控制和PID腿长控制
-            
-            // 更新VMC正解算
-            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
-                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            
-            // 执行LQR控制（包含PID腿长控制）
-            if (Chassis_LQRControl(c, c_cmd) != 0) {
-                // LQR控制失败，切换到安全模式
-                return CHASSIS_ERR;
-            }
-            
-            Chassis_Output(c); // 统一输出
-            break;
-            
-        default:
-            return CHASSIS_ERR_MODE;
-    }
-    
-    return CHASSIS_OK;
+  return CHASSIS_OK;
 }
 
 void Chassis_Output(Chassis_t *c) {
-    if (c == NULL) return;
-    for (int i = 0; i < 4; i++) {
-        MOTOR_LZ_MotionParam_t param = {0};
-        param.torque = c->output.joint[i].torque;
-        MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
-    }
-    BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
-    for (int i = 0; i < 2; i++) {
-        MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
-        // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
-    }
-    
+  if (c == NULL)
+    return;
+  for (int i = 0; i < 4; i++) {
+    MOTOR_LZ_MotionParam_t param = {0};
+    param.torque = c->output.joint[i].torque;
+    MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
+  }
+  BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
+  for (int i = 0; i < 2; i++) {
+    MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
+    // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
+  }
 }
 
 int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
-    if (c == NULL || c_cmd == NULL) {
-        return CHASSIS_ERR_NULL;
-    }
+  if (c == NULL || c_cmd == NULL) {
+    return CHASSIS_ERR_NULL;
+  }
 
-    /* 参考C++版本实现的LQR控制 */
-    
-    /* 地面接触检测（参考C++版本） */
-    float leg_fn[2];
-    bool onground_flag[2];
-    
-    // 暂时关闭离地监测，强制设置为着地状态
-    leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]);
-    leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]);
-    onground_flag[0] = true;  // 强制设置左腿着地
-    onground_flag[1] = true;  // 强制设置右腿着地
-    
-    /* 获取VMC状态（等效摆杆参数） */
-    float leg_L0[2], leg_theta[2], leg_d_theta[2];
-    VMC_GetVirtualLegState(&c->vmc_[0], &leg_L0[0], &leg_theta[0], NULL, &leg_d_theta[0]);
-    VMC_GetVirtualLegState(&c->vmc_[1], &leg_L0[1], &leg_theta[1], NULL, &leg_d_theta[1]);
-    
-    /* 运动参数（参考C++版本的状态估计） */
-    static float xhat = 0.0f, x_dot_hat = 0.0f;
-    float target_dot_x = 0.0f;
-    
-    // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
-    x_dot_hat = c->chassis_state.velocity_x;
-    xhat = c->chassis_state.position_x;
-    
-    // 目标设定
-    target_dot_x = c_cmd->move_vec.vx*2;
-    // target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
-    c->chassis_state.target_x += target_dot_x * c->dt;
-    
-    /* 分别计算左右腿的LQR控制 */
-    float Tw[2] = {0.0f, 0.0f};  // 轮毂力矩
-    float Tp[2] = {0.0f, 0.0f};  // 髋关节力矩
-    
-    for (int leg = 0; leg < 2; leg++) {
-        /* 构建当前状态 */
-        LQR_State_t current_state = {0};
-        current_state.theta = leg_theta[leg];
-        current_state.d_theta = leg_d_theta[leg];
-        current_state.x = xhat;
-        current_state.d_x = x_dot_hat;
-        current_state.phi = -c->feedback.imu.euler.pit;
-        current_state.d_phi = -c->feedback.imu.gyro.y;
-        
-        /* 构建目标状态 */
-        LQR_State_t target_state = {0};
-        target_state.theta = 0.0f; // 目标摆杆角度
-        target_state.d_theta = 0.0f; // 目标摆杆角速度
-        // target_state.x = 0; // 目标位置
-        // target_state.d_x = 0.0f; // 目标速度
-        target_state.phi = -0.1f; // 目标俯仰角
-        target_state.d_phi = 0.0f; // 目标俯仰角速度
-        // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度
-        // target_state.d_theta = 0.0f; // 目标摆杆角速度
-        target_state.x = c->chassis_state.target_x; // 目标位置
-        target_state.d_x = target_dot_x; // 目标速度
-        // target_state.phi = 0.16f; // 目标俯仰角
-        // target_state.d_phi = 0.0f; // 目标俯仰角速度
-        
-        /* 根据当前腿长更新增益矩阵 */
-        LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]);
-        
-        /* 更新LQR状态 */
-        LQR_SetTargetState(&c->lqr[leg], &target_state);
-        LQR_UpdateState(&c->lqr[leg], &current_state);
-        
-        if (onground_flag[leg]) {
-            /* 接地状态：使用LQR控制器计算输出 */
-            if (LQR_Control(&c->lqr[leg]) == 0) {
-                Tw[leg] = c->lqr[leg].control_output.T;
-                Tp[leg] = c->lqr[leg].control_output.Tp;
-                // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出
-                // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出
-            } else {
-                Tw[leg] = 0.0f;
-                Tp[leg] = 0.0f;
-            }
-        } else {
-            /* 离地状态：简化控制，只控制腿部摆动 */
-            Tw[leg] = 0.0f;
-            // 只控制摆杆角度
-            float theta_error = current_state.theta - target_state.theta;
-            float d_theta_error = current_state.d_theta - target_state.d_theta;
-            Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error);
-        }
-    }
 
-    // c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
+  /* 运动参数（参考C++版本的状态估计） */
+  static float xhat = 0.0f, x_dot_hat = 0.0f;
+  float target_dot_x = 0.0f;
 
-    // c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s
+  // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
+  x_dot_hat = c->chassis_state.velocity_x;
+  xhat = c->chassis_state.position_x;
 
-    // // 修正目标yaw角度到[-pi, pi]
-    // if (c->yaw_control.target_yaw > M_PI) {
-    //     c->yaw_control.target_yaw -= M_2PI;
-    // } else if (c->yaw_control.target_yaw < -M_PI) {
-    //     c->yaw_control.target_yaw += M_2PI;
-    // }
+  // 目标设定
+  target_dot_x = c_cmd->move_vec.vx * 2;
+  // target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x,
+  // target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
+  c->chassis_state.target_x += target_dot_x * c->dt;
 
-    // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt);
-    c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
-    c->yaw_control.target_yaw = c->param->mech_zero_yaw;
-    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);
+  /* 分别计算左右腿的LQR控制 */
+    /* 构建当前状态 */
+    LQR_State_t current_state = {0};
+    current_state.theta = c->vmc_[0].leg.theta;
+    current_state.d_theta = c->vmc_[0].leg.d_theta;
+    current_state.x = xhat;
+    current_state.d_x = x_dot_hat;
+    current_state.phi = -c->feedback.imu.euler.pit;
+    current_state.d_phi = -c->feedback.imu.gyro.y;
+
+    LQR_UpdateState(&c->lqr[0], &current_state);
+
+    current_state.theta = c->vmc_[1].leg.theta;
+    current_state.d_theta = c->vmc_[1].leg.d_theta;
+    LQR_UpdateState(&c->lqr[1], &current_state);
+
+    /* 根据当前腿长更新增益矩阵 */
+    LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
+    LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
+
+    /* 构建目标状态 */
+    LQR_State_t target_state = {0};
+    target_state.theta = 0.0f;   // 目标摆杆角度
+    target_state.d_theta = 0.0f; // 目标摆杆角速度
+    target_state.phi = -0.1f;  // 目标俯仰角
+    target_state.d_phi = 0.0f; // 目标俯仰角速度
+    target_state.x = c->chassis_state.target_x; // 目标位置
+    target_state.d_x = target_dot_x;            // 目标速度
     
-    /* 轮毂力矩输出（参考C++版本的减速比） */
-    c->output.wheel[0] = Tw[0] / 4.5f + c->yaw_control.yaw_force;
-    c->output.wheel[1] = Tw[1] / 4.5f - c->yaw_control.yaw_force;
-    /* 腿长控制和VMC逆解算（使用PID控制） */
-    float virtual_force[2];
-    float target_L0[2];
-    float leg_d_length[2]; // 腿长变化率
-    
-    /* 横滚角PID补偿计算 */
-    float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt);
-    
-    // 目标腿长设定
-    target_L0[0] = 0.15f + c_cmd->height * 0.2f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
-    target_L0[1] = 0.15f + c_cmd->height * 0.2f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
-    
-    // 获取腿长变化率
-    VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
-    VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
-    
-    /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
-    float Delta_Tp[2];
-    // 使用tp_pid进行左右腿摆角同步控制
-    // 左腿补偿力矩：使左腿theta向右腿theta靠拢
-    Delta_Tp[0] = leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
-    // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
-    Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
-    
-    for (int leg = 0; leg < 2; leg++) {
-        // 使用PID进行腿长控制
-        float pid_output = PID_Calc(&c->pid.leg_length[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
-        
-        // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
-        virtual_force[leg] = (Tp[leg]) * sinf(leg_theta[leg]) + 
-                            pid_output + 30.0f;
-                            //  + // PID腿长控制输出
-                            // 45.0f; // 基础支撑力
-        
-        // VMC逆解算（包含摆角补偿）
-        // virtual_force[leg] = 0.0f; // 暂时屏蔽虚拟力输出，避免VMC逆解算失败
-        // Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
-        // Delta_Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
-        if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg] + Delta_Tp[leg]) == 0) {
-        // if (VMC_InverseSolve(&c->vmc_[leg], 0.0, Tp[leg] + Delta_Tp[leg]) == 0) {
-            if (leg == 0) {
-                VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
-            } else {
-                VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
-            }
-        } else {
-            // VMC失败，设为0力矩
-            if (leg == 0) {
-                c->output.joint[0].torque = 0.0f;
-                c->output.joint[1].torque = 0.0f;
-            } else {
-                c->output.joint[2].torque = 0.0f;
-                c->output.joint[3].torque = 0.0f;
-            }
-        }
+    /* 更新LQR状态 */
+    LQR_SetTargetState(&c->lqr[0], &target_state);
+    LQR_SetTargetState(&c->lqr[1], &target_state);
+     
+    LQR_Control(&c->lqr[0]);
+    LQR_Control(&c->lqr[1]);
+
+
+
+  c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
+  c->yaw_control.target_yaw = c->param->mech_zero_yaw;
+  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);
+
+  /* 轮毂力矩输出（参考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;
+  /* 腿长控制和VMC逆解算（使用PID控制） */
+  float virtual_force[2];
+  float target_L0[2];
+  float leg_d_length[2]; // 腿长变化率
+
+  /* 横滚角PID补偿计算 */
+  // 使用PID控制器计算横滚角补偿力，目标横滚角为0
+  float roll_compensation_force = PID_Calc(&c->pid.roll, 0.0f, 
+                                           c->feedback.imu.euler.rol, 
+                                           c->feedback.imu.gyro.x, 
+                                           c->dt);
+  
+  // 限制补偿力范围，防止过度补偿
+  if (roll_compensation_force > 20.0f) roll_compensation_force = 20.0f;
+  if (roll_compensation_force < -20.0f) roll_compensation_force = -20.0f;
+  
+  // 目标腿长设定（移除横滚角补偿）
+  target_L0[0] = 0.15f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
+  target_L0[1] = 0.15f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节
+
+  // 获取腿长变化率
+  VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
+  VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
+
+  /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
+  float Delta_Tp[2];
+  // 使用tp_pid进行左右腿摆角同步控制
+  // 左腿补偿力矩：使左腿theta向右腿theta靠拢
+  Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
+                PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta, c->vmc_[0].leg.theta,
+                         c->vmc_[0].leg.d_theta, c->dt);
+  // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
+  Delta_Tp[1] = -Delta_Tp[0];
+
+  
+  // 左腿控制
+  {
+    // 使用PID进行腿长控制
+    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腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力）
+    virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 50.0f - roll_compensation_force;
+
+    // VMC逆解算（包含摆角补偿）
+    if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
+                         c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
+      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
+                          &c->output.joint[1].torque);
+    } else {
+      // VMC失败，设为0力矩
+      c->output.joint[0].torque = 0.0f;
+      c->output.joint[1].torque = 0.0f;
     }
-    
-    /* 安全限制 */
-    for (int i = 0; i < 2; i++) {
-        if (fabsf(c->output.wheel[i]) > 0.8f) {
-            c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f;
-        }
+  }
+
+  // 右腿控制
+  {
+    // 使用PID进行腿长控制
+    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腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力）
+    virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 50.0f + roll_compensation_force;
+
+    // VMC逆解算（包含摆角补偿）
+    if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
+                         c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
+      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
+                          &c->output.joint[2].torque);
+    } else {
+      // VMC失败，设为0力矩
+      c->output.joint[2].torque = 0.0f;
+      c->output.joint[3].torque = 0.0f;
     }
-    
-    for (int i = 0; i < 4; i++) {
-        if (fabsf(c->output.joint[i].torque) > 20.0f) {
-            c->output.joint[i].torque = (c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
-        }
+  }
+
+  /* 安全限制 */
+  for (int i = 0; i < 2; i++) {
+    if (fabsf(c->output.wheel[i]) > 0.8f) {
+      c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f;
     }
-    
-    return CHASSIS_OK;
+  }
+
+  for (int i = 0; i < 4; i++) {
+    if (fabsf(c->output.joint[i].torque) > 20.0f) {
+      c->output.joint[i].torque =
+          (c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
+    }
+  }
+
+  return CHASSIS_OK;
 }
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index 64669fa..a2313be 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -38,7 +38,6 @@ extern "C" {
 typedef enum {
   CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
   CHASSIS_MODE_RECOVER,                 /* 复位模式 */
-  CHASSIS_MODE_WHELL_BALANCE,          /* 平衡模式，底盘自我平衡 */
   CHASSIS_MODE_WHELL_LEG_BALANCE,      /* 轮子+腿平衡模式，底盘自我平衡 */
 } Chassis_Mode_t;
 
diff --git a/User/module/config.c b/User/module/config.c
index 46b8685..d330e91 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -38,21 +38,21 @@ Config_RobotParam_t robot_config = {
         },
 
         .roll={
-            .k=1.0f,
+            .k=10.0f,
             .p=5.0f,           /* 横滚角比例系数 */
             .i=0.0f,           /* 横滚角积分系数 */
             .d=0.2f,           /* 横滚角微分系数 */
             .i_limit=0.0f,     /* 积分限幅 */
-            .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */
+            .out_limit=50.0f,  /* 输出限幅，腿长差值限制 */
             .d_cutoff_freq=30.0f, /* 微分截止频率 */
             .range=-1.0f,      /* 不使用循环误差处理 */
         },
 
         .leg_length={
-            .k = 20.0f,
-            .p = 10.0f,
+            .k = 40.0f,
+            .p = 5.0f,
             .i = 0.0f,
-            .d = 0.0f,
+            .d = 1.0f,
             .i_limit = 0.0f,
             .out_limit = 100.0f,
             .d_cutoff_freq = -1.0f,
@@ -160,18 +160,18 @@ Config_RobotParam_t robot_config = {
             }
         },
 .lqr_gains = {
-    .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
+    .k11_coeff = { -1.916376919295156e+02f, 2.009487240966917e+02f, -9.481460086781939e+01f, -4.748704486775178e+00f }, // theta
+    .k12_coeff = { -1.794538872095484e+00f, -1.557720009681701e-02f, -7.253705624763870e+00f, -9.066344876912042e-01f },  // d_theta
+    .k13_coeff = { -5.530128764310525e+01f, 5.441066051951745e+01f, -1.855725101721958e+01f, -1.532320658646968e+00f },  // x
+    .k14_coeff = { -5.226956984912729e+01f, 5.134367619659140e+01f, -1.878250555345751e+01f, -2.194040977657715e+00f },  // d_x
+    .k15_coeff = { -8.041587662748894e+00f, 1.194985309170939e+01f, -6.358536570979702e+00f, 8.475914154881636e-01f }, // phi
+    .k16_coeff = { -1.171602430557222e+01f, 1.424681188601595e+01f, -6.769563511869035e+00f, 1.358696951640962e+00f },  // d_phi
+    .k21_coeff = { -1.589916277171124e+01f, 3.688717311486668e+01f, -2.693106123880470e+01f, 8.468880380216705e+00f }, // theta
+    .k22_coeff = { -4.056650578339882e+00f, 5.627051302682392e+00f, -2.802037254431724e+00f, 1.258712322539219e+00f },  // d_theta
+    .k23_coeff = { -4.615940205649521e+01f, 5.298830026615487e+01f, -2.321905486010759e+01f, 4.733000143959848e+00f },  // x
+    .k24_coeff = { -6.198443605956307e+01f, 6.867018097135634e+01f, -2.873375258895661e+01f, 5.632056979434964e+00f },  // d_x
+    .k25_coeff = { -1.149934231218892e+01f, 9.960303123832579e+00f, -2.826210920406189e+00f, 4.384038656352771e+00f }, // phi
+    .k26_coeff = { 1.452344963148120e+01f, -1.451377402367933e+01f, 5.020766559053281e+00f, 1.099371055071753e+00f },  // d_phi
 },
     },
 
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 6992301..355cb13 100644
--- a/utils/Simulation-master/balance/series_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@@ -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([700 100 2000 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[140 0;0 50];                %T Tp
+    Q=diag([500 200 1500 800 2500 200]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[100 0;0 80];                %T Tp
     
     K=lqr(A,B,Q,R);