diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 08c2051..a087331 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -8,8 +8,8 @@
 #include <string.h>
 
 /**
- * @brief 
- * @param c 
+ * @brief 使能所有电机
+ * @param c 底盘结构体指针
  * @return 
  */
 static int8_t Chassis_MotorEnable(Chassis_t *c) {
@@ -25,18 +25,38 @@ static int8_t Chassis_MotorEnable(Chassis_t *c) {
     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_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
   if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
 
+
     Chassis_MotorEnable(c);
-  // 清空pid积分
-    PID_Reset(&c->pid.leglength[0]);
-    PID_Reset(&c->pid.leglength[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_pid[0]);
-    PID_Reset(&c->pid.tp_pid[1]);
+    PID_Reset(&c->pid.tp[0]);
+    PID_Reset(&c->pid.tp[1]);
 
     c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
 
@@ -104,12 +124,12 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
     VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
 
     /*初始化pid*/
-    PID_Init(&c->pid.leglength[0], KPID_MODE_CALC_D, target_freq, &param->leglength);
-    PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, &param->leglength);
+    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_pid[0], KPID_MODE_CALC_D, target_freq, &param->tp_pid);
-    PID_Init(&c->pid.tp_pid[1], KPID_MODE_CALC_D, target_freq, &param->tp_pid);
+    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_param.max_wheel_torque, param->lqr_param.max_joint_torque);
@@ -207,31 +227,38 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
             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_[0], fn, tp);
-
-            // VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
-            
-            // MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1});
-            if (Chassis_LQRControl(c, c_cmd) != 0) {
-                // LQR控制失败，切换到安全模式
-                return CHASSIS_ERR;
-            }
+            Chassis_LQRControl(c, c_cmd);
 
             break;
 
         case CHASSIS_MODE_RECOVER:
-            // 恢复模式，使用简单的关节位置控制回到初始姿态
-            // TODO: 实现恢复逻辑
-            break;
+        {
+            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;
+            float tp1, tp2;
+            fn = -20.0f;
+
+
+            tp1 = PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
+            tp2 = PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
+
+            VMC_InverseSolve(&c->vmc_[0], fn, tp1);
+            VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
+            VMC_InverseSolve(&c->vmc_[1], fn, tp2);
+            VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
+
+            c->output.wheel[0] = 0.0f;
+            c->output.wheel[1] = 0.0f;
+            Chassis_Output(c); // 统一输出
+        }
+        break;
             
         case CHASSIS_MODE_WHELL_BALANCE:
             // 更新VMC正解算用于状态估计
-            // MOTOR_LZ_Relax(&c->param->joint_motors[0]);
-            // MOTOR_LZ_Relax(&c->param->joint_motors[1]);
-            // MOTOR_LZ_Relax(&c->param->joint_motors[2]);
-            // MOTOR_LZ_Relax(&c->param->joint_motors[3]);
-            // MOTOR_LK_Relax(&c->param->wheel_motors[0]);
-            // MOTOR_LK_Relax(&c->param->wheel_motors[1]);
             for (int i = 0; i < 4; i++) {
                 c->output.joint[i].torque = 0.0f;
             }
@@ -330,8 +357,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     xhat = c->chassis_state.position_x;
     
     // 目标设定
-    target_dot_x = c_cmd->move_vec.vx/3.0f;
-    target_dot_x = SpeedLimit_TargetSpeed(1000.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
+    target_dot_x = c_cmd->move_vec.vx;
+    target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
     target_x += target_dot_x * c->dt;
     
     /* 分别计算左右腿的LQR控制 */
@@ -405,8 +432,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     
     
     /* 轮毂力矩输出（参考C++版本的减速比） */
-    c->output.wheel[0] = Tw[0] / 5.0f - c->yaw_control.yaw_force;
-    c->output.wheel[1] = Tw[1] / 5.0f + c->yaw_control.yaw_force;
+    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];
@@ -427,13 +454,13 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     float Delta_Tp[2];
     // 使用tp_pid进行左右腿摆角同步控制
     // 左腿补偿力矩：使左腿theta向右腿theta靠拢
