From 64216877e5f1786e641d5f072ff63da30234631b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Fri, 10 Oct 2025 00:21:11 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A5=BDqr?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 98 ++++++++++++++++--- User/module/balance_chassis.h | 4 + User/module/config.c | 26 ++--- User/module/shoot.c | 2 +- .../balance/series_legs/get_k_length.m | 2 +- 5 files changed, 105 insertions(+), 27 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 49a8cbd..6274749 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -42,6 +42,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */ + // 在CHASSIS_MODE_JUMP和CHASSIS_MODE_WHELL_LEG_BALANCE之间切换时不重置 + if ((c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) || + (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP)) { + c->mode = mode; + return CHASSIS_OK; + } + Chassis_MotorEnable(c); PID_Reset(&c->pid.leg_length[0]); @@ -73,6 +80,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { c->mode = mode; c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 + c->jump_time=0; c->wz_multi=0.0f; return CHASSIS_OK; @@ -106,6 +114,8 @@ static void Chassis_UpdateChassisState(Chassis_t *c) { c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; } + + int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { if (c == NULL || param == NULL || target_freq <= 0.0f) { return -1; // 参数错误 @@ -253,15 +263,54 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_Output(c); // 统一输出 } break; - - case CHASSIS_MODE_WHELL_LEG_BALANCE: - case CHASSIS_MODE_ROTOR: + case CHASSIS_MODE_JUMP: + // 跳跃模式状态机 + // 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成) + // jump_time == 0: 未开始跳跃 + // jump_time == 1: 已完成跳跃（不再触发） + // jump_time > 1: 跳跃进行中 + + // 检测是否需要开始新的跳跃（state为0且jump_time为0） + if (c->state == 0 && c->jump_time == 0) { + // 开始跳跃，进入state 1 + c->state = 1; + c->jump_time = BSP_TIME_Get_us(); + } + + // 只有在跳跃进行中时才处理状态转换（jump_time > 1） + if (c->jump_time > 1) { + // 计算已经过的时间（微秒） + uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time; + + // 状态转换逻辑 + if (c->state == 1 && elapsed_us >= 300000) { + // state 1 保持0.3s后转到state 2 + c->state = 2; + c->jump_time = BSP_TIME_Get_us(); // 重置计时 + } else if (c->state == 2 && elapsed_us >= 200000) { + // state 2 保持0.2s后转到state 3 + c->state = 3; + c->jump_time = BSP_TIME_Get_us(); // 重置计时 + } else if (c->state == 3 && elapsed_us >= 300000) { + // state 3 保持0.3s后转到state 0 + c->state = 0; + c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发 + } + } + // 执行LQR控制（包含PID腿长控制） Chassis_LQRControl(c, c_cmd); + Chassis_Output(c); // 统一输出 + break; + case CHASSIS_MODE_WHELL_LEG_BALANCE: + case CHASSIS_MODE_ROTOR: + // 执行LQR控制（包含PID腿长控制） + Chassis_LQRControl(c, c_cmd); Chassis_Output(c); // 统一输出 break; + break; default: return CHASSIS_ERR_MODE; } @@ -301,7 +350,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 运动参数（参考C++版本的状态估计） */ static float xhat = 0.0f, x_dot_hat = 0.0f; float target_dot_x = 0.0f; - float max_acceleration = 3.0f; // 最大加速度限制: 1 m/s² + float max_acceleration = 3.0f; // 最大加速度限制: 3 m/s² // 简化的速度估计（后续可以改进为C++版本的复杂滤波） x_dot_hat = c->chassis_state.velocity_x; @@ -359,10 +408,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f; target_state.d_x = target_dot_x; // 目标速度 - /* 更新LQR状态 */ - LQR_SetTargetState(&c->lqr[0], &target_state); - LQR_SetTargetState(&c->lqr[1], &target_state); - if (c->mode != CHASSIS_MODE_ROTOR){ c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; c->yaw_control.target_yaw = @@ -380,17 +425,24 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } if (c->vmc_[0].leg.is_ground_contact) { + /* 更新LQR状态 */ + LQR_SetTargetState(&c->lqr[0], &target_state); LQR_Control(&c->lqr[0]); } else { + target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 + 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 + 0.0f) + c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f); c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 } - if (c->vmc_[1].leg.is_ground_contact) + if (c->vmc_[1].leg.is_ground_contact){ + LQR_SetTargetState(&c->lqr[1], &target_state); LQR_Control(&c->lqr[1]); - else { + }else { + target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 + 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 + 0.0f) + @@ -421,8 +473,28 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { roll_compensation_force = -20.0f; // 目标腿长设定（移除横滚角补偿） - target_L0[0] = 0.15f + c_cmd->height * 0.25f; // 左腿：基础腿长 + 高度调节 - target_L0[1] = 0.15f + c_cmd->height * 0.25f; // 右腿：基础腿长 + 高度调节 + switch (c->state) { + case 0: // 正常状态，根据高度指令调节腿长 + target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节 + target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节 + break; + case 1: // 准备阶段，腿长收缩 + target_L0[0] = 0.15f; // 左腿：收缩到基础腿长 + target_L0[1] = 0.15f; // 右腿：收缩到基础腿长 + break; + case 2: // 起跳阶段，腿长快速伸展 + target_L0[0] = 0.55f; // 左腿：伸展到最大腿长 + target_L0[1] = 0.55f; // 右腿：伸展到最大腿长 + break; + case 3: // 着地阶段，腿长缓冲 + target_L0[0] = 0.1f; // 左腿：缓冲腿长 + target_L0[1] = 0.1f; // 右腿：缓冲腿长 + break; + default: + target_L0[0] = 0.15f + c_cmd->height * 0.22f; + target_L0[1] = 0.15f + c_cmd->height * 0.22f; + break; + } // 获取腿长变化率 VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); @@ -504,3 +576,5 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { return CHASSIS_OK; } + + diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 7178b27..6e2cf3a 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -40,6 +40,7 @@ typedef enum { CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ + CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */ CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */ } Chassis_Mode_t; @@ -125,6 +126,8 @@ typedef struct { LQR_t lqr[2]; /* 两条腿的LQR控制器 */ int8_t state; + uint64_t jump_time; + float angle; float height; @@ -220,6 +223,7 @@ 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 底盘输出值 * diff --git a/User/module/config.c b/User/module/config.c index 4c64f47..c60a619 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -20,7 +20,7 @@ Config_RobotParam_t robot_config = { .shoot_param = { .trig_step_angle=M_2PI/8, .shot_delay_time=0.05f, - .shot_burst_num=1, + .shot_burst_num=50, .fric_motor_param[0] = { .can = BSP_CAN_2, .id = 0x201, @@ -254,18 +254,18 @@ Config_RobotParam_t robot_config = { } }, .lqr_gains = { - .k11_coeff = { -1.675900301510996e+02f, 2.020661327080207e+02f, -1.145642457869424e+02f, -3.747932874943916e+00f }, // theta - .k12_coeff = { 6.610013864045129e+00f, -6.495140417893399e+00f, -8.129319798942683e+00f, -2.920993723294792e-01f }, // d_theta - .k13_coeff = { -3.558043125079351e+01f, 3.982938747687503e+01f, -1.608580664764812e+01f, -1.704388165552662e+00f }, // x - .k14_coeff = { -3.082060804555171e+01f, 3.657436465245446e+01f, -1.721864301319228e+01f, -2.425477933777937e+00f }, // d_x - .k15_coeff = { -2.915524527743928e+01f, 4.841882215140198e+01f, -3.075590139522371e+01f, 9.309579833638985e+00f }, // phi - .k16_coeff = { -6.590867790309310e+00f, 8.818728233474916e+00f, -4.823548294804866e+00f, 1.465064667201413e+00f }, // d_phi - .k21_coeff = { 2.124446115135024e+02f, -1.763726543380520e+02f, 3.279099984858797e+01f, 1.233889708835248e+01f }, // theta - .k22_coeff = { 1.207615888432813e+01f, -1.237642650067993e+01f, 4.209610950938401e+00f, 1.417825883473013e+00f }, // d_theta - .k23_coeff = { -1.482017772836761e+01f, 3.364904259474741e+01f, -2.486572037866952e+01f, 7.847915428831716e+00f }, // x - .k24_coeff = { -3.273926334829641e+01f, 5.271977032684813e+01f, -3.274024194089392e+01f, 9.730698282497507e+00f }, // d_x - .k25_coeff = { 1.663230320117893e+02f, -1.816044424207695e+02f, 7.154626742111853e+01f, 4.309975259244579e+00f }, // phi - .k26_coeff = { 2.350205493668966e+01f, -2.586342446220884e+01f, 1.038824015934221e+01f, 1.031625329478827e-01f }, // d_phi + .k11_coeff = { -1.541653276294261e+02f, 1.944240977607161e+02f, -1.094196687050347e+02f, -1.991862922476537e+00f }, // theta + .k12_coeff = { 7.958802216156968e+00f, -6.496301522626967e+00f, -6.793798563394576e+00f, -9.768935145474957e-02f }, // d_theta + .k13_coeff = { -3.265025394715387e+01f, 3.775539494427401e+01f, -1.577293629786055e+01f, -6.143705005307794e-01f }, // x + .k14_coeff = { -3.342861178786544e+01f, 4.027907112212562e+01f, -1.860920837741005e+01f, -1.011945860330850e+00f }, // d_x + .k15_coeff = { -4.173395770434105e+00f, 2.208051577637197e+01f, -2.060462131200154e+01f, 7.510277938071905e+00f }, // phi + .k16_coeff = { -2.731905689135932e+00f, 4.861408834763942e+00f, -3.379865397362515e+00f, 1.218262454432878e+00f }, // d_phi + .k21_coeff = { 3.310056906118938e+02f, -2.839582288120425e+02f, 6.078882971562280e+01f, 1.248056097881360e+01f }, // theta + .k22_coeff = { 2.029863884910664e+01f, -2.130214391291087e+01f, 7.397797146713744e+00f, 1.234955263546594e+00f }, // d_theta + .k23_coeff = { 4.920315528337404e+00f, 1.585029096342149e+01f, -2.001513921771265e+01f, 7.602599395836243e+00f }, // x + .k24_coeff = { -6.228021022229131e+00f, 2.987205717969141e+01f, -2.731854197984320e+01f, 9.786304250377514e+00f }, // d_x + .k25_coeff = { 1.782118404877848e+02f, -2.009255508273753e+02f, 8.203108121734316e+01f, 7.691004625305012e-01f }, // phi + .k26_coeff = { 2.601707340816693e+01f, -2.955760901249582e+01f, 1.229540979299611e+01f, -3.567767355508533e-01f }, // d_phi }, .theta = 0.0f, diff --git a/User/module/shoot.c b/User/module/shoot.c index 064ca11..ea1611e 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -133,7 +133,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=200.0f; + s->target_variable.target_rpm=5000.0f; return 0; } diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 3dfc7a8..fe50d87 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -49,7 +49,7 @@ function K = get_k_length(leg_length) B=double(B); Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[140 0;0 40]; %T Tp + R=[250 0;0 50]; %T Tp K=lqr(A,B,Q,R);