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

fix ju

Browse Source
This commit is contained in:
Robofish 2026-03-15 20:26:28 +08:00
parent 1e7fcdc20b
commit 9cd6b85e93
1 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
User/module/balance_chassis.c
View File

@ -137,6 +137,14 @@ static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
*/
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL) return CHASSIS_ERR_NULL;
/* 自动退回BALANCE后，只要用户切到非上台阶模式，就允许下次重新触发 */
if (mode != CHASSIS_MODE_CLIMB_STEP && c->climb.completed) {
c->climb.completed = false;
c->climb.state = CLIMB_IDLE;
c->climb.state_start_time = 0;
}
if (mode == c->mode) return CHASSIS_OK;
/* 上台阶已完成，忽略重复的CLIMB_STEP请求，需先切别的模式再回来 */
@ -197,6 +205,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c->mode == CHASSIS_MODE_CLIMB_STEP && mode != CHASSIS_MODE_CLIMB_STEP) {
c->climb.completed = false;
c->climb.state = CLIMB_IDLE;
c->climb.state_start_time = 0;
}
c->mode = mode;
@ -469,9 +478,10 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
break;
case CLIMB_EXTEND: {
/* 伸腿前进：腿长最大，给定前进速度，削弱摆力矩让腿自由后摆 */
climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f;
climb_cmd.height = 1.0f; /* 最大腿长 */
/* 伸腿前进采用斜坡，避免切换模式时瞬间猛站 */
float extend_ratio = LIMIT((float)elapsed_time / 500.0f, 0.0f, 1.0f);
climb_cmd.move_vec.vx = (c->param->climb.forward_speed / 2.0f) * extend_ratio;
climb_cmd.height = extend_ratio;
/* 运行LQR和VMC */
Chassis_LQRControl(c, &climb_cmd);
@ -483,9 +493,12 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_VMCControl(c, &climb_cmd);
Chassis_Output(c);
/* 检查腿后摆角度：任一腿theta超过阈值则收腿 */
/* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */
bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
(c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
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) {
if (elapsed_time > 300 && legs_extended &&
fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
c->climb.state = CLIMB_RETRACT;
c->climb.state_start_time = current_time;
}
@ -672,6 +685,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->rotor_state.current_spin_speed = 0.0f;
c->rotor_state.exiting = false;
/* 初始化上台阶状态 */
c->climb.state = CLIMB_IDLE;
c->climb.completed = false;
c->climb.state_start_time = 0;
return CHASSIS_OK;
}