diff --git a/User/component/lqr.c b/User/component/lqr.c
index ae3c309..7cbd1cf 100644
--- a/User/component/lqr.c
+++ b/User/component/lqr.c
@@ -7,8 +7,6 @@
 #include <stddef.h>
 
 /* Private macros ----------------------------------------------------------- */
-#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
-
 /* Private variables -------------------------------------------------------- */
 
 /* Private function prototypes ---------------------------------------------- */
@@ -21,7 +19,62 @@ int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
     if (lqr == NULL || gain_matrix == NULL) {
         return -1;
     }
+    /* 计算状态误差 */
+    lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
+    lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
+    lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
+    lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
+    lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
+    lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
     
+    return 0;
+}
+
+static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
+    if (coeff == NULL) {
+        return 0.0f;
+    }
+    
+    /* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
+    float L = leg_length;
+    float L2 = L * L;
+    float L3 = L2 * L;
+    
+    return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
+}
+
+
+// static int8_t LQR_ApplyControlLimits(LQR_t *lqr) {
+//     if (lqr == NULL) {
+//         return -1;
+//     }
+    
+//     /* 限制轮毂力矩 */
+//     if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
+//         lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, 
+//                                          -lqr->param.max_wheel_torque, 
+//                                           lqr->param.max_wheel_torque);
+//         lqr->wheel_torque_limited = true;
+//     }
+    
+//     /* 限制髋关节力矩 */
+//     if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
+//         lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
+//                                           -lqr->param.max_joint_torque,
+//                                            lqr->param.max_joint_torque);
+//         lqr->joint_torque_limited = true;
+//     }
+    
+//     return 0;
+// }
+
+
+/* Public functions --------------------------------------------------------- */
+
+int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
+    if (lqr == NULL) {
+        return -1;
+    }
     /* 清零结构体 */
     memset(lqr, 0, sizeof(LQR_t));
     
@@ -68,7 +121,6 @@ int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
     }
     
     /* 限制腿长范围 */
-    leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
     lqr->current_leg_length = leg_length;
     
     /* 使用多项式拟合计算当前增益矩阵 */
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index 19a5879..fc3c6d2 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -255,7 +255,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
  * \param s 包含底盘数据的结构体
  * \param out CAN设备底盘输出结构体
  */
-void Chassis_Output(Chassis_t *c);
+int8_t Chassis_Output(Chassis_t *c);
 
 #ifdef __cplusplus
 }
