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

修自瞄

Browse Source
This commit is contained in:
Robofish 2026-03-18 23:58:49 +08:00
parent a3a05aa076
commit 5133123dc3
9 changed files with 75 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
User/module/aimbot.c
View File

@ -93,7 +93,10 @@ int8_t Aimbot_Init(Aimbot_Param_t *param) {
/* 注册 AI 指令帧队列（下层板接收／上层板发送） */
BSP_FDCAN_RegisterId(param->can, param->cmd_id,
BSP_FDCAN_DEFAULT_QUEUE_SIZE);
for (uint8_t i = 0; i < 3; i++) {
BSP_CAN_RegisterId(param->can, param->cmd_id + i,
10);
}
/* 注册反馈数据帧队列（上层板接收／下层板发送） */
for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) {
BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i,

37
User/module/balance_chassis.c
View File

@ -797,7 +797,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
/* 初始化PID控制器 */
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_SET_D, target_freq, &param->yaw);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, &param->rotor);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
@ -1143,7 +1143,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset);
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
} else {
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 3.0f;
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
}
@ -1224,9 +1224,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 对齐完成，切换到平衡模式 */
c->rotor_state.exiting = false;
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
/* 只重置 yaw/rotor PID，保留 chassis_state 避免 LQR 突变 */
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.rotor);
Chassis_ResetControllers(c);
}
}
} else {
@ -1244,8 +1242,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 退出过渡减速完成后，用yaw PID对齐零点 */
if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) {
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle,
c->feedback.imu.gyro.z, c->dt);
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
} else {
float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */
c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
@ -1262,30 +1259,16 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
/* 迟滞切换：只有当另一个零点明显更近（差值>0.3rad≈17°）时才切换，
* 避免在两个零点正中间反复跳变 */
float hysteresis = 0.3f;
if (c->yaw_control.is_reversed) {
/* 当前跟踪 target_2，只有 target_1 明显更近才切 */
if (fabsf(error_to_target1) < fabsf(error_to_target2) - hysteresis) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false;
} else {
c->yaw_control.target_yaw = base_target_2;
}
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false;
} else {
/* 当前跟踪 target_1，只有 target_2 明显更近才切 */
if (fabsf(error_to_target2) < fabsf(error_to_target1) - hysteresis) {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true;
} else {
c->yaw_control.target_yaw = base_target_1;
}
c->yaw_control.target_yaw = base_target_2;
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,
c->feedback.imu.gyro.z, c->dt);
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
}
/* ==================== 左腿LQR控制 ==================== */

25
User/module/cmd/cmd.c
View File

@ -210,8 +210,13 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
if (ctx->input.nuc.mode!=0) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_AI_CONTROL;
ctx->output.gimbal.cmd.ai_mode = ctx->input.nuc.mode;
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
ctx->output.gimbal.cmd.ff_vel_yaw = ctx->input.nuc.gimbal.vel.yaw;
ctx->output.gimbal.cmd.ff_vel_pit = ctx->input.nuc.gimbal.vel.pit;
ctx->output.gimbal.cmd.ff_accl_yaw = ctx->input.nuc.gimbal.accl.yaw;
ctx->output.gimbal.cmd.ff_accl_pit = ctx->input.nuc.gimbal.accl.pit;
}
}
@ -635,19 +640,19 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE);
// #if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
// if (ctx->input.online[CMD_SRC_NUC]) {
// if (ctx->active_source==CMD_SRC_RC) {
// if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
// ctx->output.gimbal.source = CMD_SRC_NUC;
// ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source==CMD_SRC_RC) {
if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.source = CMD_SRC_NUC;
#endif
// }
// }
// }
// #endif
}
}
}
#endif
return CMD_OK;
}

10
User/module/config.c
View File

@ -537,13 +537,15 @@ Config_RobotParam_t robot_config = {
#if CMD_ENABLE_MODULE_GIMBAL
.gimbal_sw_up = GIMBAL_MODE_RELAX,
.gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
// .gimbal_sw_mid = GIMBAL_MODE_RECOVER,
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
.gimbal_sw_down = GIMBAL_MODE_RECOVER,
// .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL,
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX,
.balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
.balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
.balance_sw_mid = CHASSIS_MODE_RELAX,
.balance_sw_down = CHASSIS_MODE_RELAX,
// .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
// .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
#endif
},
},

78
User/module/gimbal.c
View File

