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

nb

Browse Source
This commit is contained in:
Robofish 2026-01-11 17:08:52 +08:00
parent 9652f6c5cc
commit 25a930c90a
7 changed files with 5626 additions and 5806 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

2
User/component/vmc.c
View File

@ -80,7 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
if (vmc == NULL || !vmc->initialized) { if (vmc == NULL || !vmc->initialized) {
return -1; return -1;
} }
body_pitch = -body_pitch; // 机体俯仰角取反 // body_pitch = -body_pitch; // 机体俯仰角取反
VMC_Leg_t *leg = &vmc->leg; VMC_Leg_t *leg = &vmc->leg;

14
User/module/balance_chassis.c
View File

@ -475,8 +475,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
current_state.d_theta = c->vmc_[0].leg.d_theta; current_state.d_theta = c->vmc_[0].leg.d_theta;
current_state.x = xhat; current_state.x = xhat;
current_state.d_x = x_dot_hat; current_state.d_x = x_dot_hat;
current_state.phi = -c->feedback.imu.euler.pit; current_state.phi = c->feedback.imu.euler.pit;
current_state.d_phi = -c->feedback.imu.gyro.y; current_state.d_phi = c->feedback.imu.gyro.x;
LQR_UpdateState(&c->lqr[0], &current_state); LQR_UpdateState(&c->lqr[0], &current_state);
@ -499,10 +499,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} }
target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.16f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.16f + c_cmd->height * 0.25f),2) + 3.87083*(0.16f + c_cmd->height * 0.25f) + -0.4154; // 目标俯仰角
target_state.phi = 0.0f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f; // target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度 target_state.x = current_state.x; // 目标位置
// target_state.d_x = target_dot_x; // 目标速度
target_state.d_x = 0.0f; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){ if (c->mode != CHASSIS_MODE_ROTOR){
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;

24
User/module/config.c
View File

@ -235,18 +235,18 @@ Config_RobotParam_t robot_config = {
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -2.844277590546524e+02f, 3.014647915495374e+02f, -1.268350617667978e+02f, -3.081795747371510e+00f }, // theta .k11_coeff = { -2.206604090845255e+02f, 2.614506318765019e+02f, -1.219897134978825e+02f, 4.003523899242641e-01f }, // theta
.k12_coeff = { -3.549555833965762e+00f, 4.386109489290642e+00f, -8.231027185636849e+00f, -1.385726427113662e-01f }, // d_theta .k12_coeff = { 5.238547234996340e+00f, -2.415779500015947e+00f, -5.523864718957871e+00f, -3.288450536725437e-02f }, // d_theta
.k13_coeff = { -7.526831317875141e+01f, 7.503437037461828e+01f, -2.586278102813240e+01f, -2.882206207079063e-01f }, // x .k13_coeff = { -4.796628281855231e+01f, 5.222843773618256e+01f, -2.030859087373151e+01f, 9.553615633904794e-01f }, // x
.k14_coeff = { -6.463718727551078e+01f, 6.474653251665229e+01f, -2.345655849302561e+01f, -4.576568180320017e-01f }, // d_x .k14_coeff = { -5.114874044222572e+01f, 5.608342009751554e+01f, -2.250513707827996e+01f, 9.368583953998064e-01f }, // d_x
.k15_coeff = { -1.724417956662031e+01f, 2.623364741718471e+01f, -1.364297468210267e+01f, 2.161483799244492e+00f }, // phi .k15_coeff = { 8.718089045972869e+00f, -4.795765678510961e+00f, -2.015329957668221e-01f, -1.530815441117353e-01f }, // phi
.k16_coeff = { -1.670817080995841e+01f, 2.265762117831611e+01f, -1.161008984543497e+01f, 2.496364045937191e+00f }, // d_phi .k16_coeff = { 3.719718252719277e+00f, -6.362232185738266e-01f, -1.909890524420421e+00f, 8.436774993934224e-01f }, // d_phi
.k21_coeff = { 7.413511085596214e+01f, -6.090178313593463e+00f, -4.039446413615985e+01f, 1.653501139374634e+01f }, // theta .k21_coeff = { 4.340036803921230e+02f, -3.471884225219289e+02f, 5.662796851481392e+01f, 1.439535390926727e+01f }, // theta
.k22_coeff = { 6.411680140494978e+00f, -3.006095633385133e+00f, -1.177334020684990e+00f, 1.672016772859867e+00f }, // d_theta .k22_coeff = { 2.121580303492383e+01f, -2.062767124799728e+01f, 5.508502515349819e+00f, 1.362747639428705e+00f }, // d_theta
.k23_coeff = { -5.915654230562277e+01f, 7.939714736323532e+01f, -4.009665367872189e+01f, 8.868997496924505e+00f }, // x .k23_coeff = { 2.984613087238082e+01f, -8.945885746289814e+00f, -1.117089589066174e+01f, 5.995872655980672e+00f }, // x
.k24_coeff = { -6.361086354133386e+01f, 8.119104026481590e+01f, -3.922514760493152e+01f, 8.469126805164274e+00f }, // d_x .k24_coeff = { 2.698938462778495e+01f, -4.437271561776529e+00f, -1.407449794025625e+01f, 6.932320158239218e+00f }, // d_x
.k25_coeff = { 5.672537764809064e+01f, -5.809781196995141e+01f, 2.039529228765693e+01f, 5.636457827688945e+00f }, // phi .k25_coeff = { -1.849173279556238e+01f, 1.518530098209295e+01f, -4.360488190802665e+00f, 3.211721463285619e+00f }, // phi
.k26_coeff = { 7.417061748885547e+01f, -7.440121006735944e+01f, 2.577732090617134e+01f, 8.817723457684845e-01f }, // d_phi .k26_coeff = { 3.222156602968452e+01f, -3.605782047335244e+01f, 1.436306842228090e+01f, -3.707923271862080e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,

2
User/task/atti_esit.c
View File

@ -92,7 +92,7 @@ void Task_atti_esit(void *argument) {
imu_for_chassis.gyro.z = bmi088.gyro.z; imu_for_chassis.gyro.z = bmi088.gyro.z;
/* 欧拉角绕z轴旋转270度 */ /* 欧拉角绕z轴旋转270度 */
imu_for_chassis.euler.rol = eulr_to_send.pit; imu_for_chassis.euler.rol = eulr_to_send.pit;
imu_for_chassis.euler.pit = eulr_to_send.rol; imu_for_chassis.euler.pit = -eulr_to_send.rol;
imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度 imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度
osMessageQueueReset( osMessageQueueReset(
task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞 task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞

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

@ -48,7 +48,7 @@ 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=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); B=double(B);
Q=diag([100 100 2000 200 5000 600]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([100 100 1000 100 1000 100]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 40]; %T Tp R=[240 0;0 40]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);