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

整理

Browse Source
This commit is contained in:
Robofish 2026-01-12 03:18:02 +08:00
parent 39adf0a2b6
commit a4a8bb6ccb
11 changed files with 9284 additions and 8637 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -135,7 +135,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name> <Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -208,46 +208,6 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>gimbal</ItemText> <ItemText>gimbal</ItemText>
</Ww> </Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>GIMBAL_IMU_Update</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>rx_msg</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>rx_msg</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>can2_debug</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>fdcan2_init_debug</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>rx_debug</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_for_gimbal</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -296,7 +256,7 @@
<EnableFlashSeq>1</EnableFlashSeq> <EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <DbgClock>10000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

883
User/module/balance_chassis.c
View File

File diff suppressed because it is too large Load Diff

22
User/module/balance_chassis.h
View File

@ -38,19 +38,10 @@ extern "C" {
typedef enum { typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
} Chassis_Mode_t; } Chassis_Mode_t;
typedef enum {
CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */
CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
} Chassis_JumpState_t;
typedef struct { typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */ Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */ MoveVector_t move_vec; /* 底盘运动向量 */
@ -126,13 +117,6 @@ typedef struct {
VMC_t vmc_[2]; /* 两条腿的VMC */ VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_t lqr[2]; /* 两条腿的LQR控制器 */ LQR_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state;
uint64_t jump_time;
float angle;
float height;
/* 机体状态估计 */ /* 机体状态估计 */
struct { struct {
float position_x; /* 机体x位置 */ float position_x; /* 机体x位置 */
@ -151,8 +135,6 @@ typedef struct {
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */ bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control; } yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */ /* PID计算的目标值 */
struct { struct {
AHRS_Eulr_t chassis; AHRS_Eulr_t chassis;

44
User/module/config.c
View File

@ -24,7 +24,7 @@ Config_RobotParam_t robot_config = {
.gimbal_param = { .gimbal_param = {
.pid = { .pid = {
.yaw_omega = { .yaw_omega = {
.k = 1.0f, .k = 2.0f,
.p = 1.0f, .p = 1.0f,
.i = 0.3f, .i = 0.3f,
.d = 0.0f, .d = 0.0f,
@ -34,8 +34,8 @@ Config_RobotParam_t robot_config = {
.range = -1.0f, .range = -1.0f,
}, },
.yaw_angle = { .yaw_angle = {
.k = 8.0f, .k = 7.0f,
.p = 3.0f, .p = 3.5f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
@ -54,10 +54,10 @@ Config_RobotParam_t robot_config = {
.range = -1.0f, .range = -1.0f,
}, },
.pit_angle = { .pit_angle = {
.k = 5.0f, .k = 2.5f,
.p = 5.0f, .p = 5.0f,
.i = 2.5f, .i = 0.2f,
.d = 0.0f, .d = 0.01f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 10.0f, .out_limit = 10.0f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
@ -66,11 +66,11 @@ Config_RobotParam_t robot_config = {
}, },
.mech_zero = { .mech_zero = {
.yaw = 0.0f, .yaw = 0.0f,
.pit = 2.12f, .pit = 2.2f,
}, },
.travel = { .travel = {
.yaw = -1.0f, .yaw = -1.0f,
.pit = 0.9f, .pit = 0.85f,
}, },
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -180,7 +180,7 @@ Config_RobotParam_t robot_config = {
.chassis_param = { .chassis_param = {
.yaw={ .yaw={
.k=0.0f, .k=1.0f,
.p=1.0f, .p=1.0f,
.i=0.0f, .i=0.0f,
.d=0.3f, .d=0.3f,
@ -297,7 +297,7 @@ Config_RobotParam_t robot_config = {
}, },
}, },
.mech_zero_yaw = 4.34085676f, /* 250.5度，机械零点 */ .mech_zero_yaw = 1.78040409f, /* 机械零点 */
.vmc_param = { .vmc_param = {
{ // 左腿 { // 左腿
@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -2.111003343424443e+02f, 2.534552823281283e+02f, -1.288428090302336e+02f, -4.837794661314790e-01f }, // theta .k11_coeff = { -2.702074813939778e+02f, 2.894104138810802e+02f, -1.211372850409846e+02f, -6.933300731936785e-01f }, // theta
.k12_coeff = { 9.842131763802227e+00f, -7.572331320496771e+00f, -6.747008033464407e+00f, 2.794374462854655e-02f }, // d_theta .k12_coeff = { -3.327095836963479e+00f, 5.164647957579151e+00f, -7.601554284013210e+00f, 2.552123800155830e-01f }, // d_theta
.k13_coeff = { -7.026854551050478e+01f, 7.582881862804882e+01f, -2.920474536554779e+01f, 1.123343465922494e+00f }, // x .k13_coeff = { -4.196624710163955e+01f, 4.127695960453792e+01f, -1.402278033829385e+01f, 7.645039434796388e-03f }, // x
.k14_coeff = { -7.955300118741869e+01f, 8.683186601398823e+01f, -3.503519879783562e+01f, 1.204200953017506e+00f }, // d_x .k14_coeff = { -5.313365798300281e+01f, 5.256471525738568e+01f, -1.848736219345810e+01f, -1.124172698659643e-03f }, // d_x
.k15_coeff = { 2.214748921482655e+01f, -5.719424594843836e+00f, -8.215498046486028e+00f, 3.357225411090161e+00f }, // phi .k15_coeff = { -6.095849580858518e+01f, 7.441367039791133e+01f, -3.453552694561191e+01f, 7.000025215843615e+00f }, // phi
.k16_coeff = { -2.588732118811617e-01f, 3.189187555191166e+00f, -3.235989186139448e+00f, 9.848256812196189e-01f }, // d_phi .k16_coeff = { -9.662388366457952e+00f, 1.288897516182337e+01f, -6.650550674687217e+00f, 1.558651627757777e+00f }, // d_phi
.k21_coeff = { 3.783375900877637e+02f, -3.033633007638497e+02f, 4.903946574528503e+01f, 1.550109363173939e+01f }, // theta .k21_coeff = { 1.194019591863143e+02f, -7.294395555463191e+01f, -5.502401903255857e+00f, 1.056061060815681e+01f }, // theta
.k22_coeff = { 2.878430656512739e+01f, -2.993892895824752e+01f, 1.019545567095573e+01f, 1.119137437173934e+00f }, // d_theta .k22_coeff = { 1.953914704492318e+01f, -1.853477522511211e+01f, 5.522863854227361e+00f, 2.440732134290017e-01f }, // d_theta
.k23_coeff = { 2.518975783541210e+01f, 2.088553619504979e+00f, -1.929185285401291e+01f, 8.558202445583612e+00f }, // x .k23_coeff = { -3.113451478996141e+01f, 3.917196236114254e+01f, -1.888229690044390e+01f, 4.105978636011114e+00f }, // x
.k24_coeff = { 2.519335578848752e+01f, 5.878465085842500e+00f, -2.359290011046876e+01f, 1.050067818606354e+01f }, // d_x .k24_coeff = { -3.864995803122945e+01f, 4.873841855321316e+01f, -2.361615127826612e+01f, 5.240190452941591e+00f }, // d_x
.k25_coeff = { 1.146427691025439e+02f, -1.303101485018965e+02f, 5.234994297700838e+01f, 1.889931627651257e+00f }, // phi .k25_coeff = { 2.402815287983674e+02f, -2.353554477048087e+02f, 7.938206986720357e+01f, 7.179992245652441e-01f }, // phi
.k26_coeff = { 3.214791991578748e+01f, -3.467989029865478e+01f, 1.318125503363052e+01f, -3.471228203459202e-01f }, // d_phi .k26_coeff = { 5.110194134606057e+01f, -5.057473149871059e+01f, 1.733200146622070e+01f, -2.070892022734363e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,

22
User/module/gimbal.c
View File

@ -33,6 +33,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
if (mode == g->mode) if (mode == g->mode)
return GIMBAL_OK; return GIMBAL_OK;
// 检查是否relax->absolute
int relax_to_absolute = (g->mode == GIMBAL_MODE_RELAX && mode == GIMBAL_MODE_ABSOLUTE);
PID_Reset(&g->pid.yaw_angle); PID_Reset(&g->pid.yaw_angle);
PID_Reset(&g->pid.yaw_omega); PID_Reset(&g->pid.yaw_omega);
PID_Reset(&g->pid.pit_angle); PID_Reset(&g->pid.pit_angle);
@ -41,16 +44,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
MOTOR_DM_Enable(&(g->param->yaw_motor)); MOTOR_DM_Enable(&(g->param->yaw_motor));
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
// if (g->mode == GIMBAL_MODE_RELAX) {
// if (mode == GIMBAL_MODE_ABSOLUTE) { if (relax_to_absolute) {
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; // pitch回到限位中点
// } else if (mode == GIMBAL_MODE_RELATIVE) { g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; } else {
// } g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
// } }
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->mode = mode; g->mode = mode;
@ -253,7 +255,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
void Gimbal_Output(Gimbal_t *g){ void Gimbal_Output(Gimbal_t *g){
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit); MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
MOTOR_MIT_Output_t output = {0}; MOTOR_MIT_Output_t output = {0};
output.torque = g->out.yaw * 5.0f; // 乘以减速比 output.torque = g->out.yaw * 5.0f;
output.kd = 0.3f; output.kd = 0.3f;
MOTOR_RM_Ctrl(&g->param->pit_motor); MOTOR_RM_Ctrl(&g->param->pit_motor);
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output); MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);

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->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output)); memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */ s->target_variable.target_rpm=4000.0f; /* 归一化目标转速 */
return 0; return 0;
} }

4
User/task/ctrl_gimbal.c
View File

@ -52,8 +52,8 @@ void Task_ctrl_gimbal(void *argument) {
Gimbal_UpdateFeedback(&gimbal); Gimbal_UpdateFeedback(&gimbal);
// osMessageQueueReset(task_runtime.msgq.chassis_yaw); osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
// osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0); osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
Gimbal_Control(&gimbal, &gimbal_cmd); Gimbal_Control(&gimbal, &gimbal_cmd);

2
User/task/init.c
View File

@ -34,7 +34,7 @@ void Task_Init(void *argument) {
/* 创建任务线程 */ /* 创建任务线程 */
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc); task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit); task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
// task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal); task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor); task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);

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

@ -17,9 +17,9 @@ function K = get_k_length(leg_length)
R1=0.068; %驱动轮半径 R1=0.068; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离 L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离 LM1=leg_length/2; %摆杆重心到其转轴距离
l1=-0.03; %机体质心距离转轴距离 l1=-0.01; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量 mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量 mp1=1.5; %杆质量
M1=12.0; %机体质量 M1=12.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量 Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
@ -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=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([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([500 1 600 200 8000 100]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[250 0;0 50]; %T Tp R=[200 0;0 60]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);