-    Delta_Tp[0] = -leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp_pid[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
+    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_pid[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
+    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.leglength[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
+        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] + Delta_Tp[leg]) * sinf(leg_theta[leg]) + 
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index ffe1d28..3775fd9 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -69,25 +69,25 @@ typedef struct {
 /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
 typedef struct {
 
-  VMC_Param_t vmc_param[2];        /* VMC参数 */
-  KPID_Params_t leglength;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
-  KPID_Params_t yaw;               /* yaw轴PID控制参数，用于控制底盘朝向 */
-  KPID_Params_t roll;              /* roll轴PID控制参数，用于横滚角补偿 */
-  KPID_Params_t tp_pid;          /* 摆力矩PID控制参数，用于控制摆力矩 */
-
-
   MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
   MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
 
-  float mech_zero_yaw; /* 机械零点 */
-  
+  VMC_Param_t vmc_param[2];        /* VMC参数 */
+  LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
+
   /* LQR控制器参数 */
   struct {
     float max_wheel_torque; /* 轮毂电机最大力矩限制 */
     float max_joint_torque; /* 关节电机最大力矩限制 */
   } lqr_param;
   
-  LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
+  KPID_Params_t yaw;               /* yaw轴PID控制参数，用于控制底盘朝向 */
+  KPID_Params_t roll;              /* roll轴PID控制参数，用于横滚角补偿 */
+  KPID_Params_t tp;          /* 摆力矩PID控制参数，用于控制摆力矩 */
+  KPID_Params_t leg_length;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
+  KPID_Params_t leg_theta;          /* 摆角PID控制参数，用于控制虚拟腿摆角 */
+
+  float mech_zero_yaw; /* 机械零点 */
 
   /* 低通滤波器截止频率 */
   struct {
@@ -146,14 +146,11 @@ typedef struct {
 
   /* 反馈控制用的PID */
   struct {
-    KPID_t left_wheel;  /* 左轮PID */
-    KPID_t right_wheel; /* 右轮PID */
-    KPID_t follow; /* 跟随云台用的PID */
-    KPID_t balance; /* 平衡用的PID */
-    KPID_t yaw;     /* yaw轴控制PID */
+    KPID_t yaw; /* 跟随云台用的PID */
     KPID_t roll;    /* 横滚角控制PID */
-    KPID_t tp_pid[2];
-    KPID_t leglength[2]; /* 两条腿的腿长PID */
+    KPID_t tp[2];
+    KPID_t leg_length[2]; /* 两条腿的腿长PID */
+    KPID_t leg_theta[2]; /* 两条腿的摆角PID */
   } pid;
 
   /* 滤波器 */
diff --git a/User/module/config.c b/User/module/config.c
index 16ba46e..1427a1c 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -68,16 +68,6 @@ Config_RobotParam_t robot_config = {
     },
     
     .chassis_param = {
-        .leglength={
-            .k = 20.0f,
-            .p = 1.0f,
-            .i = 0.0f,
-            .d = 0.0f,
-            .i_limit = 0.0f,
-            .out_limit = 100.0f,
-            .d_cutoff_freq = -1.0f,
-            .range = -1.0f,
-        },
         .yaw={
             .k=1.0f,
             .p=1.0f,
@@ -88,6 +78,7 @@ Config_RobotParam_t robot_config = {
             .d_cutoff_freq=30.0f,
             .range=M_2PI, /* 2*PI，用于处理角度循环误差 */
         },
+
         .roll={
             .k=1.0f,
             .p=5.0f,           /* 横滚角比例系数 */
@@ -99,7 +90,28 @@ Config_RobotParam_t robot_config = {
             .range=-1.0f,      /* 不使用循环误差处理 */
         },
 
-        .tp_pid={
+        .leg_length={
+            .k = 20.0f,
+            .p = 1.0f,
+            .i = 0.0f,
+            .d = 0.0f,
+            .i_limit = 0.0f,
+            .out_limit = 100.0f,
+            .d_cutoff_freq = -1.0f,
+            .range = -1.0f,
+        },
+        .leg_theta={
+            .k=1.0f,
+            .p=5.0f,           /* 摆角比例系数 */
+            .i=0.0f,           /* 摆角积分系数 */
+            .d=0.0f,           /* 摆角微分系数 */
+            .i_limit=0.0f,     /* 积分限幅 */
+            .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */
+            .d_cutoff_freq=30.0f, /* 微分截止频率 */
+            .range=-1.0f,      /* 不使用循环误差处理 */
+        },
+
+        .tp={
             .k=1.0f,
             .p=1.0f,          /* 俯仰角比例系数 */
             .i=0.0f,           /* 俯仰角积分系数 */
@@ -236,7 +248,7 @@ Config_RobotParam_t robot_config = {
 
         },
         .lqr_param.max_joint_torque = 20.0f,  // 关节电机最大力矩 20Nm
-        .lqr_param.max_wheel_torque = 2.5f,  // 轮毂电机最大力矩 2.5Nm
+        .lqr_param.max_wheel_torque = 4.5f,  // 轮毂电机最大力矩 2.5Nm
     }
 };
 
diff --git a/User/module/mod_wheelleg_chassis.cpp b/User/module/mod_wheelleg_chassis.cpp
index 8f82c6c..2958ad2 100644
--- a/User/module/mod_wheelleg_chassis.cpp
+++ b/User/module/mod_wheelleg_chassis.cpp
@@ -4,7 +4,7 @@
 #include <tuple>
 
 using namespace Module;
-Wheelleg::Wheelleg(Param& param)
+Wheelleg::Wheelleg(Param &param)
     : param_(param),
       roll_pid_(param.roll_pid_param, 333.0f),
       yaw_pid_(param.yaw_pid_param, 333.0f),
@@ -13,7 +13,8 @@ Wheelleg::Wheelleg(Param& param)
       acclfilter_(333.0f, 30.0f),
       manfilter_(param.manfilter_param),
 
-      ctrl_lock_(true) {
+      ctrl_lock_(true)
+{
   memset(&(this->cmd_), 0, sizeof(this->cmd_));
 
   this->hip_motor_.at(0) =
@@ -46,31 +47,34 @@ Wheelleg::Wheelleg(Param& param)
   this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
   this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
 
-  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
+  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg *chassis)
+  {
     chassis->ctrl_lock_.Wait(UINT32_MAX);
-    switch (event) {
-      case SET_MODE_RELAX:
-        chassis->SetMode(RELAX);
-        break;
-      case SET_MODE_STAND:
-        chassis->SetMode(STAND);
-        break;
-      case SET_MODE_ROTOR:
-        chassis->SetMode(ROTOR);
-        break;
-      case SET_MODE_RESET:
-        chassis->SetMode(RESET);
-        break;
-      default:
-        break;
+    switch (event)
+    {
+    case SET_MODE_RELAX:
+      chassis->SetMode(RELAX);
+      break;
+    case SET_MODE_STAND:
+      chassis->SetMode(STAND);
+      break;
+    case SET_MODE_ROTOR:
+      chassis->SetMode(ROTOR);
+      break;
+    case SET_MODE_RESET:
+      chassis->SetMode(RESET);
+      break;
+    default:
+      break;
     }
     chassis->ctrl_lock_.Post();
   };
 
-  Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
+  Component::CMD::RegisterEvent<Wheelleg *, ChassisEvent>(
       event_callback, this, this->param_.EVENT_MAP);
 
-  auto chassis_thread = [](Wheelleg* chassis) {
+  auto chassis_thread = [](Wheelleg *chassis)
+  {
     auto cmd_sub =
         Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
     auto eulr_sub =
@@ -85,7 +89,8 @@ Wheelleg::Wheelleg(Param& param)
     // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
     uint32_t last_online_time = bsp_time_get_ms();
 
-    while (1) {
+    while (1)
+    {
       eulr_sub.DumpData(chassis->eulr_); /* imu */
       gyro_sub.DumpData(chassis->gyro_); /* imu */
       accl_sub.DumpData(chassis->accl_);
@@ -109,8 +114,10 @@ Wheelleg::Wheelleg(Param& param)
                        System::Thread::MEDIUM);
 }
 
-void Wheelleg::MotorSetAble() {
-  if (this->hip_motor_flag_ == 0) {
+void Wheelleg::MotorSetAble()
+{
+  if (this->hip_motor_flag_ == 0)
+  {
     this->hip_motor_[0]->Relax();
     this->hip_motor_[1]->Relax();
     this->hip_motor_[2]->Relax();
@@ -118,18 +125,24 @@ void Wheelleg::MotorSetAble() {
     this->dm_motor_flag_ = 0;
   }
 
-  else {
-    if (this->dm_motor_flag_ == 0) {
-      for (int i = 0; i < 5; i++) {
+  else
+  {
+    if (this->dm_motor_flag_ == 0)
+    {
+      for (int i = 0; i < 5; i++)
+      {
         this->hip_motor_[0]->Enable();
       }
-      for (int i = 0; i < 5; i++) {
+      for (int i = 0; i < 5; i++)
+      {
         this->hip_motor_[1]->Enable();
       }
-      for (int i = 0; i < 5; i++) {
+      for (int i = 0; i < 5; i++)
+      {
         this->hip_motor_[2]->Enable();
       }
-      for (int i = 0; i < 5; i++) {
+      for (int i = 0; i < 5; i++)
+      {
         this->hip_motor_[3]->Enable();
       }
 
@@ -138,7 +151,8 @@ void Wheelleg::MotorSetAble() {
   };
 }
 
-void Wheelleg::UpdateFeedback() {
+void Wheelleg::UpdateFeedback()
+{
   this->now_ = bsp_time_get();
   this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
   this->last_wakeup_ = this->now_;
@@ -148,27 +162,30 @@ void Wheelleg::UpdateFeedback() {
 
   this->leg_argu_[0].phi4_ =
       this->hip_motor_[0]->GetAngle() -
-      this->param_.wheel_param.mech_zero[0];  // 前关节角度
+      this->param_.wheel_param.mech_zero[0]; // 前关节角度
   this->leg_argu_[0].phi1_ =
       this->hip_motor_[1]->GetAngle() -
-      this->param_.wheel_param.mech_zero[1];  // 后关节角度
+      this->param_.wheel_param.mech_zero[1]; // 后关节角度
 
   this->leg_argu_[1].phi4_ =
       (-this->hip_motor_[2]->GetAngle() +
-       this->param_.wheel_param.mech_zero[3]);  // 前关节角度
-  if (leg_argu_[1].phi4_ > M_PI) {
+       this->param_.wheel_param.mech_zero[3]); // 前关节角度
+  if (leg_argu_[1].phi4_ > M_PI)
+  {
     this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
   }
 
   this->leg_argu_[1].phi1_ =
       (-this->hip_motor_[3]->GetAngle() +
-       this->param_.wheel_param.mech_zero[2]);  // 后关节角度
-  if (leg_argu_[1].phi1_ > M_PI) {
+       this->param_.wheel_param.mech_zero[2]); // 后关节角度
+  if (leg_argu_[1].phi1_ > M_PI)
+  {
     this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
   }
 
   this->pit_ = this->eulr_.pit;
-  if (this->pit_ > M_PI) {
+  if (this->pit_ > M_PI)
+  {
     this->pit_ = this->eulr_.pit - 2 * M_PI;
   }
 
@@ -181,10 +198,12 @@ void Wheelleg::UpdateFeedback() {
   this->leg_argu_[0].theta = -std::get<2>(result0);
   this->leg_argu_[0].d_theta = -std::get<3>(result0);
 
-  if (leg_argu_[0].theta > M_PI) {
+  if (leg_argu_[0].theta > M_PI)
+  {
     leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
   }
-  if (leg_argu_[0].theta < -M_PI) {
+  if (leg_argu_[0].theta < -M_PI)
+  {
     leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
   }
 
@@ -196,10 +215,12 @@ void Wheelleg::UpdateFeedback() {
   this->leg_argu_[1].theta = -std::get<2>(result1);
   this->leg_argu_[1].d_theta = -std::get<3>(result1);
 
-  if (leg_argu_[1].theta > M_PI) {
+  if (leg_argu_[1].theta > M_PI)
+  {
     leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
   }
-  if (leg_argu_[1].theta < -M_PI) {
+  if (leg_argu_[1].theta < -M_PI)
+  {
     leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
   }
 
@@ -230,7 +251,8 @@ void Wheelleg::UpdateFeedback() {
 
   this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
 
-  if (now_ > 1000000) {
+  if (now_ > 1000000)
+  {
     this->move_argu_.x_dot_hat =
         manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
                                this->move_argu_.last_x_dot, accl_.y * 9.8f,
@@ -244,344 +266,388 @@ void Wheelleg::UpdateFeedback() {
   }
 }
 
-void Wheelleg::Calculate() {
-  switch (this->mode_) {
-    case Wheelleg::RELAX:
-      // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
-      //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
-      //     move_argu_.target_dot_x -= 0.004;
-      //   }
-      //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
-      //     move_argu_.target_dot_x += 0.004;
-      //   }
-      // } else {
-      //   move_argu_.target_dot_x = cmd_.y * 1.5f;
-      // }
-      // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
-      // move_argu_.target_x;
-      move_argu_.target_x = 0;
-      move_argu_.target_dot_x = 0;
-      break;
-    case Wheelleg::STAND:
+void Wheelleg::Calculate()
+{
+  switch (this->mode_)
+  {
+  case Wheelleg::RELAX:
+    // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
+    //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
+    //     move_argu_.target_dot_x -= 0.004;
+    //   }
+    //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
+    //     move_argu_.target_dot_x += 0.004;
+    //   }
+    // } else {
+    //   move_argu_.target_dot_x = cmd_.y * 1.5f;
+    // }
+    // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
+    // move_argu_.target_x;
+    move_argu_.target_x = 0;
+    move_argu_.target_dot_x = 0;
+    break;
+  case Wheelleg::STAND:
 
-      /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
-      if (fabs(move_argu_.target_dot_x -
-               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
-        if (move_argu_.target_dot_x >
-            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
-          move_argu_.target_dot_x -= 0.004;
-        }
-        if (move_argu_.target_dot_x <
-            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
-          move_argu_.target_dot_x += 0.004;
-        }
-      } else {
-        move_argu_.target_dot_x =
-            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+    /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
+    if (fabs(move_argu_.target_dot_x -
+             cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
+    {
+      if (move_argu_.target_dot_x >
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x -= 0.004;
       }
-      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
-
-      this->move_argu_.target_yaw = 0.0f;
-
-      /*双零点*/
-      if (this->yaw_ > M_PI_2) {
-        this->move_argu_.target_yaw = 3.14158f;
+      if (move_argu_.target_dot_x <
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x += 0.004;
       }
-      if (this->yaw_ < -M_PI_2) {
-        this->move_argu_.target_yaw = 3.14158f;
+    }
+    else
+    {
+      move_argu_.target_dot_x =
+          cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+    }
+    move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+
+    this->move_argu_.target_yaw = 0.0f;
+
+    /*双零点*/
+    if (this->yaw_ > M_PI_2)
+    {
+      this->move_argu_.target_yaw = 3.14158f;
+    }
+    if (this->yaw_ < -M_PI_2)
+    {
+      this->move_argu_.target_yaw = 3.14158f;
+    }
+    break;
+
+  case Wheelleg::ROTOR:
+    if (fabs(move_argu_.target_dot_x -
+             cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
+    {
+      if (move_argu_.target_dot_x >
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x -= 0.004;
       }
-      break;
-
-    case Wheelleg::ROTOR:
-      if (fabs(move_argu_.target_dot_x -
-               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
-        if (move_argu_.target_dot_x >
-            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
-          move_argu_.target_dot_x -= 0.004;
-        }
-        if (move_argu_.target_dot_x <
-            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
-          move_argu_.target_dot_x += 0.004;
-        }
-      } else {
-        move_argu_.target_dot_x =
-            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      if (move_argu_.target_dot_x <
+          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
+      {
+        move_argu_.target_dot_x += 0.004;
       }
-      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
-      this->move_argu_.target_yaw = this->yaw_ + 0.01;
+    }
+    else
+    {
+      move_argu_.target_dot_x =
+          cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+    }
+    move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+    this->move_argu_.target_yaw = this->yaw_ + 0.01;
 
-      break;
-      // move_argu_.target_dot_x = cmd_.x;
-      // move_argu_.target_x =
-      //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
-      // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
-      // cmd_.y);
-      // // move_argu_.x_dot =
-      // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
-      // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
-      // break;
+    break;
+    // move_argu_.target_dot_x = cmd_.x;
+    // move_argu_.target_x =
+    //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+    // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
+    // cmd_.y);
+    // // move_argu_.x_dot =
+    // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+    // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
+    // break;
 
-    case Wheelleg::RESET:
-      this->move_argu_.target_dot_x = 0;
-      move_argu_.target_x = 0;
-      this->move_argu_.target_yaw = this->eulr_.yaw;
-      this->move_argu_.xhat = 0;
+  case Wheelleg::RESET:
+    this->move_argu_.target_dot_x = 0;
+    move_argu_.target_x = 0;
+    this->move_argu_.target_yaw = this->eulr_.yaw;
+    this->move_argu_.xhat = 0;
 
-      //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
-      // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
-      //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
-      // }
-      break;
+    //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
+    // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
+    //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
+    // }
+    break;
 
-    default:
-      XB_ASSERT(false);
-      return;
+  default:
+    XB_ASSERT(false);
+    return;
   }
 
   leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
   onground0_flag_ = false;
-  if (leg_argu_[0].Fn > 30) {
+  if (leg_argu_[0].Fn > 30)
+  {
     onground0_flag_ = true;
   }
   leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
   onground1_flag_ = false;
-  if (leg_argu_[1].Fn > 30) {
+  if (leg_argu_[1].Fn > 30)
+  {
     onground1_flag_ = true;
   }
   std::tuple<float, float> result3;
   std::tuple<float, float> result4;
 
-  switch (this->mode_) {
-    case Wheelleg::RELAX:
-    case Wheelleg::ROTOR:
-    case Wheelleg::STAND:
+  switch (this->mode_)
+  {
+  case Wheelleg::RELAX:
+  case Wheelleg::ROTOR:
+  case Wheelleg::STAND:
 
-      for (int i = 0; i < 12; i++) {
-        leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
-            &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
-            leg_argu_[0].L0);
+    for (int i = 0; i < 12; i++)
+    {
+      leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
+          &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
+          leg_argu_[0].L0);
+    }
+
+    for (int i = 0; i < 12; i++)
+    {
+      leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
+          &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
+          leg_argu_[1].L0);
+    }
+    if (now_ > 1000000)
+      if (fabs(move_argu_.target_dot_x) > 0.5 ||
+          fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
+          ((!onground0_flag_) & (!onground1_flag_)))
+      {
+        leg_argu_[0].LQR_K[2] = 0;
+        leg_argu_[1].LQR_K[2] = 0;
+        this->move_argu_.xhat = 0;
+        move_argu_.target_x = 0;
       }
 
-      for (int i = 0; i < 12; i++) {
-        leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
-            &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
-            leg_argu_[1].L0);
-      }
-      if (now_ > 1000000)
-        if (fabs(move_argu_.target_dot_x) > 0.5 ||
-            fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
-            ((!onground0_flag_) & (!onground1_flag_))) {
-          leg_argu_[0].LQR_K[2] = 0;
-          leg_argu_[1].LQR_K[2] = 0;
-          this->move_argu_.xhat = 0;
-          move_argu_.target_x = 0;
+    if (onground0_flag_)
+    {
+      leg_argu_[0].Tw =
+          (leg_argu_[0].LQR_K[0] *
+               (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+           leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
+           leg_argu_[0].LQR_K[2] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[0].LQR_K[3] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
+           leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
+      leg_argu_[0].Tp =
+          (leg_argu_[0].LQR_K[6] *
+               (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+           leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
+           leg_argu_[0].LQR_K[8] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[0].LQR_K[9] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
+           leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
+    }
+    else
+    {
+      leg_argu_[0].Tw = 0;
+
+      leg_argu_[0].Tp =
+          (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
+           leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
+    }
+    if (onground1_flag_)
+    {
+      leg_argu_[1].Tw =
+          (leg_argu_[1].LQR_K[0] *
+               (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+           leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
+           leg_argu_[1].LQR_K[2] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[1].LQR_K[3] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
+           leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
+      leg_argu_[1].Tp =
+          (leg_argu_[1].LQR_K[6] *
+               (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+           leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
+           leg_argu_[1].LQR_K[8] *
+               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+           leg_argu_[1].LQR_K[9] *
+               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+           leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
+           leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
+    }
+    else
+    {
+      leg_argu_[1].Tw = 0;
+      leg_argu_[1].Tp =
+          (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
+           leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
+    }
+
+    this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                  this->param_.wheel_param.static_L0[0] +
+                                  +roll_pid_.Calculate(0, eulr_.rol, dt_);
+    this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                  this->param_.wheel_param.static_L0[1] -
+                                  roll_pid_.Calculate(0, eulr_.rol, dt_);
+
+    leg_argu_[0].F0 =
+        leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
+        this->param_.wheel_param.static_F0[0] +
+        leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
+                                        this->leg_argu_[0].L0, this->dt_);
+    leg_argu_[1].F0 =
+        leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
+        this->param_.wheel_param.static_F0[1] +
+        leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
+                                        this->leg_argu_[1].L0, this->dt_);
+
+    this->leg_argu_[0].Delta_Tp =
+        leg_argu_[0].L0 * 10.0f *
+        tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                 this->leg_argu_[0].theta, this->dt_);
+    this->leg_argu_[1].Delta_Tp =
+        -leg_argu_[1].L0 * 10.0f *
+        tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                 this->leg_argu_[0].theta, this->dt_);
+
+    result3 = leg_[0]->VMCinserve(
+        -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+        -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
+    this->leg_argu_[0].T1 = std::get<0>(result3);
+    this->leg_argu_[0].T2 = std::get<1>(result3);
+
+    result4 = leg_[1]->VMCinserve(
+        -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+        -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
+    this->leg_argu_[1].T1 = -std::get<0>(result4);
+    this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+    if (onground0_flag_ & onground1_flag_)
+    {
+      move_argu_.yaw_force =
+          -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
+    }
+    else
+    {
+      move_argu_.yaw_force = 0;
+    }
+    /*3508不带减速箱是Tw*3.2f*/
+    /*2006带减速是Tw/1.8f*/
+    /* 3508带15.7减速箱是Tw/4.9256 */
+
+    this->wheel_motor_out_[0] =
+        this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
+
+    this->wheel_motor_out_[1] =
+        this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
+
+    this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+    this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+    this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+    this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+    this->hip_motor_flag_ = 1;
+    break;
+
+  case Wheelleg::RESET:
+    if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
+        fabs(leg_argu_[1].theta) > 1.57)
+    {
+      leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+
+      if (fabs(pit_) > M_PI / 2)
+      {
+        if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03)
+        {
+          leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
         }
-
-      if (onground0_flag_) {
-        leg_argu_[0].Tw =
-            (leg_argu_[0].LQR_K[0] *
-                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
-             leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
-             leg_argu_[0].LQR_K[2] *
-                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
-             leg_argu_[0].LQR_K[3] *
-                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
-             leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
-             leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
-        leg_argu_[0].Tp =
-            (leg_argu_[0].LQR_K[6] *
-                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
-             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
-             leg_argu_[0].LQR_K[8] *
-                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
-             leg_argu_[0].LQR_K[9] *
-                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
-             leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
-             leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
-      } else {
-        leg_argu_[0].Tw = 0;
-
-        leg_argu_[0].Tp =
-            (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
-             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
+        if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03)
+        {
+          leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
+        }
+        leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
+                                   0.65f, this->leg_argu_[0].L0, this->dt_);
+        leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
+                                   0.65f, this->leg_argu_[1].L0, this->dt_);
       }
-      if (onground1_flag_) {
-        leg_argu_[1].Tw =
-            (leg_argu_[1].LQR_K[0] *
-                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
-             leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
-             leg_argu_[1].LQR_K[2] *
-                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
-             leg_argu_[1].LQR_K[3] *
-                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
-             leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
-             leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
-        leg_argu_[1].Tp =
-            (leg_argu_[1].LQR_K[6] *
-                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
-             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
-             leg_argu_[1].LQR_K[8] *
-                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
-             leg_argu_[1].LQR_K[9] *
-                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
-             leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
-             leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
-      } else {
-        leg_argu_[1].Tw = 0;
-        leg_argu_[1].Tp =
-            (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
-             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
+      if (fabs(leg_argu_[0].theta) < 1.57)
+      {
+        leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
+        leg_argu_[0].target_theta = leg_argu_[0].theta;
       }
 
-      this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
-                                    this->param_.wheel_param.static_L0[0] +
-                                    +roll_pid_.Calculate(0, eulr_.rol, dt_);
-      this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
-                                    this->param_.wheel_param.static_L0[1] -
-                                    roll_pid_.Calculate(0, eulr_.rol, dt_);
-
-      leg_argu_[0].F0 =
-          leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
-          this->param_.wheel_param.static_F0[0] +
-          leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
-                                          this->leg_argu_[0].L0, this->dt_);
-      leg_argu_[1].F0 =
-          leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
-          this->param_.wheel_param.static_F0[1] +
-          leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
-                                          this->leg_argu_[1].L0, this->dt_);
-
-      this->leg_argu_[0].Delta_Tp =
-          leg_argu_[0].L0 * 10.0f *
-          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
-                                   this->leg_argu_[0].theta, this->dt_);
-      this->leg_argu_[1].Delta_Tp =
-          -leg_argu_[1].L0 * 10.0f *
-          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
-                                   this->leg_argu_[0].theta, this->dt_);
-
-      result3 = leg_[0]->VMCinserve(
-          -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-          -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
-      this->leg_argu_[0].T1 = std::get<0>(result3);
-      this->leg_argu_[0].T2 = std::get<1>(result3);
-
-      result4 = leg_[1]->VMCinserve(
-          -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-          -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
-      this->leg_argu_[1].T1 = -std::get<0>(result4);
-      this->leg_argu_[1].T2 = -std::get<1>(result4);
-
-      if (onground0_flag_ & onground1_flag_) {
-        move_argu_.yaw_force =
-            -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
-      } else {
-        move_argu_.yaw_force = 0;
-      }
-      /*3508不带减速箱是Tw*3.2f*/
-      /*2006带减速是Tw/1.8f*/
-      /* 3508带15.7减速箱是Tw/4.9256 */
-
-      this->wheel_motor_out_[0] =
-          this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
-
-      this->wheel_motor_out_[1] =
-          this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
-
-      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
-      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
-      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
-      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
-
-      this->hip_motor_flag_ = 1;
-      break;
-
-    case Wheelleg::RESET:
-      if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
-          fabs(leg_argu_[1].theta) > 1.57) {
+      if (fabs(leg_argu_[1].theta) < 1.57)
+      {
         leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
-
-        if (fabs(pit_) > M_PI / 2) {
-          if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
-            leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
-          }
-          if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
-            leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
-          }
-          leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
-                                     0.65f, this->leg_argu_[0].L0, this->dt_);
-          leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
-                                     0.65f, this->leg_argu_[1].L0, this->dt_);
-        }
-        if (fabs(leg_argu_[0].theta) < 1.57) {
-          leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
-          leg_argu_[0].target_theta = leg_argu_[0].theta;
-        }
-
-        if (fabs(leg_argu_[1].theta) < 1.57) {
-          leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
-          leg_argu_[1].target_theta = leg_argu_[1].theta;
-        }
-
-        if (leg_argu_[0].target_theta > M_PI) {
-          leg_argu_[0].target_theta -= 2 * M_PI;
-        }
-        if (leg_argu_[0].target_theta < -M_PI) {
-          leg_argu_[0].target_theta += 2 * M_PI;
-        }
-        if (leg_argu_[1].target_theta > M_PI) {
-          leg_argu_[1].target_theta -= 2 * M_PI;
-        }
-        if (leg_argu_[1].target_theta < -M_PI) {
-          leg_argu_[1].target_theta += 2 * M_PI;
-        }
-        leg_argu_[0].Tp =
-            500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
-                                                 leg_argu_[0].theta, dt_);
-        leg_argu_[1].Tp =
-            500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
-                                                 leg_argu_[1].theta, dt_);
-
-      } else {
-        leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
-                                  0.10f, this->leg_argu_[0].L0, this->dt_);
-        leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
-                                  0.10f, this->leg_argu_[1].L0, this->dt_);
-
-        if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
-          leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
-                                      0.1, leg_argu_[0].theta, dt_);
-          leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
-                                      0.1, leg_argu_[1].theta, dt_);
-          clampf(&leg_argu_[0].Tp, -10, 10);
-          clampf(&leg_argu_[1].Tp, -10, 10);
-        } else {
-          leg_argu_[0].Tp = 0;
-          leg_argu_[1].Tp = 0;
-        }
+        leg_argu_[1].target_theta = leg_argu_[1].theta;
       }
 
-      result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-                                    -leg_argu_[0].Tp, leg_argu_[0].F0);
-      this->leg_argu_[0].T1 = std::get<0>(result3);
-      this->leg_argu_[0].T2 = std::get<1>(result3);
+      if (leg_argu_[0].target_theta > M_PI)
+      {
+        leg_argu_[0].target_theta -= 2 * M_PI;
+      }
+      if (leg_argu_[0].target_theta < -M_PI)
+      {
+        leg_argu_[0].target_theta += 2 * M_PI;
+      }
+      if (leg_argu_[1].target_theta > M_PI)
+      {
+        leg_argu_[1].target_theta -= 2 * M_PI;
+      }
+      if (leg_argu_[1].target_theta < -M_PI)
+      {
+        leg_argu_[1].target_theta += 2 * M_PI;
+      }
+      leg_argu_[0].Tp =
+          500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
+                                               leg_argu_[0].theta, dt_);
+      leg_argu_[1].Tp =
+          500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
+                                               leg_argu_[1].theta, dt_);
+    }
+    else
+    {
+      leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
+                                0.10f, this->leg_argu_[0].L0, this->dt_);
+      leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
+                                0.10f, this->leg_argu_[1].L0, this->dt_);
 
-      result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-                                    -leg_argu_[1].Tp, leg_argu_[1].F0);
-      this->leg_argu_[1].T1 = -std::get<0>(result4);
-      this->leg_argu_[1].T2 = -std::get<1>(result4);
+      if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20))
+      {
+        leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
+                                    0.1, leg_argu_[0].theta, dt_);
+        leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
+                                    0.1, leg_argu_[1].theta, dt_);
+        clampf(&leg_argu_[0].Tp, -10, 10);
+        clampf(&leg_argu_[1].Tp, -10, 10);
+      }
+      else
+      {
+        leg_argu_[0].Tp = 0;
+        leg_argu_[1].Tp = 0;
+      }
+    }
 
-      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
-      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
-      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
-      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+    result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                                  -leg_argu_[0].Tp, leg_argu_[0].F0);
+    this->leg_argu_[0].T1 = std::get<0>(result3);
+    this->leg_argu_[0].T2 = std::get<1>(result3);
 
-      this->hip_motor_flag_ = 1;
-      break;
+    result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                                  -leg_argu_[1].Tp, leg_argu_[1].F0);
+    this->leg_argu_[1].T1 = -std::get<0>(result4);
+    this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+    this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+    this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+    this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+    this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+    this->hip_motor_flag_ = 1;
+    break;
   }
 }
 
-void Wheelleg::Control() {
+void Wheelleg::Control()
+{
   clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
   clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
   clampf(&hip_motor_out_[0], -20.0f, 20.0f);
@@ -599,51 +665,54 @@ void Wheelleg::Control() {
   //   }
   // }
 
-  switch (this->mode_) {
-    case Wheelleg::RELAX:
+  switch (this->mode_)
+  {
+  case Wheelleg::RELAX:
 
-      this->wheel_motor_[0]->Relax();
-      this->wheel_motor_[1]->Relax();
-      hip_motor_flag_ = 0;
-      hip_motor_[0]->SetMit(0.0f);
-      hip_motor_[1]->SetMit(0.0f);
-      hip_motor_[2]->SetMit(0.0f);
-      hip_motor_[3]->SetMit(0.0f);
-      break;
+    this->wheel_motor_[0]->Relax();
+    this->wheel_motor_[1]->Relax();
+    hip_motor_flag_ = 0;
+    hip_motor_[0]->SetMit(0.0f);
+    hip_motor_[1]->SetMit(0.0f);
+    hip_motor_[2]->SetMit(0.0f);
+    hip_motor_[3]->SetMit(0.0f);
+    break;
 
-    case Wheelleg::STAND:
-    case Wheelleg::ROTOR:
-      // this->wheel_motor_[0]->Relax();
-      // this->wheel_motor_[1]->Relax();
-      this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
-      this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
-      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
-      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
-      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
-      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
-      // hip_motor_[0]->SetMit(0.0f);
-      // hip_motor_[1]->SetMit(0.0f);
-      // hip_motor_[2]->SetMit(0.0f);
-      // hip_motor_[3]->SetMit(0.0f);
-      break;
+  case Wheelleg::STAND:
+  case Wheelleg::ROTOR:
+    // this->wheel_motor_[0]->Relax();
+    // this->wheel_motor_[1]->Relax();
+    this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
+    this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
+    hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+    hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+    hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+    hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+    // hip_motor_[0]->SetMit(0.0f);
+    // hip_motor_[1]->SetMit(0.0f);
+    // hip_motor_[2]->SetMit(0.0f);
+    // hip_motor_[3]->SetMit(0.0f);
+    break;
 
-    case Wheelleg::RESET:
+  case Wheelleg::RESET:
 
-      this->wheel_motor_[0]->Relax();
-      this->wheel_motor_[1]->Relax();
+    this->wheel_motor_[0]->Relax();
+    this->wheel_motor_[1]->Relax();
 
-      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
-      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+    hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+    hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
 
-      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
-      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+    hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+    hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
 
-      break;
+    break;
   }
 }
 
-void Wheelleg::SetMode(Wheelleg::Mode mode) {
-  if (mode == this->mode_) {
+void Wheelleg::SetMode(Wheelleg::Mode mode)
+{
+  if (mode == this->mode_)
+  {
     return;
   }
 
diff --git a/User/task/rc.c b/User/task/rc.c
index 2a303ff..bcbc569 100644
--- a/User/task/rc.c
+++ b/User/task/rc.c
@@ -46,7 +46,7 @@ void Task_rc(void *argument) {
                   cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
                   break;
               case 3: // 中位  
-                  cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
+                  cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
                   break;
               case 2: // 下位
                   cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;