@ -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）
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 ;
@ -884,11 +762,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 目标状态 ==================== */
LQR_State_t target_state = {
. theta = 0.0f ,
. theta = 0.0f + c - > param - > lqr_offset . theta ,
. d_theta = 0.0f ,
. phi = 0.0f ,
. phi = 0.0f + c - > param - > lqr_offset . phi ,
. d_phi = 0.0f ,
. x = c - > chassis_state . target_x ,
. x = c - > chassis_state . target_x + c - > param - > lqr_offset . x ,
. d_x = c - > chassis_state . target_velocity_x ,
} ;
@ -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 ; /* 髋宽的一半 */
