Merge branch 'MC_02' of http://gitea.qutrobot.top/robofish/rm_balance into MC_02

User/module/balance_chassis.c

@@ -31,6 +31,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c);
 static void Chassis_ResetControllers(Chassis_t *c);
 static void Chassis_SelectYawZeroPoint(Chassis_t *c);
 static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
+static void Chassis_RecoverControl(Chassis_t *c);
 
 /* Private functions -------------------------------------------------------- */
 
@@ -77,7 +78,6 @@ static void Chassis_ResetControllers(Chassis_t *c) {
   PID_Reset(&c->pid.leg_length[0]);
   PID_Reset(&c->pid.leg_length[1]);
   PID_Reset(&c->pid.yaw);
-  PID_Reset(&c->pid.rotor);
   PID_Reset(&c->pid.roll);
   PID_Reset(&c->pid.roll_force);
   PID_Reset(&c->pid.tp[0]);
@@ -144,13 +144,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
     return CHASSIS_OK;
   }
 
-  /* ROTOR -> BALANCE：启动退出过渡（减速→对齐），不立即切换 */
-  if (c->mode == CHASSIS_MODE_BALANCE_ROTOR &&
-      mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
-    c->rotor_state.exiting = true;
-    return CHASSIS_OK;  /* 保持在 ROTOR 模式，由控制循环处理退出 */
-  }
-
   Chassis_MotorEnable(c);
   Chassis_ResetControllers(c);
   Chassis_SelectYawZeroPoint(c);
@@ -179,12 +172,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
     c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
   }
 
-  /* 进入小陀螺时重置旋转状态 */
-  if (mode == CHASSIS_MODE_BALANCE_ROTOR) {
-    c->rotor_state.current_spin_speed = 0.0f;
-    c->rotor_state.exiting = false;
-  }
-
   c->mode = mode;
   return CHASSIS_OK;
 }
@@ -468,7 +455,6 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   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.rotor, KPID_MODE_CALC_D, target_freq, &param->rotor);
   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);
@@ -557,13 +543,6 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   c->jump.launch_force = c->param->jump.launch_force;
   c->jump.retract_leg_length = c->param->jump.retract_leg_length;
 
-  /* 初始化电机离线检测 */
-  c->motor_status.any_offline = false;
-
-  /* 初始化小陀螺状态 */
-  c->rotor_state.current_spin_speed = 0.0f;
-  c->rotor_state.exiting = false;
-
   return CHASSIS_OK;
 }
 
@@ -580,19 +559,10 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
   MOTOR_LK_UpdateAll();
 
   /* 更新关节电机反馈 */
-  uint64_t now_us = BSP_TIME_Get_us();
-  bool any_offline = false;
-
   for (int i = 0; i < 4; i++) {
     MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
     if (joint_motor != NULL) {
       c->feedback.joint[i] = joint_motor->motor.feedback;
-      /* 检测关节电机离线：500ms 无更新 */
-      if (now_us - joint_motor->motor.header.last_online_time > 500000u) {
-        any_offline = true;
-      }
-    } else {
-      any_offline = true;
     }
 
     /* 机械零点调整 */
@@ -610,17 +580,9 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
     MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
     if (wheel_motor != NULL) {
       c->feedback.wheel[i] = wheel_motor->motor.feedback;
-      /* 检测轮子电机离线：500ms 无更新 */
-      if (now_us - wheel_motor->motor.header.last_online_time > 500000u) {
-        any_offline = true;
-      }
-    } else {
-      any_offline = true;
     }
   }
 
-  c->motor_status.any_offline = any_offline;
-
   /* 更新机体状态估计 */
   Chassis_UpdateChassisState(c);
 
@@ -653,17 +615,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
   c->lask_wakeup = BSP_TIME_Get_us();
 
-  /* 电机离线保护：有电机离线时强制 RELAX 并持续使能 */
-  if (c->motor_status.any_offline) {
-    if (c->mode != CHASSIS_MODE_RELAX) {
-      c->mode = CHASSIS_MODE_RELAX;
-      Chassis_ResetControllers(c);
-    }
-    Chassis_MotorRelax(c);
-    Chassis_MotorEnable(c);
-    return CHASSIS_OK;
-  }
-
   /* 设置底盘模式 */
   if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
     return CHASSIS_ERR_MODE;
