腿长好了

Core/Inc/FreeRTOSConfig.h

@@ -72,7 +72,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)0x20000)
+#define configTOTAL_HEAP_SIZE                    ((size_t)0x10000)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configGENERATE_RUN_TIME_STATS            1
 #define configUSE_TRACE_FACILITY                 1

Core/Src/usart.c

@@ -48,7 +48,7 @@ void MX_UART5_Init(void)
   huart5.Instance = UART5;
   huart5.Init.BaudRate = 100000;
   huart5.Init.WordLength = UART_WORDLENGTH_9B;
-  huart5.Init.StopBits = UART_STOPBITS_1;
+  huart5.Init.StopBits = UART_STOPBITS_2;
   huart5.Init.Parity = UART_PARITY_EVEN;
   huart5.Init.Mode = UART_MODE_RX;
   huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE;

CtrBoard-H7_ALL.ioc

@@ -587,7 +587,7 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
 ProjectManager.ProjectStructure=
 ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x2000
-ProjectManager.TargetToolchain=MDK-ARM V5.32
+ProjectManager.TargetToolchain=CMake
 ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
@@ -748,9 +748,10 @@ TIM3.IPParameters=Channel-PWM Generation4 CH4,Prescaler,Period,AutoReloadPreload
 TIM3.Period=10000-1
 TIM3.Prescaler=24-1
 UART5.BaudRate=100000
-UART5.IPParameters=Mode,WordLength,Parity,BaudRate
+UART5.IPParameters=Mode,WordLength,Parity,BaudRate,StopBits
 UART5.Mode=MODE_RX
 UART5.Parity=PARITY_EVEN
+UART5.StopBits=UART_STOPBITS_2
 UART5.WordLength=WORDLENGTH_9B
 UART7.BaudRate=921600
 UART7.IPParameters=BaudRate

User/module/balance_chassis.c

@@ -86,6 +86,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
   PID_Reset(&c->pid.leg_length[1]);
   PID_Reset(&c->pid.yaw);
   PID_Reset(&c->pid.roll);
+  PID_Reset(&c->pid.roll_force);
   PID_Reset(&c->pid.tp[0]);
   PID_Reset(&c->pid.tp[1]);
 
@@ -415,6 +416,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   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.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
   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);
 
@@ -805,26 +807,66 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
 
   /* ==================== 横滚角补偿 ==================== */
-  float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
+  /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
-                                      c->feedback.imu.gyro.x, c->dt);
+  float Rl = c->param->hip_width / 2.0f;  /* 髋宽的一半 */
-
+  float roll_error = c->feedback.imu.euler.rol - 0.0f;  /* Roll角误差 */
-  /* 限制补偿范围 */
+  float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0;  /* 右腿减左腿腿长差 */
-  float max_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
+  
-                               ? 0.05f : 0.02f;
+  float A = Delta_L0 * cosf(roll_error) + 2.0f * Rl * sinf(roll_error);
-  roll_compensation = LIMIT(roll_compensation, -max_compensation, max_compensation);
+  float B = -Delta_L0 * sinf(roll_error) + 2.0f * Rl * cosf(roll_error);
+  
+  float roll_leg_compensation_left = 0.0f;
+  float roll_leg_compensation_right = 0.0f;
+  
+  if (fabsf(B) > 0.001f) {  /* 避免除零 */
+    float tan_delta = A / B;
+    roll_leg_compensation_left = -Rl * tan_delta;
+    roll_leg_compensation_right = Rl * tan_delta;
+  }
+  
+  /* 限制几何补偿范围 (适当提高限幅增强补偿效果) */
+  float max_leg_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
+                               ? 0.12f : 0.0f;  /* 提高到0.05/0.025以提供足够补偿 */
+  roll_leg_compensation_left = LIMIT(roll_leg_compensation_left, -max_leg_compensation, max_leg_compensation);
+  roll_leg_compensation_right = LIMIT(roll_leg_compensation_right, -max_leg_compensation, max_leg_compensation);
+  
+  /* 方法2: PID力补偿 (低增益避免抖动) */
+  float roll_force_compensation = PID_Calc(&c->pid.roll_force, 0.0f, c->feedback.imu.euler.rol,
+                                            c->feedback.imu.gyro.x, c->dt);
 
   /* ==================== 腿长控制 ==================== */
   float target_L0[2];
   float jump_extra_force[2] = {0.0f, 0.0f};
   
