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

加error pid init

Browse Source
This commit is contained in:
Robofish 2026-01-11 18:30:25 +08:00
parent 25a930c90a
commit c54ff4f988
9 changed files with 5233 additions and 5969 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -135,7 +135,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"" -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
<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>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

29
User/module/balance_chassis.c
View File

@ -401,9 +401,6 @@ void Chassis_Output(Chassis_t *c) {
LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]);
c->output.joint[3] =
LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]);
// float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2],
// c->output.joint[3]};
// Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
MOTOR_LZ_MotionParam_t motion_param = {
.torque=0.0f,
@ -476,7 +473,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
current_state.x = xhat;
current_state.d_x = x_dot_hat;
current_state.phi = c->feedback.imu.euler.pit;
current_state.d_phi = c->feedback.imu.gyro.x;
current_state.d_phi = -c->feedback.imu.gyro.x;
LQR_UpdateState(&c->lqr[0], &current_state);
@ -497,15 +494,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} else {
target_state.theta = 0.00; // 目标摆杆角度
}
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 0.0f; // 目标俯仰角
target_state.phi = -0.2f; // 目标俯仰角
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 = current_state.x; // 目标位置
// target_state.d_x = target_dot_x; // 目标速度
target_state.x = 0.0f; // 目标位置
target_state.d_x = 0.0f; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){
@ -529,9 +521,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
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);
// 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);
c->yaw_control.yaw_force = 0.0f; // 关闭偏航控制
}else{
// 小陀螺模式：使用速度环控制
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
@ -622,8 +615,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 目标腿长设定（包含横滚角补偿）
switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
target_L0[0] = 0.12f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
target_L0[1] = 0.12f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿
@ -727,8 +720,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
}
for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i]) > 20.0f) {
c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f;
if (fabsf(c->output.joint[i]) > 15.0f) {
c->output.joint[i] = (c->output.joint[i] > 0) ? 15.0f : -15.0f;
}
}

28
User/module/config.c
View File

@ -22,8 +22,8 @@ Config_RobotParam_t robot_config = {
/* USER CODE BEGIN robot_config */
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=0.05f,
.shot_burst_num=1,
.shot_delay_time=0.5f,
.shot_burst_num=20,
.fric_motor_param[0] = {
.can = BSP_CAN_1,
.id = 0x201,
@ -235,18 +235,18 @@ Config_RobotParam_t robot_config = {
},
.lqr_gains = {
.k11_coeff = { -2.206604090845255e+02f, 2.614506318765019e+02f, -1.219897134978825e+02f, 4.003523899242641e-01f }, // theta
.k12_coeff = { 5.238547234996340e+00f, -2.415779500015947e+00f, -5.523864718957871e+00f, -3.288450536725437e-02f }, // d_theta
.k13_coeff = { -4.796628281855231e+01f, 5.222843773618256e+01f, -2.030859087373151e+01f, 9.553615633904794e-01f }, // x
.k14_coeff = { -5.114874044222572e+01f, 5.608342009751554e+01f, -2.250513707827996e+01f, 9.368583953998064e-01f }, // d_x
.k15_coeff = { 8.718089045972869e+00f, -4.795765678510961e+00f, -2.015329957668221e-01f, -1.530815441117353e-01f }, // phi
.k16_coeff = { 3.719718252719277e+00f, -6.362232185738266e-01f, -1.909890524420421e+00f, 8.436774993934224e-01f }, // d_phi
.k21_coeff = { 4.340036803921230e+02f, -3.471884225219289e+02f, 5.662796851481392e+01f, 1.439535390926727e+01f }, // theta
.k22_coeff = { 2.121580303492383e+01f, -2.062767124799728e+01f, 5.508502515349819e+00f, 1.362747639428705e+00f }, // d_theta
.k23_coeff = { 2.984613087238082e+01f, -8.945885746289814e+00f, -1.117089589066174e+01f, 5.995872655980672e+00f }, // x
.k24_coeff = { 2.698938462778495e+01f, -4.437271561776529e+00f, -1.407449794025625e+01f, 6.932320158239218e+00f }, // d_x
.k25_coeff = { -1.849173279556238e+01f, 1.518530098209295e+01f, -4.360488190802665e+00f, 3.211721463285619e+00f }, // phi
.k26_coeff = { 3.222156602968452e+01f, -3.605782047335244e+01f, 1.436306842228090e+01f, -3.707923271862080e-01f }, // d_phi
.k11_coeff = { -2.111003343424443e+02f, 2.534552823281283e+02f, -1.288428090302336e+02f, -4.837794661314790e-01f }, // theta
.k12_coeff = { 9.842131763802227e+00f, -7.572331320496771e+00f, -6.747008033464407e+00f, 2.794374462854655e-02f }, // d_theta
.k13_coeff = { -7.026854551050478e+01f, 7.582881862804882e+01f, -2.920474536554779e+01f, 1.123343465922494e+00f }, // x
.k14_coeff = { -7.955300118741869e+01f, 8.683186601398823e+01f, -3.503519879783562e+01f, 1.204200953017506e+00f }, // d_x
.k15_coeff = { 2.214748921482655e+01f, -5.719424594843836e+00f, -8.215498046486028e+00f, 3.357225411090161e+00f }, // phi
.k16_coeff = { -2.588732118811617e-01f, 3.189187555191166e+00f, -3.235989186139448e+00f, 9.848256812196189e-01f }, // d_phi
.k21_coeff = { 3.783375900877637e+02f, -3.033633007638497e+02f, 4.903946574528503e+01f, 1.550109363173939e+01f }, // theta
.k22_coeff = { 2.878430656512739e+01f, -2.993892895824752e+01f, 1.019545567095573e+01f, 1.119137437173934e+00f }, // d_theta
.k23_coeff = { 2.518975783541210e+01f, 2.088553619504979e+00f, -1.929185285401291e+01f, 8.558202445583612e+00f }, // x
.k24_coeff = { 2.519335578848752e+01f, 5.878465085842500e+00f, -2.359290011046876e+01f, 1.050067818606354e+01f }, // d_x
.k25_coeff = { 1.146427691025439e+02f, -1.303101485018965e+02f, 5.234994297700838e+01f, 1.889931627651257e+00f }, // phi
.k26_coeff = { 3.214791991578748e+01f, -3.467989029865478e+01f, 1.318125503363052e+01f, -3.471228203459202e-01f }, // d_phi
},
.theta = 0.0f,
.x = 0.0f,

5
User/module/shoot.c
View File

@ -121,6 +121,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
for(int i=0;i<SHOOT_FRIC_NUM;i++){
MOTOR_RM_Register(&param->fric_motor_param[i]);
PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
PID_Init(&s->pid.fric_err[i], KPID_MODE_CALC_D, target_freq,&param->fric_err);
LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
}
@ -133,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=4000.0f;
s->target_variable.target_rpm=5000.0f; /* 归一化目标转速 */
return 0;
}
@ -209,7 +210,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
case SHOOT_STATE_IDLE:/*熄火等待*/
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_ResetIntegral(&s->pid.fric_follow[i]);
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_fric[i]=s->output.out_follow[i];
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);

4
User/task/init.c
View File

@ -33,8 +33,8 @@ void Task_Init(void *argument) {
/* 创建任务线程 */
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.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_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.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);

2
User/task/user_task.c
View File

@ -42,5 +42,5 @@ const osThreadAttr_t attr_blink = {
const osThreadAttr_t attr_ctrl_shoot = {
.name = "ctrl_shoot",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};

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([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
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
K=lqr(A,B,Q,R);