起不来站不住

User/module/balance_chassis.c

@@ -229,7 +229,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
             break;
             
         case CHASSIS_MODE_WHELL_LEG_BALANCE:
-            // 轮腿平衡模式，使用LQR控制
+            // 轮腿平衡模式，使用LQR控制和PID腿长控制
             
             // 更新VMC正解算
             VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
@@ -237,15 +237,13 @@ 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);
             
-
+            // 执行LQR控制（包含PID腿长控制）
-
+            if (Chassis_LQRControl(c, c_cmd) != 0) {
-            // 执行LQR控制
+                // LQR控制失败，切换到安全模式
-            // if (Chassis_LQRControl(c, c_cmd) != 0) {
+                return CHASSIS_ERR;
-            //     // LQR控制失败，切换到安全模式
+            }
-            //     return CHASSIS_ERR;
-            // }
             
-            // Chassis_Output(c); // 统一输出
+            Chassis_Output(c); // 统一输出
             break;
             
         default:
@@ -282,11 +280,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     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] = (leg_fn[0] > 30.0f);
+    onground_flag[0] = true;  // 强制设置左腿着地
-    onground_flag[1] = (leg_fn[1] > 30.0f);
+    onground_flag[1] = true;  // 强制设置右腿着地
     
     /* 获取VMC状态（等效摆杆参数） */
     float leg_L0[2], leg_theta[2], leg_d_theta[2];
@@ -383,8 +381,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     float leg_d_length[2]; // 腿长变化率
     
     // 目标腿长设定
-    target_L0[0] = 0.13f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节
+    target_L0[0] = 0.12f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节
-    target_L0[1] = 0.13f + c_cmd->height * 0.1f;
+    target_L0[1] = 0.12f + c_cmd->height * 0.1f;
     
     // 获取腿长变化率
     VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
@@ -397,7 +395,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
         // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
         virtual_force[leg] = Tp[leg] * sinf(leg_theta[leg]) + 
                             pid_output + // PID腿长控制输出
-                            50.0f; // 基础支撑力
+                            35.0f; // 基础支撑力
         
         // VMC逆解算
         if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg]) == 0) {

User/module/balance_chassis.h

@@ -70,7 +70,8 @@ typedef struct {
 typedef struct {
 
   VMC_Param_t vmc_param[2];        /* VMC参数 */
-  KPID_Params_t leglength;
+  KPID_Params_t leglength;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
+  KPID_Params_t yaw;               /* yaw轴PID控制参数，用于控制底盘朝向 */
 
 
   MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
@@ -127,6 +128,13 @@ typedef struct {
     float last_velocity_x; /* 上一次速度用于数值微分 */
   } chassis_state;
 
+  /* yaw控制相关 */
+  struct {
+    float target_yaw;     /* 目标yaw角度 */
+    float current_yaw;    /* 当前yaw角度 */
+    float yaw_force;      /* yaw轴控制力矩 */
+  } yaw_control;
+
   float wz_multi; /* 小陀螺模式旋转方向 */
 
   /* PID计算的目标值 */
@@ -140,6 +148,7 @@ typedef struct {
     KPID_t right_wheel; /* 右轮PID */
     KPID_t follow; /* 跟随云台用的PID */
     KPID_t balance; /* 平衡用的PID */
+    KPID_t yaw;     /* yaw轴控制PID */
 
     KPID_t leglength[2]; /* 两条腿的腿长PID */
 

User/module/config.c

@@ -70,14 +70,24 @@ Config_RobotParam_t robot_config = {
     .chassis_param = {
         .leglength={
             .k=1.0f,
-            .p=0.10f,
+            .p=10.0f,
             .i=0.0f,
             .d=0.0f,
             .i_limit=0.0f,
-            .out_limit=100.0f,
+            .out_limit=10.0f,
             .d_cutoff_freq=-1.0f,
             .range=-1.0f,
-        }
+        },
+        .yaw={
+            .k=0.0f,
+            .p=0.1f,
+            .i=0.0f,
+            .d=0.0f,
+            .i_limit=0.0f,
+            .out_limit=0.2f,
+            .d_cutoff_freq=30.0f,
+            .range=M_2PI, /* 2*PI，用于处理角度循环误差 */
+        },
         
 
         .low_pass_cutoff_freq = {