-  /* 基础腿长目标 */
+  /* 基础腿长目标 (应用几何前馈补偿) */
-  target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
+  float base_leg_length = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE;
-  target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
+  target_L0[0] = base_leg_length + roll_leg_compensation_left;
+  target_L0[1] = base_leg_length + roll_leg_compensation_right;
 
   /* 跳跃控制：可能会修改目标腿长和额外力 */
   Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);
 
-  /* 腿长限幅 */
+  /* 智能限幅：短腿不够时将补偿转移到长腿 */
+  float compensation_transfer = 0.0f;
+  
+  /* 检查哪条腿更短（需要缩短的那条） */
+  if (target_L0[0] < target_L0[1]) {
+    /* 左腿更短，检查是否触碰下限 */
+    if (target_L0[0] < MIN_LEG_LENGTH) {
+      compensation_transfer = MIN_LEG_LENGTH - target_L0[0];  /* 计算短缺量 */
+      target_L0[0] = MIN_LEG_LENGTH;  /* 左腿限制在最小值 */
+      target_L0[1] += compensation_transfer;  /* 右腿补偿增加 */
+    }
+  } else if (target_L0[1] < target_L0[0]) {
+    /* 右腿更短，检查是否触碰下限 */
+    if (target_L0[1] < MIN_LEG_LENGTH) {
+      compensation_transfer = MIN_LEG_LENGTH - target_L0[1];  /* 计算短缺量 */
+      target_L0[1] = MIN_LEG_LENGTH;  /* 右腿限制在最小值 */
+      target_L0[0] += compensation_transfer;  /* 左腿补偿增加 */
+    }
+  }
+  
+  /* 最终安全限幅 */
   target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
   target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
 
@@ -855,8 +897,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
     virtual_torque_left = 0.0f;  /* 收腿时不控制摆角 */
   } else {
+    /* 正常状态：添加Roll力补偿 (低增益避免抖动) */
     virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
-                          pid_output_left + LEFT_BASE_FORCE - spring_force_left + jump_extra_force[0];
+                          pid_output_left + LEFT_BASE_FORCE - spring_force_left + 
+                          jump_extra_force[0] + roll_force_compensation;
     virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
   }
 
@@ -880,8 +924,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
     virtual_torque_right = 0.0f;  /* 收腿时不控制摆角 */
   } else {
+    /* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */
     virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
-                           pid_output_right + RIGHT_BASE_FORCE - spring_force_right + jump_extra_force[1];
+                           pid_output_right + RIGHT_BASE_FORCE - spring_force_right + 
+                           jump_extra_force[1] - roll_force_compensation;
     virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
   }
 

User/module/balance_chassis.h

