会斜腿

User/module/balance_chassis.c

@@ -27,6 +27,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   // 清空pid积分
     PID_Reset(&c->pid.leglength[0]);
     PID_Reset(&c->pid.leglength[1]);
+    PID_Reset(&c->pid.yaw);
 
   c->mode = mode;
   c->state = 0;  // 重置状态，确保每次切换模式时都重新初始化
@@ -86,6 +87,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float 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.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
     
     /*初始化LQR控制器*/
     LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
@@ -98,6 +100,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
     c->chassis_state.velocity_x = 0.0f;
     c->chassis_state.last_velocity_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;
 }
 
@@ -366,9 +373,39 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     
     /* 转向控制（参考C++版本） */
     float yaw_force = 0.0f;
+    
+    // 更新当前yaw角度
+    c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
+    
+    // 更新目标yaw角度（基于遥控器输入）
+    if (fabsf(c_cmd->move_vec.wz) > 0.01f) {
+        // 有转向指令时，更新目标角度
+        c->yaw_control.target_yaw += c_cmd->move_vec.wz * c->dt;
+        // 角度归一化到 [-π, π]
+        while (c->yaw_control.target_yaw > M_PI) {
+            c->yaw_control.target_yaw -= M_2PI;
+        }
+        while (c->yaw_control.target_yaw < -M_PI) {
+            c->yaw_control.target_yaw += M_2PI;
+        }
+    }
+    
     if (onground_flag[0] && onground_flag[1]) {
-        // 简化的转向控制，后续可以改进为PID
-        yaw_force = -c_cmd->move_vec.wz * 0.1f;  
+        // 使用PID控制yaw角度
+        float yaw_error = c->yaw_control.target_yaw - c->yaw_control.current_yaw;
+        
+        // 处理角度循环误差（选择最短路径）
+        if (yaw_error > M_PI) {
+            yaw_error -= M_2PI;
+        } else if (yaw_error < -M_PI) {
+            yaw_error += M_2PI;
+        }
+        
+        // 使用yaw轴陀螺仪作为微分项
+        float yaw_d_feedback = c->feedback.imu.gyro.z;
+        
+        yaw_force = PID_Calc(&c->pid.yaw, 0.0f, yaw_error, yaw_d_feedback, c->dt);
+        c->yaw_control.yaw_force = yaw_force;
     }
     
     /* 轮毂力矩输出（参考C++版本的减速比） */

User/module/config.c

@@ -69,8 +69,8 @@ Config_RobotParam_t robot_config = {
     
     .chassis_param = {
         .leglength={
-            .k = 0.0f,
-            .p = 10.0f,
+            .k = 5.0f,
+            .p = 15.0f,
             .i = 0.0f,
             .d = 0.0f,
             .i_limit = 0.0f,
@@ -79,7 +79,7 @@ Config_RobotParam_t robot_config = {
             .range = -1.0f,
         },
         .yaw={
-            .k=0.0f,
+            .k=0.01f,
             .p=0.1f,
             .i=0.0f,
             .d=0.0f,