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

拿出vmc

Browse Source
This commit is contained in:
Robofish 2026-03-15 09:55:19 +08:00
parent 511b9809c9
commit bc9801a60a
2 changed files with 30 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
User/module/balance_chassis.c
View File

@ -411,70 +411,6 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float
}
}
/**
* @brief 倒地自起控制
* @note 自起流程（左右腿独立状态机）：
* STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成)
* 两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式
*
* @param c 底盘结构体指针
*/
static void Chassis_RecoverControl(Chassis_t *c) {
float theta_l = c->vmc_[0].leg.theta;
float theta_r = c->vmc_[1].leg.theta;
/* ===== 左腿状态机 ===== */
switch (c->recover.state[0]) {
case RECOVER_FLIP:
/* 翻身阶段：theta朝上（|theta| > π/2），收腿慢慢翻转 */
/* 等待 theta 转到腿朝下（|theta| < π/2） */
if (fabsf(theta_l) < (float)M_PI_2) {
c->recover.state[0] = RECOVER_STRETCH;
}
break;
case RECOVER_STRETCH:
/* 伸腿：等待腿伸到位 */
if (c->vmc_[0].leg.L0 > 0.28f) {
c->recover.state[0] = RECOVER_BACKLEG;
}
break;
case RECOVER_BACKLEG:
/* 后甩腿：等待 phi0 趋近目标角度 */
if (fabsf(c->vmc_[0].leg.phi0 - 0.5f) < 0.1f) {
c->recover.state[0] = RECOVER_COMPLETE;
}
break;
case RECOVER_COMPLETE:
break;
}
/* ===== 右腿状态机 ===== */
switch (c->recover.state[1]) {
case RECOVER_FLIP:
if (fabsf(theta_r) < (float)M_PI_2) {
c->recover.state[1] = RECOVER_STRETCH;
}
break;
case RECOVER_STRETCH:
if (c->vmc_[1].leg.L0 > 0.28f) {
c->recover.state[1] = RECOVER_BACKLEG;
}
break;
case RECOVER_BACKLEG:
if (fabsf(c->vmc_[1].leg.phi0 - 0.5f) < 0.1f) {
c->recover.state[1] = RECOVER_COMPLETE;
}
break;
case RECOVER_COMPLETE:
break;
}
}
/**
* @brief 初始化底盘
@ -716,81 +652,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
case CHASSIS_MODE_RELAX:
Chassis_MotorRelax(c);
Chassis_LQRControl(c, c_cmd);
Chassis_VMCControl(c, c_cmd);
break;
case CHASSIS_MODE_RECOVER: {
/* 运行自起状态机 */
Chassis_RecoverControl(c);
/* 根据状态决定输出力 */
float fn_left, fn_right;
float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
if (isnan(spring_left)) spring_left = 0.0f;
if (isnan(spring_right)) spring_right = 0.0f;
/* 左腿 */
if (c->recover.state[0] == RECOVER_FLIP) {
fn_left = 30.0f - spring_left; /* 收腿，借重力慢慢翻转 */
} else if (c->recover.state[0] == RECOVER_STRETCH) {
fn_left = -80.0f - spring_left; /* 大力伸腿 */
} else if (c->recover.state[0] == RECOVER_BACKLEG) {
fn_left = -40.0f - spring_left; /* 维持伸腿 */
} else {
fn_left = -20.0f - spring_left;
}
/* 右腿 */
if (c->recover.state[1] == RECOVER_FLIP) {
fn_right = 30.0f - spring_right;
} else if (c->recover.state[1] == RECOVER_STRETCH) {
fn_right = -80.0f - spring_right;
} else if (c->recover.state[1] == RECOVER_BACKLEG) {
fn_right = -40.0f - spring_right;
} else {
fn_right = -20.0f - spring_right;
}
VMC_InverseSolve(&c->vmc_[0], fn_left, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
Chassis_Output(c);
/* 两腿均完成，自动切换到平衡模式 */
if (c->recover.state[0] == RECOVER_COMPLETE &&
c->recover.state[1] == RECOVER_COMPLETE) {
Chassis_ResetControllers(c);
Chassis_SelectYawZeroPoint(c);
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
}
} break;
// case CHASSIS_MODE_CALIBRATE: {
// Chassis_LQRControl(c, c_cmd);
// LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
// RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
// L_fn = -LF;
// VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
// VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
// VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
// VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
// c->output.wheel[0] = 0.0f;
// c->output.wheel[1] = 0.0f;
// Chassis_Output(c);
// } break;
// case CHASSIS_MODE_RECOVER:
// Chassis_RecoverControl(c);
// break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
Chassis_LQRControl(c, c_cmd);
Chassis_VMCControl(c, c_cmd);
Chassis_Output(c);
break;
case CHASSIS_MODE_BALANCE_ROTOR:
Chassis_LQRControl(c, c_cmd);
c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
Chassis_VMCControl(c, c_cmd);
Chassis_Output(c);
break;
@ -952,6 +830,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->output.wheel[0] = c->lqr[0].control_output.T / c->param->mech.wheel_gear_ratio + scaled_yaw_force;
c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - scaled_yaw_force;
return CHASSIS_OK;
}
/**
* @brief VMC控制（虚拟模型控制）
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @return 操作结果
* @note 将LQR输出的力矩通过VMC逆解算转换为关节力矩，
* 同时处理横滚补偿、腿长控制、跳跃控制和摆角同步
*/
int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
/* ==================== 横滚角补偿 ==================== */
/* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
float Rl = c->param->mech.hip_width / 2.0f; /* 髋宽的一半 */

10
User/module/balance_chassis.h
View File

@ -291,6 +291,16 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
*/
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief VMC控制（虚拟模型控制）
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief 底盘输出值
*