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

Compare commits

base: robofish:6480f59e1e9bb8c912bb72180b2a38f85198622c
Branches Tags
robofish:main
robofish:MC_02
robofish:ai
..
compare: robofish:c5d59282122926f37c575bbc7e380bde95ab496f
Branches Tags
robofish:MC_02
robofish:main
robofish:ai

No commits in common. "6480f59e1e9bb8c912bb72180b2a38f85198622c" and "c5d59282122926f37c575bbc7e380bde95ab496f" have entirely different histories.
6480f59e1e ... c5d5928212

10 changed files with 7561 additions and 7854 deletions
Show Stats Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

14815
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

369
User/module/balance_chassis.c
View File

@ -18,8 +18,16 @@
/* Private defines ---------------------------------------------------------- */
#define LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
/* 物理常量（不可调） */
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m)，固定机械尺寸 */
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
#define WHEEL_GEAR_RATIO 4.5f /* 轮毂电机扭矩 */
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.33f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.0f /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
@ -31,7 +39,6 @@ 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_RecoverControl(Chassis_t *c);
/* Private functions -------------------------------------------------------- */
@ -113,7 +120,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
*/
static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
float current_yaw = c->feedback.yaw.rotor_abs_angle;
float zero_point_1 = c->param->mech.mech_zero_yaw;
float zero_point_1 = c->param->mech_zero_yaw;
float zero_point_2 = zero_point_1 + M_PI;
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
@ -138,40 +145,10 @@ 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;
/* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */
if (c->mode == CHASSIS_MODE_RECOVER &&
mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
return CHASSIS_OK;
}
Chassis_MotorEnable(c);
Chassis_ResetControllers(c);
Chassis_SelectYawZeroPoint(c);
/* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */
if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) {
float pitch = c->feedback.imu.euler.pit;
/* pitch 绝对值大于 0.8rad（约46度）认为是倒地，需要自起 */
if (fabsf(pitch) > 0.2f) {
/* 根据 theta 判断腿朝上还是朝下 */
float theta_l = c->vmc_[0].leg.theta;
float theta_r = c->vmc_[1].leg.theta;
c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
c->mode = CHASSIS_MODE_RECOVER;
return CHASSIS_OK;
}
/* 否则直接进入平衡模式 */
}
/* 直接进入 RECOVER 时也重置状态机 */
if (mode == CHASSIS_MODE_RECOVER) {
float theta_l = c->vmc_[0].leg.theta;
float theta_r = c->vmc_[1].leg.theta;
c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
}
c->mode = mode;
return CHASSIS_OK;
}
@ -312,169 +289,88 @@ static float Chassis_CalcSpringForce(float leg_length)
}
/**
* @brief 跳跃控制
* @brief 跳跃控制状态机
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @param target_L0 输出的目标腿长数组[2]
* @param extra_force 输出的额外力数组[2]
*
* @note 跳跃流程：
* 触发后直接进入 LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE
*/
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) {
uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000); /* 当前时间 ms */
uint32_t elapsed_time = current_time - c->jump.state_start_time;
/* 初始化额外力为0 */
extra_force[0] = 0.0f;
extra_force[1] = 0.0f;
/* ==================== 跳跃触发检测 ==================== */
/* 触发后直接进入LAUNCH */
/* 检测跳跃触发 */
if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
c->jump.state = JUMP_LAUNCH; /* 直接进入起跳阶段 */
c->jump.state = JUMP_CROUCH;
c->jump.state_start_time = current_time;
c->jump.trigger = false;
c->jump.trigger = false; /* 清除触发标志 */
}
/* elapsed_time 必须在触发逻辑之后计算 */
uint32_t elapsed_time = current_time - c->jump.state_start_time;
/* ==================== 跳跃状态机 ==================== */
/* 跳跃状态机 */
switch (c->jump.state) {
case JUMP_IDLE:
/* 待机状态，预收腿逻辑已在上面处理 */
/* 待机状态，使用正常腿长控制 */
break;
case JUMP_CROUCH:
/* CROUCH状态已废弃，直接跳到LAUNCH */
c->jump.state = JUMP_LAUNCH;
c->jump.state_start_time = current_time;
/* 蓄力阶段：缩短腿长 */
target_L0[0] = c->param->jump_params.crouch_leg_length;
target_L0[1] = c->param->jump_params.crouch_leg_length;
if (elapsed_time >= c->param->jump_params.crouch_time_ms) {
c->jump.state = JUMP_LAUNCH;
c->jump.state_start_time = current_time;
}
break;
case JUMP_LAUNCH: {
/* 起跳阶段：腿伸长 + 大推力 */
target_L0[0] = c->param->leg.max_length;
target_L0[1] = c->param->leg.max_length;
/* 参考Chassis项目：直接给大推力，不依赖PID增益 */
/* 腿长误差约0.17m，Kp=70000时，力=70000*0.17=11900N */
/* 考虑到机器人重量和跳跃高度，给5000-8000N的推力 */
extra_force[0] = 6000.0f; /* 直接给推力(N) */
extra_force[1] = 6000.0f;
/* 位置驱动：腿伸到位就收腿（差值<3cm） */
bool leg_ext = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
(c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
if (leg_ext) {
case JUMP_LAUNCH:
/* 起跳阶段：发力向下推地面 */
target_L0[0] = MAX_LEG_LENGTH; /* 腿伸长 */
target_L0[1] = MAX_LEG_LENGTH;
extra_force[0] = c->param->jump_params.launch_force; /* 额外向下的力 */
extra_force[1] = c->param->jump_params.launch_force;
if (elapsed_time >= c->param->jump_params.launch_time_ms) {
c->jump.state = JUMP_RETRACT;
c->jump.state_start_time = current_time;
}
/* 超时保护 */
if (elapsed_time >= c->param->jump.launch_time_ms) {
c->jump.state = JUMP_RETRACT;
c->jump.state_start_time = current_time;
}
} break;
case JUMP_RETRACT: {
/* 收腿阶段 */
target_L0[0] = c->param->jump.retract_leg_length;
target_L0[1] = c->param->jump.retract_leg_length;
extra_force[0] = c->param->jump.retract_force;
extra_force[1] = c->param->jump.retract_force;
/* 位置驱动：腿缩到位就进落地缓冲 */
float rt = c->param->jump.retract_leg_length;
bool leg_retracted = (c->vmc_[0].leg.L0 < rt + 0.02f) &&
(c->vmc_[1].leg.L0 < rt + 0.02f);
if (leg_retracted) {
break;
case JUMP_RETRACT:
/* 收腿阶段：腿收缩准备落地 */
target_L0[0] = c->param->jump_params.retract_leg_length;
target_L0[1] = c->param->jump_params.retract_leg_length;
extra_force[0] = c->param->jump_params.retract_force; /* 前馈力帮助收腿 */
extra_force[1] = c->param->jump_params.retract_force;
/* 检测落地或超时 */
if ( elapsed_time >= c->param->jump_params.retract_time_ms) {
c->jump.state = JUMP_LAND;
c->jump.state_start_time = current_time;
}
/* 超时保护 */
if (elapsed_time >= c->param->jump.retract_time_ms) {
c->jump.state = JUMP_LAND;
c->jump.state_start_time = current_time;
}
} break;
break;
case JUMP_LAND:
/* 落地缓冲：时间到就回 IDLE */
if (elapsed_time >= c->param->jump.land_time_ms) {
/* 落地阶段：缓冲并恢复正常 */
/* 使用正常腿长，让PID自动恢复 */
if (elapsed_time >= c->param->jump_params.land_time_ms) {
c->jump.state = JUMP_IDLE;
c->jump.state_start_time = current_time;
}
break;
default:
c->jump.state = JUMP_IDLE;
break;
}
}
/**
* @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;
}
}
/* Public functions --------------------------------------------------------- */
/**
* @brief 初始化底盘
@ -603,9 +499,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->jump.state = JUMP_IDLE;
c->jump.trigger = false;
c->jump.state_start_time = 0;
c->jump.crouch_leg_length = c->param->jump.crouch_leg_length;
c->jump.launch_force = c->param->jump.launch_force;
c->jump.retract_leg_length = c->param->jump.retract_leg_length;
c->jump.crouch_leg_length = c->param->jump_params.crouch_leg_length;
c->jump.launch_force = c->param->jump_params.launch_force;
c->jump.retract_leg_length = c->param->jump_params.retract_leg_length;
return CHASSIS_OK;
}
@ -636,7 +532,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
/* 应用零点偏移 */
c->feedback.joint[i].rotor_abs_angle -= c->param->mech.joint_zero[i];
c->feedback.joint[i].rotor_abs_angle -= c->param->joint_zero[i];
}
/* 更新轮子电机反馈 */
@ -686,7 +582,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */
if (c_cmd->jump_trigger &&
(c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) &&
c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE &&
c->jump.state == JUMP_IDLE) {
c->jump.trigger = true;
}
@ -719,53 +615,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
break;
case CHASSIS_MODE_RECOVER: {
/* 运行自起状态机 */
Chassis_RecoverControl(c);
float fn = -20.0f, tp = 0.0f;
Chassis_LQRControl(c, c_cmd);
/* 根据状态决定输出力 */
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;
float fn_left = fn - (isnan(spring_left) ? 0.0f : spring_left);
float fn_right = fn - (isnan(spring_right) ? 0.0f : spring_right);
/* 左腿 */
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_InverseSolve(&c->vmc_[0], fn_left, tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f);
VMC_InverseSolve(&c->vmc_[1], fn_right, 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);
/* 两腿均完成，自动切换到平衡模式 */
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;
}
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
Chassis_Output(c);
} break;
// case CHASSIS_MODE_CALIBRATE: {
@ -850,7 +716,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// /* 加速度限制 */
// float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
// float max_velocity_change = c->param->motion.max_acceleration * c->dt;
// float max_velocity_change = MAX_ACCELERATION * c->dt;
// velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
// float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
@ -884,9 +750,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 目标状态 ==================== */
LQR_State_t target_state = {
.theta = 0.0f,
.theta = 0.1f,
.d_theta = 0.0f,
.phi = 0.0f,
.phi = -0.1f,
.d_phi = 0.0f,
.x = c->chassis_state.target_x,
.d_x = c->chassis_state.target_velocity_x,
@ -897,8 +763,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
float base_target_1 = c->param->mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
@ -920,11 +786,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]);
} else {
target_state.theta = c->param->motion.non_contact_theta;
target_state.theta = NON_CONTACT_THETA;
LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + c->param->motion.non_contact_theta) +
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + NON_CONTACT_THETA) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
c->yaw_control.yaw_force = 0.0f;
}
@ -934,27 +800,22 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]);
} else {
target_state.theta = c->param->motion.non_contact_theta;
target_state.theta = NON_CONTACT_THETA;
LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + c->param->motion.non_contact_theta) +
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + NON_CONTACT_THETA) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
c->yaw_control.yaw_force = 0.0f;
}
/* ==================== 轮毂输出 ==================== */
/* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */
float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
float yaw_scale = c->param->leg.base_length / avg_L0; /* 以基础腿长为基准归一化 */
float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale;
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;
c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
/* ==================== 横滚角补偿 ==================== */
/* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
float Rl = c->param->mech.hip_width / 2.0f; /* 髋宽的一半 */
float Rl = c->param->hip_width / 2.0f; /* 髋宽的一半 */
float roll_error = c->feedback.imu.euler.rol - 0.0f; /* Roll角误差 */
float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0; /* 右腿减左腿腿长差 */
@ -983,10 +844,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 腿长控制 ==================== */
float target_L0[2];
float jump_extra_force[2] = {0.0f, 0.0f};
/* 基础腿长目标 (应用几何前馈补偿) */
/* height 可以为负值用于收腿（预蹲），正值用于伸腿 */
float base_leg_length = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range;
float base_leg_length = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE;
target_L0[0] = base_leg_length + roll_leg_compensation_left;
target_L0[1] = base_leg_length + roll_leg_compensation_right;
@ -999,23 +859,23 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 检查哪条腿更短（需要缩短的那条） */
if (target_L0[0] < target_L0[1]) {
/* 左腿更短，检查是否触碰下限 */
if (target_L0[0] < c->param->leg.min_length) {
compensation_transfer = c->param->leg.min_length - target_L0[0]; /* 计算短缺量 */
target_L0[0] = c->param->leg.min_length; /* 左腿限制在最小值 */
if (target_L0[0] < MIN_LEG_LENGTH) {
compensation_transfer = MIN_LEG_LENGTH - target_L0[0]; /* 计算短缺量 */
target_L0[0] = MIN_LEG_LENGTH; /* 左腿限制在最小值 */
target_L0[1] += compensation_transfer; /* 右腿补偿增加 */
}
} else if (target_L0[1] < target_L0[0]) {
/* 右腿更短，检查是否触碰下限 */
if (target_L0[1] < c->param->leg.min_length) {
compensation_transfer = c->param->leg.min_length - target_L0[1]; /* 计算短缺量 */
target_L0[1] = c->param->leg.min_length; /* 右腿限制在最小值 */
if (target_L0[1] < MIN_LEG_LENGTH) {
compensation_transfer = MIN_LEG_LENGTH - target_L0[1]; /* 计算短缺量 */
target_L0[1] = MIN_LEG_LENGTH; /* 右腿限制在最小值 */
target_L0[0] += compensation_transfer; /* 左腿补偿增加 */
}
}
/* 最终安全限幅 */
target_L0[0] = LIMIT(target_L0[0], c->param->leg.min_length, c->param->leg.max_length);
target_L0[1] = LIMIT(target_L0[1], c->param->leg.min_length, c->param->leg.max_length);
target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
/* 获取腿长变化率 */
float leg_d_length[2];
@ -1035,21 +895,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
if (isnan(spring_force_left)) spring_force_left = 0.0f;
if (isnan(spring_force_left)) spring_force_left = 0.0f; /* 处理无效值 */
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
float virtual_force_left, virtual_torque_left;
if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
/* 跳跃阶段：完全接管力控 */
/* 所有跳跃阶段统一处理：PID + 额外力 */
virtual_force_left = jump_extra_force[0] + pid_output_left;
virtual_torque_left = 0.0f;
/* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
if (c->jump.state == JUMP_RETRACT) {
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
virtual_torque_left = 0.0f; /* 收腿时不控制摆角 */
} else {
/* 正常状态：添加Roll力补偿 (低增益避免抖动) */
virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
pid_output_left + c->param->leg.left_base_force - spring_force_left +
pid_output_left + LEFT_BASE_FORCE - spring_force_left +
jump_extra_force[0] + roll_force_compensation;
virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
}
@ -1065,18 +922,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
if (isnan(spring_force_right)) spring_force_right = 0.0f;
if (isnan(spring_force_right)) spring_force_right = 0.0f; /* 处理无效值 */
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
float virtual_force_right, virtual_torque_right;
if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
/* 跳跃阶段：完全接管力控 */
/* 所有跳跃阶段统一处理：PID + 额外力 */
virtual_force_right = jump_extra_force[1] + pid_output_right;
virtual_torque_right = 0.0f;
if (c->jump.state == JUMP_RETRACT) {
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
virtual_torque_right = 0.0f; /* 收腿时不控制摆角 */
} else {
/* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */
virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
pid_output_right + c->param->leg.right_base_force - spring_force_right +
pid_output_right + RIGHT_BASE_FORCE - spring_force_right +
jump_extra_force[1] - roll_force_compensation;
virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
}

85
User/module/balance_chassis.h
View File

@ -41,7 +41,7 @@ typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
} Chassis_Mode_t;
@ -54,14 +54,6 @@ typedef enum {
JUMP_LAND /* 落地阶段：恢复正常控制 */
} Jump_State_t;
/* 自起状态枚举 */
typedef enum {
RECOVER_FLIP, /* 翻身阶段：theta朝上时，先慢慢翻转到腿朝下 */
RECOVER_STRETCH, /* 伸腿阶段：腿伸到最大长度 */
RECOVER_BACKLEG, /* 后甩腿阶段：phi0 趋近目标角度 */
RECOVER_COMPLETE /* 完成：切换到平衡模式 */
} Recover_State_t;
typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
@ -97,61 +89,37 @@ typedef struct {
VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
KPID_Params_t yaw; /* yaw轴PID控制参数 */
KPID_Params_t roll; /* roll轴腿长补偿PID */
KPID_Params_t roll_force; /* roll轴力补偿PID */
KPID_Params_t tp; /* 摆力矩PID */
KPID_Params_t leg_length; /* 腿长PID */
KPID_Params_t leg_theta; /* 摆角PID */
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
KPID_Params_t roll; /* roll轴腿长补偿PID控制参数 */
KPID_Params_t roll_force; /* roll轴力补偿PID控制参数 */
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
/* 机械参数 */
struct {
float wheel_radius; /* 轮子半径 (m) */
float wheel_gear_ratio; /* 轮毂电机减速比 */
float hip_width; /* 髋宽，两腿间距 (m) */
float joint_zero[4]; /* 关节电机零点偏移 */
float mech_zero_yaw; /* yaw轴机械零点 */
} mech;
uint32_t crouch_time_ms; /* 蓄力时间 (ms) */
uint32_t launch_time_ms; /* 起跳发力时间 (ms) */
uint32_t retract_time_ms; /* 收腿时间 (ms) */
uint32_t land_time_ms; /* 落地缓冲时间 (ms) */
float crouch_leg_length; /* 蓄力时腿长 (m) */
float launch_force; /* 起跳力 (N) */
float retract_leg_length; /* 收腿时腿长 (m) */
float retract_force; /* 收腿前馈力 (N)，负值表示向上收 */
} jump_params;
/* 腿长控制参数 */
struct {
float base_length; /* 基础腿长 (m) */
float length_range; /* 腿长调节范围 (m)，波轮控制 */
float min_length; /* 最小腿长 (m) */
float max_length; /* 最大腿长 (m) */
float left_base_force; /* 左腿基础支撑力 (N) */
float right_base_force; /* 右腿基础支撑力 (N) */
} leg;
float joint_zero[4]; /* 关节电机零点偏移位置 */
float hip_width; /* 髋宽，两腿间距 (m) */
/* 跳跃参数 */
struct {
uint32_t crouch_time_ms; /* 蓄力超时 (ms) */
uint32_t launch_time_ms; /* 起跳超时 (ms) */
uint32_t retract_time_ms; /* 收腿超时 (ms) */
uint32_t land_time_ms; /* 落地缓冲时间 (ms) */
float crouch_leg_length; /* 蓄力时腿长 (m) */
float launch_force; /* 起跳额外力 (N) */
float retract_leg_length; /* 收腿时腿长 (m) */
float retract_force; /* 收腿前馈力 (N)，负值=向上 */
} jump;
float mech_zero_yaw; /* 机械零点 */
/* 运动控制参数 */
struct {
float max_acceleration; /* 最大加速度 (m/s²) */
float non_contact_theta; /* 离地时摆角目标 (rad) */
} motion;
/* LQR 目标状态偏移 */
struct {
float theta;
float x;
float phi;
} lqr_offset;
float theta;
float x;
float phi;
/* 低通滤波器截止频率 */
struct {
float in;
float out;
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
} Chassis_Params_t;
@ -205,11 +173,6 @@ typedef struct {
float retract_leg_length; /* 收腿时的腿长 */
} jump;
/* 自起控制相关 */
struct {
Recover_State_t state[2]; /* 左右腿自起状态 */
} recover;
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;