@ -63,17 +63,6 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
}
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
/* 进入 RECOVER 时初始化双零点选择 */
if (mode == GIMBAL_MODE_RECOVER) {
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
float zero0 = g->param->mech_zero.yaw;
float zero1 = zero0 + M_PI;
if (zero1 >= M_2PI) zero1 -= M_2PI;
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
g->recover_yaw_reversed = (err1 < err0);
}
g->mode = mode;
return 0;
}
@ -110,13 +99,13 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.pit_omega));
PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.yaw_angle));
&(g->param->pid.yaw_angle));
PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.yaw_omega));
&(g->param->pid.yaw_omega));
PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.pit_angle));
&(g->param->pid.pit_angle));
PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.pit_omega));
&(g->param->pid.pit_omega));
/* Recover模式PID（单环，编码器反馈） */
PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
@ -227,31 +216,26 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) {
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
if (g_cmd->setpoint_yaw == 0.0f && g_cmd->setpoint_pit == 0.0f) {
/* 自瞄丢失：切回ABSOLUTE，setpoint锁定当前IMU角度，停在原位 */
g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
Gimbal_SetMode(g, GIMBAL_MODE_ABSOLUTE);
} else {
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
}
} else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
/* 双零点选近：mech_zero 和 mech_zero+π，带迟滞避免临界点跳变 */
/* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
float zero0 = g->param->mech_zero.yaw;
float zero1 = zero0 + M_PI;
if (zero1 >= M_2PI) zero1 -= M_2PI;
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
float hysteresis = 0.3f;
if (g->recover_yaw_reversed) {
/* 当前跟踪 zero1，只有 zero0 明显更近才切 */
if (err0 < err1 - hysteresis) {
g->recover_yaw_reversed = false;
}
} else {
/* 当前跟踪 zero0，只有 zero1 明显更近才切 */
if (err1 < err0 - hysteresis) {
g->recover_yaw_reversed = true;
}
}
g->setpoint.eulr.yaw = g->recover_yaw_reversed ? zero1 : zero0;
g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
}
@ -277,25 +261,21 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
break;
case GIMBAL_MODE_AI_CONTROL:
/* --- YAW --- */
// 位置环: Kp * (pos_ref - pos_fdb)
yaw_omega_set_point = PID_Calc(&(g->pid.aimbot.yaw_angle),
g->setpoint.eulr.yaw,
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
// 速度环: Kd * (vel_ref - vel_fdb)，用上位机下发的 yaw_vel 作为前馈参考
g->out.yaw = PID_Calc(&(g->pid.aimbot.yaw_omega),
yaw_omega_set_point + g_cmd->ff_vel_yaw,
g->feedback.imu.gyro.z, 0.0f, g->dt)
+ g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc
yaw_omega_set_point += g_cmd->ff_vel_yaw; /* 速度前馈叠加到角速度设定值 */
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt);
g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; /* 加速度前馈叠加到力矩输出 */
/* --- PITCH --- (反馈轴与 ABSOLUTE 模式一致: eulr.rol / gyro.y) */
pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle),
g->setpoint.eulr.pit,
/* --- PIT --- */
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega),
pit_omega_set_point + g_cmd->ff_vel_pit,
g->feedback.imu.gyro.y, 0.0f, g->dt)
+ g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc
break;
pit_omega_set_point -= g_cmd->ff_vel_pit; /* 速度前馈 */
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt);
g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; /* 加速度前馈 */
break;
case GIMBAL_MODE_RECOVER:
g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);

3
User/module/gimbal.h
View File

@ -41,6 +41,7 @@ typedef struct {
typedef struct {
Gimbal_Mode_t mode;
Gimbal_Mode_t ai_mode; /* AI控制模式下的子模式，用于区分不同的AI控制策略 */
float delta_yaw;
float delta_pit;
/* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */
@ -171,8 +172,6 @@ typedef struct {
Gimbal_Output_t out; /* 云台输出 */
Gimbal_Feedback_t feedback; /* 反馈 */
bool recover_yaw_reversed; /* RECOVER模式下跟踪的是第二个零点(+π) */
} Gimbal_t;
/* Exported functions prototypes -------------------------------------------- */

12
User/module/shoot.c
View File

@ -796,12 +796,12 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
}
}
/* 输出 */
MOTOR_RM_Ctrl(&s->param->motor.fric[0].param);
if(s->param->basic.fric_num>4)
{
MOTOR_RM_Ctrl(&s->param->motor.fric[4].param);
}
MOTOR_RM_Ctrl(&s->param->motor.trig);
// MOTOR_RM_Ctrl(&s->param->motor.fric[0].param);
// if(s->param->basic.fric_num>4)
// {
// MOTOR_RM_Ctrl(&s->param->motor.fric[4].param);
// }
// MOTOR_RM_Ctrl(&s->param->motor.trig);
last_firecmd = cmd->firecmd;
return SHOOT_OK;
}

2
User/task/ctrl_shoot.c
View File

@ -75,7 +75,7 @@ void Task_ctrl_shoot(void *argument) {
shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit;
shoot.heatcontrol.Hcd = (float)shoot_ref.robot_status.shooter_barrel_cooling_value;
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
shoot.heatcontrol.Hgen = 100.0f; /* 42mm弹丸每发产生热量 */
shoot.heatcontrol.Hgen = 10.0f; /* 42mm弹丸每发产生热量 */
}
Shoot_UpdateFeedback(&shoot);
Shoot_SetMode(&shoot,shoot_cmd.mode);

4
User/task/init.c
View File

@ -10,8 +10,10 @@
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "module/cmd/cmd.h"
#include "device/dr16.h"
#include "device/referee.h"
#include "module/aimbot.h"
/* USER INCLUDE END */
@ -62,7 +64,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
task_runtime.msgq.cmd.nuc= osMessageQueueNew(2u, sizeof(Aimbot_AIResult_t), NULL);
/* AI */
task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL);
task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);