@@ -715,6 +666,8 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       break;
     case CHASSIS_MODE_BALANCE_ROTOR:
       Chassis_LQRControl(c, c_cmd);
+      c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
+      c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
       Chassis_VMCControl(c, c_cmd);
       Chassis_Output(c);
       break;
@@ -770,18 +723,21 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
 
   /* ==================== 速度控制 ==================== */
-  if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR) {
-    /* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */
-    float yaw_angle = c->feedback.imu.euler.yaw;
-    float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed;
-    c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle);
-    c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
-    c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
-  } else {
-    c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
-    c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
-    c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
-  }
+  // float raw_vx = c_cmd->move_vec.vx * 2.0f;
+  // float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
+
+  // /* 加速度限制 */
+  // float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
+  // float max_velocity_change = c->param->motion.max_acceleration * c->dt;
+  // velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
+
+  // float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
+  // c->chassis_state.target_velocity_x = target_dot_x;
+  // c->chassis_state.last_target_velocity_x = target_dot_x;
+  // c->chassis_state.target_x += target_dot_x * c->dt;
+  c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
+  c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
+  c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
 
 
   /* ==================== 状态更新 ==================== */
@@ -805,71 +761,17 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
 
   /* ==================== 目标状态 ==================== */
-  /* 腿长-位移补偿：根据腿长多项式拟合补偿位移偏移 */
-  float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
-  float leg_x_comp = -45.406f * avg_L0 * avg_L0 * avg_L0
-                   +  7.06585f * avg_L0 * avg_L0
-                   +  5.98465f * avg_L0
-                   -  1.14975f;
-
   LQR_State_t target_state = {
       .theta = 0.0f+c->param->lqr_offset.theta,
       .d_theta = 0.0f,
       .phi = 0.0f+c->param->lqr_offset.phi,
       .d_phi = 0.0f,
-      .x = c->chassis_state.target_x+c->param->lqr_offset.x + leg_x_comp,
+      .x = c->chassis_state.target_x+c->param->lqr_offset.x,
       .d_x = c->chassis_state.target_velocity_x,
   };
 
   /* ==================== Yaw轴控制 ==================== */
