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

add上台阶

Browse Source
This commit is contained in:
Robofish 2026-03-15 19:28:43 +08:00
parent 6b59d5922d
commit 1e7fcdc20b
3 changed files with 151 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
User/module/balance_chassis.c
View File

@ -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_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/* Private functions -------------------------------------------------------- */
@ -138,6 +139,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (mode == c->mode) return CHASSIS_OK;
/* 上台阶已完成，忽略重复的CLIMB_STEP请求，需先切别的模式再回来 */
if (mode == CHASSIS_MODE_CLIMB_STEP && c->climb.completed) {
return CHASSIS_OK;
}
/* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */
if (c->mode == CHASSIS_MODE_RECOVER &&
mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
@ -180,6 +186,19 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
c->rotor_state.exiting = false;
}
/* 进入上台阶模式：重置状态机 */
if (mode == CHASSIS_MODE_CLIMB_STEP) {
c->climb.state = CLIMB_EXTEND;
c->climb.completed = false;
c->climb.state_start_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
}
/* 离开上台阶模式时重置completed标志 */
if (c->mode == CHASSIS_MODE_CLIMB_STEP && mode != CHASSIS_MODE_CLIMB_STEP) {
c->climb.completed = false;
c->climb.state = CLIMB_IDLE;
}
c->mode = mode;
return CHASSIS_OK;
}
@ -419,6 +438,100 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float
}
}
/**
* @brief 上台阶控制
* @param c 底盘结构体指针
* @param c_cmd 控制指令
*
* @note 流程：
* EXTEND(伸腿前进，削弱前摆力) -> RETRACT(腿后摆超阈值，快速收腿) -> SETTLE(稳定) -> 自动退出到BALANCE
* 单次执行：完成后即使CMD继续发CLIMB_STEP也不重复，需切换到其他模式后再回来
*/
static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
uint32_t elapsed_time = current_time - c->climb.state_start_time;
/* 已完成一次上台阶，自动切回BALANCE */
if (c->climb.completed) {
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
Chassis_ResetControllers(c);
return;
}
/* 上台阶状态机中始终运行LQR+VMC保持平衡 */
Chassis_CMD_t climb_cmd = *c_cmd;
switch (c->climb.state) {
case CLIMB_IDLE:
/* 不应到达此状态，进入CLIMB_STEP时会设为EXTEND */
c->climb.state = CLIMB_EXTEND;
c->climb.state_start_time = current_time;
break;
case CLIMB_EXTEND: {
/* 伸腿前进：腿长最大，给定前进速度，削弱摆力矩让腿自由后摆 */
climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f;
climb_cmd.height = 1.0f; /* 最大腿长 */
/* 运行LQR和VMC */
Chassis_LQRControl(c, &climb_cmd);
/* 削弱LQR的摆力矩Tp，让腿撞到台阶后能自由后摆 */
c->lqr[0].control_output.Tp *= c->param->climb.tp_scale;
c->lqr[1].control_output.Tp *= c->param->climb.tp_scale;
Chassis_VMCControl(c, &climb_cmd);
Chassis_Output(c);
/* 检查腿后摆角度：任一腿theta超过阈值则收腿 */
float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
if (fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
c->climb.state = CLIMB_RETRACT;
c->climb.state_start_time = current_time;
}
/* 超时保护：10秒未触发收腿则放弃 */
if (elapsed_time > 10000) {
c->climb.completed = true;
}
} break;
case CLIMB_RETRACT: {
/* 快速收腿，继续前进 */
climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f;
climb_cmd.height = -1.0f; /* 最小腿长 */
Chassis_LQRControl(c, &climb_cmd);
Chassis_VMCControl(c, &climb_cmd);
Chassis_Output(c);
/* 腿收到位或超时，进入稳定阶段 */
bool legs_retracted = (c->vmc_[0].leg.L0 < c->param->leg.min_length + 0.02f) &&
(c->vmc_[1].leg.L0 < c->param->leg.min_length + 0.02f);
if (legs_retracted || elapsed_time > 1000) {
c->climb.state = CLIMB_SETTLE;
c->climb.state_start_time = current_time;
}
} break;
case CLIMB_SETTLE:
/* 稳定阶段：恢复正常控制，等待稳定 */
climb_cmd.height = 0.0f;
Chassis_LQRControl(c, &climb_cmd);
Chassis_VMCControl(c, &climb_cmd);
Chassis_Output(c);
if (elapsed_time > c->param->climb.settle_time_ms) {
c->climb.completed = true;
}
break;
default:
c->climb.completed = true;
break;
}
}
/**
* @brief 初始化底盘
@ -715,6 +828,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_Output(c);
break;
case CHASSIS_MODE_CLIMB_STEP:
Chassis_ClimbControl(c, c_cmd);
break;
default:
return CHASSIS_ERR_MODE;
}

27
User/module/balance_chassis.h
View File

@ -42,7 +42,8 @@ typedef enum {
CHASSIS_MODE_RECOVER, /* 复位模式 */
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
CHASSIS_MODE_CLIMB_STEP /* 上台阶模式 */
} Chassis_Mode_t;
/* 跳跃状态枚举 */
@ -62,11 +63,20 @@ typedef enum {
RECOVER_COMPLETE /* 完成：切换到平衡模式 */
} Recover_State_t;
/* 上台阶状态枚举 */
typedef enum {
CLIMB_IDLE, /* 待机，未触发 */
CLIMB_EXTEND, /* 伸腿前进：腿长最大，削弱前摆力，等待撞到台阶 */
CLIMB_RETRACT, /* 收腿：腿后摆超过阈值后快速收腿 */
CLIMB_SETTLE /* 稳定：收腿完成，短暂稳定后退出 */
} Climb_State_t;
typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
float height; /* 目标高度 */
bool jump_trigger; /* 跳跃触发标志 */
bool climb_trigger; /* 上台阶触发标志 */
} Chassis_CMD_t;
typedef struct {
@ -156,6 +166,14 @@ typedef struct {
float align_threshold; /* 退出对齐阈值 (rad)，yaw误差小于此值时回到BALANCE模式 */
} rotor_ctrl;
/* 上台阶参数 */
struct {
float forward_speed; /* 前进速度 (m/s) */
float theta_retract_threshold; /* 腿后摆收腿阈值 (rad)，约30° = 0.52 */
float tp_scale; /* 摆力矩缩放系数，<1削弱前摆力 */
uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */
} climb;
/* LQR 目标状态偏移 */
struct {
float theta;
@ -231,6 +249,13 @@ typedef struct {
Recover_State_t state[2]; /* 左右腿自起状态 */
} recover;
/* 上台阶控制相关 */
struct {
Climb_State_t state; /* 上台阶状态 */
bool completed; /* 本次已完成标志，防止重复触发 */
uint32_t state_start_time; /* 当前状态开始时间 (ms) */
} climb;
/* 电机离线检测 */
struct {
bool any_offline; /* 是否有电机离线 */

9
User/module/config.c
View File

@ -448,6 +448,13 @@ Config_RobotParam_t robot_config = {
.retract_force = -20.0f, /* 收腿前馈力减小 */
},
.climb = {
.forward_speed = 1.0f, /* 上台阶前进速度 (m/s) */
.theta_retract_threshold = 0.52f, /* 腿后摆收腿阈值 (rad)，约30° */
.tp_scale = 0.1f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */
.settle_time_ms = 500, /* 收腿后稳定时间 (ms) */
},
.lqr_offset = {
.theta = 0.0f,
.x = 0.0f,
@ -485,7 +492,7 @@ Config_RobotParam_t robot_config = {
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX,
.balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
.balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
.balance_sw_down = CHASSIS_MODE_CLIMB_STEP,
#endif
},
},