79
User/module/config.c
View File

@ -66,7 +66,7 @@ Config_RobotParam_t robot_config = {
},
.mech_zero = {
.yaw = 0.0f,
.pit = 0.137291431f,
.pit = 2.2f,
},
.travel = {
.yaw = -1.0f,
@ -214,11 +214,11 @@ Config_RobotParam_t robot_config = {
.leg_length={
.k = 40.0f,
.p = 50.0f,
.p = 20.0f,
.i = 0.01f,
.d = 2.0f,
.i_limit = 0.0f,
.out_limit = 500.0f,
.out_limit = 200.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
@ -308,29 +308,10 @@ Config_RobotParam_t robot_config = {
},
},
.mech = {
.wheel_radius = 0.068f,
.wheel_gear_ratio = 4.5f,
.hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
.joint_zero = {0.0f, 0.0f, 1.32422018f, 1.16195965f}, /* 关节电机零点偏移 */
.mech_zero_yaw = 2.96925735f, /* 机械零点 */
},
.leg = {
.base_length = 0.16f, /* 基础腿长 (m) */
.length_range = 0.20f, /* 腿长调节范围 (m) */
.min_length = 0.14f, /* 最小腿长 (m) */
.max_length = 0.36f, /* 最大腿长 (m) */
.left_base_force = 65.0f, /* 左腿基础支撑力 (N) */
.right_base_force = 65.0f, /* 右腿基础支撑力 (N) */
},
.motion = {
.max_acceleration = 5.0f, /* 最大加速度 (m/s^2) */
.non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */
},
.vmc_param = {
.mech_zero_yaw = 1.78040409f, /* 机械零点 */
.joint_zero = {0.0f, 0.0f, 3.84780002f, -1.16999996f}, /* 关节电机零点偏移位置 */
.hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
.vmc_param = {
{ // 左腿
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
@ -346,38 +327,36 @@ Config_RobotParam_t robot_config = {
.hip_length = 0.0f // 髋宽 (L5) (m)
}
},
.lqr_gains = {
.k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta
.k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta
.k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x
.k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f }, // d_x
.k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi
.k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f }, // d_phi
.k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta
.k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f }, // d_theta
.k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f }, // x
.k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x
.k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi
.k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi
.k11_coeff = { -2.110325959941390e+02f, 2.474876302869424e+02f, -1.259312069344584e+02f, -3.555067587081389e+00f }, // theta
.k12_coeff = { 1.729035713826111e+00f, -1.094205267383853e+00f, -8.235618497594484e+00f, -2.949091603597677e-01f }, // d_theta
.k13_coeff = { -3.661055595462458e+01f, 4.066046385752247e+01f, -1.618786894025212e+01f, -1.618742454043447e+00f }, // x
.k14_coeff = { -2.988926501197955e+01f, 3.445642428071118e+01f, -1.547117906530474e+01f, -2.039186188549067e+00f }, // d_x
.k15_coeff = { -4.094585161782607e+01f, 6.119228618474273e+01f, -3.603009625243965e+01f, 1.037078067693317e+01f }, // phi
.k16_coeff = { -7.625020535837323e+00f, 1.097226354374976e+01f, -6.302244199082423e+00f, 1.920720114282383e+00f }, // d_phi
.k21_coeff = { 3.113860878856309e+02f, -2.577426343951217e+02f, 4.982404130107013e+01f, 1.415180465415879e+01f }, // theta
.k22_coeff = { 1.126082466887103e+01f, -9.812804977178935e+00f, 1.814407629267238e+00f, 1.941377111901043e+00f }, // d_theta
.k23_coeff = { -1.771607516002724e+01f, 4.047388400543465e+01f, -2.962942748901807e+01f, 8.971209797230671e+00f }, // x
.k24_coeff = { -3.391460592939657e+01f, 5.575858169617614e+01f, -3.466996436706782e+01f, 9.833893476570065e+00f }, // d_x
.k25_coeff = { 2.392778926806863e+02f, -2.574664846375947e+02f, 9.953632837845201e+01f, 2.579297039170335e+00f }, // phi
.k26_coeff = { 4.297556375859175e+01f, -4.657509218314731e+01f, 1.826212630358702e+01f, 1.257410509642398e-01f }, // d_phi
},
.jump = {
.crouch_time_ms = 500,
.launch_time_ms = 300,
.retract_time_ms = 500,
.land_time_ms = 500,
.crouch_leg_length = 0.13f, /* 不用蹲太低 */
.launch_force = 20.0f, /* 起跳力减小 */
.retract_leg_length = 0.12f, /* 收腿目标 */
.retract_force = -20.0f, /* 收腿前馈力减小 */
.jump_params = {
.crouch_time_ms = 100,
.launch_time_ms = 120,
.retract_time_ms = 80,
.land_time_ms = 300,
.crouch_leg_length = 0.14f,
.launch_force = 200.0f,
.retract_leg_length = 0.1f, /* 收腿目标更短 */
.retract_force = -120.0f, /* 收腿前馈力加大 */
},
.lqr_offset = {
.theta = 0.0f,
.x = 0.0f,
.phi = 0.0f,
},
},

