add上台阶
This commit is contained in:
parent
6b59d5922d
commit
1e7fcdc20b
@ -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;
|
||||
}
|
||||
|
||||
@ -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; /* 是否有电机离线 */
|
||||
|
||||
@ -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
|
||||
},
|
||||
},
|
||||
|
||||
Loading…
Reference in New Issue
Block a user