robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

测测recover

Browse Source
This commit is contained in:
Robofish 2026-03-16 00:00:48 +08:00
parent ec50f074d5
commit 632cd81ddb
3 changed files with 140 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
User/module/balance_chassis.c
View File

@ -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:

10
User/module/balance_chassis.h
View File

@ -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;
/* 上台阶控制相关 */

8
User/module/config.c
View File

@ -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,