2
User/module/shoot.c
View File

@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=6800.0f; /* 归一化目标转速 */
s->target_variable.target_rpm=2000.0f; /* 归一化目标转速 */
return 0;
}

12
User/task/ctrl_chassis.c
View File

@ -41,21 +41,13 @@ Chassis_IMU_t chassis_imu;
static int print_chassis(const void *data, char *buf, size_t size) {
const Chassis_t *chassis = (const Chassis_t *)data;
return MRobot_Snprintf(buf, size,
" Mode : %d Recover: L=%d R=%d\r\n"
" Jump : state=%d trigger=%d\r\n"
" LegLen : L=%.3f R=%.3f (m)\r\n"
" Theta : L=%.3f R=%.3f (rad)\r\n"
" Phi0 : L=%.3f R=%.3f (rad)\r\n"
" Mode : %d\r\n"
" IMU : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n"
" Motors : J0=%.1f J1=%.1f J2=%.1f J3=%.1f (rpm)\r\n"
" Wheels : W0=%.1f W1=%.1f (rpm)\r\n"
" Output : J0=%.1f J1=%.1f J2=%.1f J3=%.1f\r\n"
" VelX : %.3f m/s\r\n",
chassis->mode, chassis->recover.state[0], chassis->recover.state[1],
chassis->jump.state, chassis->jump.trigger,
chassis->vmc_[0].leg.L0, chassis->vmc_[1].leg.L0,
chassis->vmc_[0].leg.theta, chassis->vmc_[1].leg.theta,
chassis->vmc_[0].leg.phi0, chassis->vmc_[1].leg.phi0,
chassis->mode,
chassis->feedback.imu.euler.rol, chassis->feedback.imu.euler.pit, chassis->feedback.imu.euler.yaw,
chassis->feedback.joint[0].rotor_speed, chassis->feedback.joint[1].rotor_speed,
chassis->feedback.joint[2].rotor_speed, chassis->feedback.joint[3].rotor_speed,

49
User/task/rc.c
View File

@ -141,37 +141,18 @@ void Task_rc(void *argument) {
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
/************************* 底盘命令 **************************************/
/* 蓄力-释放跳跃逻辑：向上推蓄力收腿，松开回弹时触发跳跃 */
static float last_ch_res = 0.0f; /* 上一次拨杆位置 */
static float min_ch_res = 0.0f; /* 记录最小值（最大蓄力） */
static uint8_t jump_trigger_hold_cnt = 0; /* 触发保持计数 */
static bool in_charge_state = false; /* 是否处于蓄力状态 */
/* 跳跃触发检测：ch_res 从 -1.0f 松开回 0 时触发 */
static bool ch_res_was_min = false; /* 记录上次是否在最低位置 */
static uint8_t jump_trigger_hold_cnt = 0; /* 触发保持计数，避免单拍脉冲被队列重置吞掉 */
const float CH_RES_MIN_THRESHOLD = -0.9f; /* 判定为最低位置的阈值 */
const float CH_RES_RELEASE_THRESHOLD = -0.3f; /* 判定为松开的阈值 */
const float CHARGE_THRESHOLD = -0.4f; /* 蓄力阈值：低于此值开始蓄力 */
const float RELEASE_THRESHOLD = -0.2f; /* 释放阈值：回到此值以上触发跳跃 */
const float MIN_CHARGE = -0.5f; /* 最小蓄力量：至少要推到此值才能触发 */
/* 检测蓄力：拨杆向上推 */
if (dr16.data.ch_res < CHARGE_THRESHOLD) {
in_charge_state = true;
if (dr16.data.ch_res < min_ch_res) {
min_ch_res = dr16.data.ch_res; /* 更新最小值 */
}
}
/* 检测释放：拨杆回弹到阈值以上 && 之前有足够蓄力 */
if (in_charge_state &&
dr16.data.ch_res > RELEASE_THRESHOLD &&
min_ch_res < MIN_CHARGE) {
jump_trigger_hold_cnt = 5; /* 触发跳跃，保持5个RC周期 */
in_charge_state = false; /* 退出蓄力状态 */
min_ch_res = 0.0f; /* 重置蓄力状态 */
}
/* 拨杆回到中位，重置蓄力 */
if (dr16.data.ch_res > -0.1f) {
in_charge_state = false;
min_ch_res = 0.0f;
if (dr16.data.ch_res < CH_RES_MIN_THRESHOLD) {
ch_res_was_min = true; /* 记录已到达最低位置 */
} else if (ch_res_was_min && dr16.data.ch_res > CH_RES_RELEASE_THRESHOLD) {
/* 从最低位置松开，触发跳跃 */
jump_trigger_hold_cnt = 5; /* 保持5个RC周期，确保ctrl任务一定能收到 */
ch_res_was_min = false; /* 重置状态 */
}
cmd_for_chassis.jump_trigger = (jump_trigger_hold_cnt > 0);
@ -179,8 +160,6 @@ void Task_rc(void *argument) {
jump_trigger_hold_cnt--;
}
last_ch_res = dr16.data.ch_res; /* 保存当前值 */
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
@ -190,7 +169,10 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
@ -199,8 +181,7 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
/* height 传递拨杆位置，负值表示下推收腿，正值表示上推伸腿 */
cmd_for_chassis.height = dr16.data.ch_res;
cmd_for_chassis.height = max(dr16.data.ch_res, 0.0f);
osMessageQueueReset(
task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞

4
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([4000 200 500 50 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[150 0;0 5]; %T Tp
Q=diag([1 1 20 5 200 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[1.5 0;0 0.25]; %T Tp
K=lqr(A,B,Q,R);