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

修ctrl

Browse Source
This commit is contained in:
Robofish 2025-10-10 16:59:25 +08:00
parent 64216877e5
commit 596de8c320
4 changed files with 31 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
User/module/balance_chassis.c
View File

@ -42,12 +42,18 @@ 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)) {
// 特殊处理：从JUMP切换到WHELL_LEG_BALANCE时不重置
// 但从WHELL_LEG_BALANCE切换到JUMP时，需要重置以触发新的跳跃
if (c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
c->mode = mode;
return CHASSIS_OK;
}
if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP) {
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
return CHASSIS_OK;
}
Chassis_MotorEnable(c);
@ -58,7 +64,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
c->yaw_control.target_yaw = c->feedback.yaw.rotor_abs_angle;
// 清空位移
c->chassis_state.position_x = 0.0f;
@ -80,7 +86,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0;
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
c->wz_multi=0.0f;
return CHASSIS_OK;
@ -287,11 +293,11 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// state 1 保持0.3s后转到state 2
c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 200000) {
} else if (c->state == 2 && elapsed_us >= 160000) {
// state 2 保持0.2s后转到state 3
c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 300000) {
} else if (c->state == 3 && elapsed_us >= 160000) {
// state 3 保持0.3s后转到state 0
c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
@ -416,9 +422,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
}else{
CircleAdd(&c->wz_multi,0.000001, M_2PI);
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
CircleAdd(&c->yaw_control.target_yaw,c->wz_multi, M_2PI);
c->yaw_control.target_yaw = c->yaw_control.current_yaw + 0.5f;
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);

24
User/module/config.c
View File

@ -254,18 +254,18 @@ Config_RobotParam_t robot_config = {
}
},
.lqr_gains = {
.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
.k11_coeff = { -1.498109852818409e+02f, 1.918923604951453e+02f, -1.080984818173493e+02f, -1.540703437137126e+00f }, // theta
.k12_coeff = { 8.413682810667103e+00f, -6.603584762798329e+00f, -6.421063158996702e+00f, -5.450666844349098e-02f }, // d_theta
.k13_coeff = { -3.146938649452867e+01f, 3.671781823697795e+01f, -1.548019975847165e+01f, -3.659482046966758e-01f }, // x
.k14_coeff = { -3.363190268966418e+01f, 4.073114332036178e+01f, -1.877821151363973e+01f, -6.755848684037919e-01f }, // d_x
.k15_coeff = { 1.813346556940570e+00f, 1.542139674818206e+01f, -1.784151260000496e+01f, 6.975189972486323e+00f }, // phi
.k16_coeff = { -1.683339907923917e+00f, 3.762911505427638e+00f, -2.966405826579746e+00f, 1.145611903160937e+00f }, // d_phi
.k21_coeff = { 3.683683451383080e+02f, -3.184826128050574e+02f, 7.012281968985167e+01f, 1.232691707185163e+01f }, // theta
.k22_coeff = { 2.256451382235226e+01f, -2.387074220964917e+01f, 8.355357863648345e+00f, 1.148384164091676e+00f }, // d_theta
.k23_coeff = { 1.065026516142075e+01f, 1.016717837738013e+01f, -1.810442639595643e+01f, 7.368019318950717e+00f }, // x
.k24_coeff = { 2.330418054102148e+00f, 2.193671212435775e+01f, -2.502198732490354e+01f, 9.621144599080463e+00f }, // d_x
.k25_coeff = { 1.784444885521937e+02f, -2.030738611028434e+02f, 8.375405599993576e+01f, -2.204042546242858e-01f }, // phi
.k26_coeff = { 2.646425532528492e+01f, -3.034702782249508e+01f, 1.275100635568078e+01f, -4.878639273974432e-01f }, // d_phi
},
.theta = 0.0f,

6
User/task/rc.c
View File

@ -61,10 +61,12 @@ void Task_rc(void *argument) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;

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

@ -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=[250 0;0 50]; %T Tp
R=[300 0;0 55]; %T Tp
K=lqr(A,B,Q,R);