diff --git a/mod_wheelleg_chassis.cpp b/mod_wheelleg_chassis.cpp
new file mode 100644
index 0000000..8f82c6c
--- /dev/null
+++ b/mod_wheelleg_chassis.cpp
@@ -0,0 +1,662 @@
+#include "mod_wheelleg_chassis.hpp"
+
+#include <cmath>
+#include <tuple>
+
+using namespace Module;
+Wheelleg::Wheelleg(Param& param)
+    : param_(param),
+      roll_pid_(param.roll_pid_param, 333.0f),
+      yaw_pid_(param.yaw_pid_param, 333.0f),
+      yaccl_pid_(param.yaccl_pid_param, 333.0f),
+      lowfilter_(333.0f, 50.0f),
+      acclfilter_(333.0f, 30.0f),
+      manfilter_(param.manfilter_param),
+
+      ctrl_lock_(true) {
+  memset(&(this->cmd_), 0, sizeof(this->cmd_));
+
+  this->hip_motor_.at(0) =
+      new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
+  this->hip_motor_.at(1) =
+      new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
+  this->hip_motor_.at(2) =
+      new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
+  this->hip_motor_.at(3) =
+      new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
+
+  this->wheel_motor_.at(0) =
+      new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
+  this->wheel_motor_.at(1) =
+      new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
+
+  this->leglength_pid_.at(0) =
+      new Component::PID(param.leglength_pid_param.at(0), 333.0f);
+  this->leglength_pid_.at(1) =
+      new Component::PID(param.leglength_pid_param.at(1), 333.0f);
+
+  this->theta_pid_.at(0) =
+      new Component::PID(param.theta_pid_param.at(0), 333.0f);
+  this->theta_pid_.at(1) =
+      new Component::PID(param.theta_pid_param.at(1), 333.0f);
+
+  this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
+  this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
+
+  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) {
+    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;
+    }
+    chassis->ctrl_lock_.Post();
+  };
+
+  Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
+      event_callback, this, this->param_.EVENT_MAP);
+
+  auto chassis_thread = [](Wheelleg* chassis) {
+    auto cmd_sub =
+        Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
+    auto eulr_sub =
+        Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
+    auto gyro_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
+
+    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+
+    auto accl_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
+    // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+    uint32_t last_online_time = bsp_time_get_ms();
+
+    while (1) {
+      eulr_sub.DumpData(chassis->eulr_); /* imu */
+      gyro_sub.DumpData(chassis->gyro_); /* imu */
+      accl_sub.DumpData(chassis->accl_);
+
+      yaw_sub.DumpData(chassis->yaw_);
+      cmd_sub.DumpData(chassis->cmd_);
+      // yaw_sub.DumpData(chassis->yaw_);
+
+      chassis->ctrl_lock_.Wait(UINT32_MAX);
+      chassis->MotorSetAble();
+      chassis->UpdateFeedback();
+      chassis->Calculate();
+      chassis->Control();
+      chassis->ctrl_lock_.Post();
+
+      /* 运行结束，等待下一次唤醒 */
+      chassis->thread_.SleepUntil(3, last_online_time);
+    }
+  };
+  this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
+                       System::Thread::MEDIUM);
+}
+
+void Wheelleg::MotorSetAble() {
+  if (this->hip_motor_flag_ == 0) {
+    this->hip_motor_[0]->Relax();
+    this->hip_motor_[1]->Relax();
+    this->hip_motor_[2]->Relax();
+    this->hip_motor_[3]->Relax();
+    this->dm_motor_flag_ = 0;
+  }
+
+  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++) {
+        this->hip_motor_[1]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[2]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[3]->Enable();
+      }
+
+      this->dm_motor_flag_ = 1;
+    }
+  };
+}
+
+void Wheelleg::UpdateFeedback() {
+  this->now_ = bsp_time_get();
+  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+  this->last_wakeup_ = this->now_;
+
+  this->wheel_motor_[0]->Update();
+  this->wheel_motor_[1]->Update();
+
+  this->leg_argu_[0].phi4_ =
+      this->hip_motor_[0]->GetAngle() -
+      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->leg_argu_[1].phi4_ =
+      (-this->hip_motor_[2]->GetAngle() +
+       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->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
+  }
+
+  this->pit_ = this->eulr_.pit;
+  if (this->pit_ > M_PI) {
+    this->pit_ = this->eulr_.pit - 2 * M_PI;
+  }
+
+  /*  注意极性 */
+  std::tuple<float, float, float, float> result0 =
+      this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[0].L0 = std::get<0>(result0);
+  this->leg_argu_[0].d_L0 = std::get<1>(result0);
+  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) {
+    leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
+  }
+  if (leg_argu_[0].theta < -M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
+  }
+
+  std::tuple<float, float, float, float> result1 =
+      this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[1].L0 = std::get<0>(result1);
+  this->leg_argu_[1].d_L0 = std::get<1>(result1);
+  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) {
+    leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
+  }
+  if (leg_argu_[1].theta < -M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
+  }
+
+  /* 轮子转速近似于编码器速度 由此推测机体速度 */
+  this->leg_argu_[0].single_x_dot =
+      -wheel_motor_[0]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+      leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
+
+  this->leg_argu_[1].single_x_dot =
+      wheel_motor_[1]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+      leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
+
+  this->move_argu_.last_x_dot = this->move_argu_.x_dot;
+  this->move_argu_.x_dot =
+
+      (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
+  this->move_argu_.x_dot =
+      (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
+      this->param_.wheel_param.wheel_radius / 60.f / 15.765;
+  this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
+
+  this->move_argu_.delta_speed =
+      lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
+
+  this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
+
+  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,
+                               dt_) -
+        ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+          leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
+         (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+          leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
+            2;
+    this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
+  }
+}
+
+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;
+      }
+      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;
+        }
+        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;
+      }
+      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;
+
+    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;
+
+    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) {
+    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) {
+    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:
+
+      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;
+        }
+
+      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 ((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;
+        }
+      }
+
+      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);
+
+      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() {
+  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);
+  clampf(&hip_motor_out_[1], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[2], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[3], -20.0f, 20.0f);
+
+  // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
+  //     fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
+  //   wheel_motor_out_[0] = 0;
+  //   wheel_motor_out_[1] = 0;
+
+  //   if (fabs(this->pit_) > 0.5f) {
+  //     this->hip_motor_flag_ = 0;
+  //   }
+  // }
+
+  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;
+
+    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:
+
+      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_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+
+      break;
+  }
+}
+
+void Wheelleg::SetMode(Wheelleg::Mode mode) {
+  if (mode == this->mode_) {
+    return;
+  }
+
+  this->leg_[0]->Reset();
+  this->leg_[1]->Reset();
+  move_argu_.x = 0;
+  move_argu_.x_dot = 0;
+  move_argu_.last_x_dot = 0;
+  move_argu_.target_x = move_argu_.xhat;
+  move_argu_.target_yaw = this->eulr_.yaw;
+  move_argu_.target_dot_x = 0;
+  move_argu_.xhat = 0;
+  move_argu_.x_dot_hat = 0;
+  this->manfilter_.reset_comp();
+  this->mode_ = mode;
+}