-  if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) {
-    /* 小陀螺模式：PID跟踪目标角速度，带速度斜坡 */
-    float target_spin = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_spin_speed;
-
-    if (c->rotor_state.exiting) {
-      /* 退出过渡：目标角速度为0，用减速斜坡 */
-      target_spin = 0.0f;
-      float decel_step = c->param->rotor_ctrl.spin_decel * c->dt;
-      if (c->rotor_state.current_spin_speed > decel_step) {
-        c->rotor_state.current_spin_speed -= decel_step;
-      } else if (c->rotor_state.current_spin_speed < -decel_step) {
-        c->rotor_state.current_spin_speed += decel_step;
-      } else {
-        /* 减速完成，检查yaw对齐 */
-        c->rotor_state.current_spin_speed = 0.0f;
-        Chassis_SelectYawZeroPoint(c);
-        float yaw_error = CircleError(c->yaw_control.target_yaw,
-                                      c->feedback.yaw.rotor_abs_angle, M_2PI);
-        if (fabsf(yaw_error) < c->param->rotor_ctrl.align_threshold) {
-          /* 对齐完成，切换到平衡模式 */
-          c->rotor_state.exiting = false;
-          c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-          Chassis_ResetControllers(c);
-        }
-      }
-    } else {
-      /* 正常旋转：平滑加速斜坡 */
-      float accel_step = c->param->rotor_ctrl.spin_accel * c->dt;
-      if (c->rotor_state.current_spin_speed < target_spin - accel_step) {
-        c->rotor_state.current_spin_speed += accel_step;
-      } else if (c->rotor_state.current_spin_speed > target_spin + accel_step) {
-        c->rotor_state.current_spin_speed -= accel_step;
-      } else {
-        c->rotor_state.current_spin_speed = target_spin;
-      }
-    }
-
-    /* 退出过渡减速完成后，用yaw PID对齐零点 */
-    if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) {
-      c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
-                                          c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
-    } else {
-      float current_yaw_omega = c->feedback.imu.gyro.z;
-      c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
-                                          current_yaw_omega, 0.0f, c->dt);
-    }
-    c->yaw_control.is_reversed = false;
-  } else {
+  if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) {
     c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
 
     float manual_offset = c_cmd->move_vec.vy * M_PI_2;
@@ -921,6 +823,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
 
   /* ==================== 轮毂输出 ==================== */
   /* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */
+  float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
   float yaw_scale = c->param->leg.base_length / avg_L0;  /* 以基础腿长为基准归一化 */
   float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale;
 

User/module/balance_chassis.h

@@ -103,7 +103,6 @@ typedef struct {
   LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
   
   KPID_Params_t yaw;               /* yaw轴PID控制参数 */
-  KPID_Params_t rotor;             /* 小陀螺角速度PID */
   KPID_Params_t roll;              /* roll轴腿长补偿PID */
   KPID_Params_t roll_force;        /* roll轴力补偿PID */
   KPID_Params_t tp;                /* 摆力矩PID */
@@ -147,15 +146,6 @@ typedef struct {
     float non_contact_theta;       /* 离地时摆角目标 (rad) */
   } motion;
 
-  /* 小陀螺参数 */
-  struct {
-    float max_spin_speed;          /* 最大旋转角速度 (rad/s) */
-    float max_trans_speed;         /* 小陀螺时最大平移速度 (m/s) */
-    float spin_accel;              /* 旋转加速度 (rad/s²) */
-    float spin_decel;              /* 退出减速度 (rad/s²) */
-    float align_threshold;         /* 退出对齐阈值 (rad)，yaw误差小于此值时回到BALANCE模式 */
-  } rotor_ctrl;
-
   /* LQR 目标状态偏移 */
   struct {
     float theta;
@@ -210,12 +200,6 @@ typedef struct {
     bool is_reversed;     /* 是否使用反转的零点（180度零点），影响前后方向 */
   } yaw_control;
 
-  /* 小陀螺状态 */
-  struct {
-    float current_spin_speed;  /* 当前旋转速度 (rad/s)，含斜坡 */
-    bool exiting;              /* 正在退出小陀螺 */
-  } rotor_state;
-
   /* 跳跃控制相关 */
   struct {
     Jump_State_t state;       /* 跳跃状态 */
@@ -231,11 +215,6 @@ typedef struct {
     Recover_State_t state[2]; /* 左右腿自起状态 */
   } recover;
 
-  /* 电机离线检测 */
-  struct {
-    bool any_offline;        /* 是否有电机离线 */
-  } motor_status;
-
   /* PID计算的目标值 */
   struct {
     AHRS_Eulr_t chassis;
@@ -244,7 +223,6 @@ typedef struct {
   /* 反馈控制用的PID */
   struct {
     KPID_t yaw; /* 跟随云台用的PID */
-    KPID_t rotor; /* 小陀螺角速度PID */
     KPID_t roll;    /* 横滚角腿长补偿PID */
     KPID_t roll_force; /* 横滚角力补偿PID */
     KPID_t tp[2];

User/module/config.c

@@ -243,17 +243,6 @@ Config_RobotParam_t robot_config = {
             .range=M_2PI, /* 2*PI，用于处理角度循环误差 */
         },
         
-        .rotor={
-            .k=1.0f,
-            .p=0.5f,
-            .i=0.0f,
-            .d=0.01f,
-            .i_limit=0.0f,
-            .out_limit=1.0f,
-            .d_cutoff_freq=30.0f,
-            .range=-1.0f, /* 角速度不需要循环误差处理 */
-        },
-        
         .roll={
             .k=0.15f,
             .p=0.3f,           /* 横滚角腿长补偿比例系数 (降低避免抖动) */
@@ -394,14 +383,6 @@ Config_RobotParam_t robot_config = {
             .non_contact_theta = -0.01f,    /* 离地摆角目标 (rad) */
         },
 
-        .rotor_ctrl = {
-            .max_spin_speed = 0.05236f,    /* 最大旋转角速度 (rad/s)，0.5rpm */
-            .max_trans_speed = 1.5f,       /* 小陀螺时最大平移速度 (m/s) */
-            .spin_accel = 5.0f,            /* 旋转加速度 (rad/s²) */
-            .spin_decel = 8.0f,            /* 退出减速度 (rad/s²)，比加速更快 */
-            .align_threshold = 0.15f,      /* 退出对齐阈值 (rad)，约8.6° */
-        },
-
         .vmc_param = {
             { // 左腿
                 .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)