@@ -90,7 +90,8 @@ typedef struct {
   LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
   
   KPID_Params_t yaw;               /* yaw轴PID控制参数，用于控制底盘朝向 */
-  KPID_Params_t roll;              /* roll轴PID控制参数，用于横滚角补偿 */
+  KPID_Params_t roll;              /* roll轴腿长补偿PID控制参数 */
+  KPID_Params_t roll_force;        /* roll轴力补偿PID控制参数 */
   KPID_Params_t tp;          /* 摆力矩PID控制参数，用于控制摆力矩 */
   KPID_Params_t leg_length;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
   KPID_Params_t leg_theta;          /* 摆角PID控制参数，用于控制虚拟腿摆角 */
@@ -107,6 +108,7 @@ typedef struct {
   } jump_params;
 
   float joint_zero[4]; /* 关节电机零点偏移位置 */
+  float hip_width;     /* 髋宽，两腿间距 (m) */
 
   float mech_zero_yaw; /* 机械零点 */
 
@@ -179,7 +181,8 @@ typedef struct {
   /* 反馈控制用的PID */
   struct {
     KPID_t yaw; /* 跟随云台用的PID */
-    KPID_t roll;    /* 横滚角控制PID */
+    KPID_t roll;    /* 横滚角腿长补偿PID */
+    KPID_t roll_force; /* 横滚角力补偿PID */
     KPID_t tp[2];
     KPID_t leg_length[2]; /* 两条腿的腿长PID */
     KPID_t leg_theta[2]; /* 两条腿的摆角PID */

User/module/config.c

@@ -191,12 +191,23 @@ Config_RobotParam_t robot_config = {
         },
         
         .roll={
-            .k=0.2f,
+            .k=0.15f,
-            .p=0.5f,           /* 横滚角比例系数 */
+            .p=0.3f,           /* 横滚角腿长补偿比例系数 (降低避免抖动) */
-            .i=0.01f,           /* 横滚角积分系数 */
+            .i=0.0f,           /* 横滚角腿长补偿积分系数 (关闭避免震荡) */
-            .d=0.01f,           /* 横滚角微分系数 */
+            .d=0.01f,           /* 横滚角腿长补偿微分系数 (关闭避免噪声放大) */
-            .i_limit=0.2f,     /* 积分限幅 */
+            .i_limit=0.0f,     /* 积分限幅 */
-            .out_limit=1.0f,  /* 输出限幅，腿长差值限制 */
+            .out_limit=0.03f,  /* 输出限幅，腿长差值限制(m) 降低避免过度补偿 */
+            .d_cutoff_freq=30.0f, /* 微分截止频率 */
+            .range=-1.0f,      /* 不使用循环误差处理 */
+        },
+        
+        .roll_force={
+            .k=5.0f,
+            .p=10.0f,          /* 横滚角力补偿比例系数 (大幅降低避免抖动) */
+            .i=0.0f,           /* 横滚角力补偿积分系数 */
+            .d=5.0f,           /* 横滚角力补偿微分系数 (降低避免高频抖动) */
+            .i_limit=0.0f,     /* 积分限幅 */
+            .out_limit=20.0f,  /* 输出限幅，力补偿限制(N) 降低避免过度补偿 */
             .d_cutoff_freq=30.0f, /* 微分截止频率 */
             .range=-1.0f,      /* 不使用循环误差处理 */
         },
@@ -204,8 +215,8 @@ Config_RobotParam_t robot_config = {
         .leg_length={
             .k = 40.0f,
             .p = 30.0f,
-            .i = 0.01f,
+            .i = 0.0f,
-            .d = 2.0f,
+            .d = 1.5f,
             .i_limit = 0.0f,
             .out_limit = 200.0f,
             .d_cutoff_freq = -1.0f,
@@ -299,6 +310,7 @@ Config_RobotParam_t robot_config = {
 
         .mech_zero_yaw = 1.78040409f, /* 机械零点 */
         .joint_zero = {0.0f, 0.0f, 0.0f, 0.0f}, /* 关节电机零点偏移位置 */
+        .hip_width = 0.423f,  /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
                 .vmc_param = {
             { // 左腿
                 .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)

User/task/user_task.c

@@ -12,7 +12,7 @@ const osThreadAttr_t attr_init = {
 const osThreadAttr_t attr_rc = {
     .name = "rc",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 4,
+    .stack_size = 256 * 8,
 };
 const osThreadAttr_t attr_atti_esit = {
     .name = "atti_esit",
@@ -22,7 +22,7 @@ const osThreadAttr_t attr_atti_esit = {
 const osThreadAttr_t attr_ctrl_chassis = {
     .name = "ctrl_chassis",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 4,
+    .stack_size = 256 * 8,
 };
 const osThreadAttr_t attr_ctrl_gimbal = {
     .name = "ctrl_gimbal",

User/task/user_task.h

@@ -24,7 +24,7 @@ extern "C" {
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
-#define RC_INIT_DELAY (0)
+#define RC_INIT_DELAY (500u)
 #define ATTI_ESIT_INIT_DELAY (0)
 #define CTRL_CHASSIS_INIT_DELAY (0)
 #define CTRL_GIMBAL_INIT_DELAY (0)