From 632cd81ddb0582710b2d0cda6054965d15812cad Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 16 Mar 2026 00:00:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E6=B5=8Brecover?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 130 ++++++++++++++++++++++++++++++++-- User/module/balance_chassis.h | 10 +++ User/module/config.c | 8 ++- 3 files changed, 140 insertions(+), 8 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 09f22eb..1257a02 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -32,6 +32,7 @@ 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_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); +static void Chassis_RecoverControl(Chassis_t *c); /* Private functions -------------------------------------------------------- */ @@ -169,15 +170,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { Chassis_ResetControllers(c); Chassis_SelectYawZeroPoint(c); - /* RELAX -> BALANCE：pitch过大时保持RELAX，避免进入未实现的RECOVER */ + /* RELAX -> BALANCE：pitch过大时自动进入RECOVER自起 */ if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) { float pitch = c->feedback.imu.euler.pit; if (fabsf(pitch) > 0.8f) { - /* pitch过大，机体未扶正，保持RELAX */ - Chassis_MotorRelax(c); - return CHASSIS_OK; + /* pitch过大，机体倒地，进入自起模式 */ + mode = CHASSIS_MODE_RECOVER; } - /* 否则直接进入平衡模式 */ } /* 直接进入 RECOVER 时也重置状态机 */ @@ -186,6 +185,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { float theta_r = c->vmc_[1].leg.theta; c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; + c->recover.state_start_time = (uint32_t)(BSP_TIME_Get_us() / 1000); } /* 进入小陀螺时重置旋转状态 */ @@ -559,6 +559,111 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } +/** + * @brief 倒地自起控制 + * @param c 底盘结构体指针 + * @note 三阶段状态机: FLIP→STRETCH→BACKLEG→COMPLETE + * 使用MIT模式(kp=0)做速度控制，轮子全程关闭 + */ +static void Chassis_RecoverControl(Chassis_t *c) { + uint32_t now = (uint32_t)(BSP_TIME_Get_us() / 1000); + uint32_t elapsed = now - c->recover.state_start_time; + + /* 构建MIT速度控制参数模板 */ + MOTOR_LZ_MotionParam_t vel_cmd = { + .torque = 0.0f, + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = c->param->recover.kd, + }; + + /* 轮子全程关闭 */ + MOTOR_LK_Relax(&c->param->wheel_param[0]); + MOTOR_LK_Relax(&c->param->wheel_param[1]); + + float stretch_vel = c->param->recover.stretch_velocity; + Recover_State_t state = c->recover.state[0]; /* 两腿同步 */ + + switch (state) { + case RECOVER_FLIP: { + /* 翻身阶段：腿朝上（|theta|>π/2）时，转动腿使其朝下 + * 所有关节同向旋转，使腿绕机体转动 */ + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 腿转到朝下 → 进入伸腿 */ + if ((fabsf(c->vmc_[0].leg.theta) < (float)M_PI_2 && + fabsf(c->vmc_[1].leg.theta) < (float)M_PI_2) || + elapsed > 5000) { + c->recover.state[0] = c->recover.state[1] = RECOVER_STRETCH; + c->recover.state_start_time = now; + } + } break; + + case RECOVER_STRETCH: { + /* 伸腿阶段：两关节反向 → 延伸五连杆 → 增大L0 + * 左腿: J0=+V, J1=-V; 右腿: J2=-V, J3=+V */ + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 两腿L0都超过目标 → 进入甩腿 */ + if ((c->vmc_[0].leg.L0 > c->param->recover.target_leg_length && + c->vmc_[1].leg.L0 > c->param->recover.target_leg_length) || + elapsed > 3000) { + c->recover.state[0] = c->recover.state[1] = RECOVER_BACKLEG; + c->recover.state_start_time = now; + } + } break; + + case RECOVER_BACKLEG: { + /* 甩腿阶段：两关节同向 → 推动机体立起 + * 左腿: J0=-V, J1=-V; 右腿: J2=+V, J3=+V */ + float back_vel = c->param->recover.backleg_velocity; + vel_cmd.target_velocity = -back_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + vel_cmd.target_velocity = back_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 体姿接近竖直 → 完成 */ + if (fabsf(c->feedback.imu.euler.pit) < c->param->recover.phi_tolerance || + elapsed > 5000) { + c->recover.state[0] = c->recover.state[1] = RECOVER_COMPLETE; + c->recover.state_start_time = now; + } + } break; + + case RECOVER_COMPLETE: { + /* 收腿回中：反向于伸腿，快速缩短 */ + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 短暂收腿后切回BALANCE */ + if (elapsed > 500) { + c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + Chassis_ResetControllers(c); + } + } break; + } +} + + /** * @brief 初始化底盘 * @param c 底盘结构体指针 @@ -703,6 +808,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { c->climb.completed = false; c->climb.state_start_time = 0; + /* 初始化自起状态 */ + c->recover.state[0] = RECOVER_STRETCH; + c->recover.state[1] = RECOVER_STRETCH; + c->recover.state_start_time = 0; + return CHASSIS_OK; } @@ -808,6 +918,13 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { return CHASSIS_ERR_MODE; } + /* 倒地自动检测：BALANCE/ROTOR模式下pitch过大 → 自动进入RECOVER */ + if ((c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || + c->mode == CHASSIS_MODE_BALANCE_ROTOR) && + fabsf(c->feedback.imu.euler.pit) > 1.5f) { + Chassis_SetMode(c, CHASSIS_MODE_RECOVER); + } + /* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */ if (c_cmd->jump_trigger && (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) && @@ -844,8 +961,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { break; case CHASSIS_MODE_RECOVER: - /* 自起功能未实现，安全放松电机 */ - Chassis_MotorRelax(c); + Chassis_RecoverControl(c); break; case CHASSIS_MODE_WHELL_LEG_BALANCE: diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 98d0ef3..d0f07f1 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -174,6 +174,15 @@ typedef struct { uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */ } climb; + /* 倒地自起参数 */ + struct { + float stretch_velocity; /* 伸腿角速度 (rad/s) */ + float backleg_velocity; /* 甩腿角速度 (rad/s) */ + float target_leg_length; /* 伸腿目标长度 (m) */ + float phi_tolerance; /* 体姿角到达容差 (rad) */ + float kd; /* MIT速度模式阻尼 (Nm·s/rad) */ + } recover; + /* LQR 目标状态偏移 */ struct { float theta; @@ -247,6 +256,7 @@ typedef struct { /* 自起控制相关 */ struct { Recover_State_t state[2]; /* 左右腿自起状态 */ + uint32_t state_start_time; /* 当前阶段开始时间 (ms) */ } recover; /* 上台阶控制相关 */ diff --git a/User/module/config.c b/User/module/config.c index d945449..ad087c9 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -454,7 +454,13 @@ Config_RobotParam_t robot_config = { .tp_scale = 0.08f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ .settle_time_ms = 500, /* 收腿后稳定时间 (ms) */ }, - + .recover = { + .stretch_velocity = 2.0f, /* 伸腿角速度 (rad/s) */ + .backleg_velocity = 2.0f, /* 甩腿角速度 (rad/s) */ + .target_leg_length = 0.28f, /* 伸腿目标长度 (m) */ + .phi_tolerance = 0.5f, /* 体姿接近站立容差 (rad)，约28° */ + .kd = 3.0f, /* MIT速度模式阻尼 */ + }, .lqr_offset = { .theta = 0.0f, .x = 0.0f,