From 5133123dc32f9cc0810c6f736fde3efaca26ce10 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 18 Mar 2026 23:58:49 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E8=87=AA=E7=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/aimbot.c | 5 ++- User/module/balance_chassis.c | 37 +++++------------ User/module/cmd/cmd.c | 25 ++++++----- User/module/config.c | 10 +++-- User/module/gimbal.c | 78 +++++++++++++---------------------- User/module/gimbal.h | 3 +- User/module/shoot.c | 12 +++--- User/task/ctrl_shoot.c | 2 +- User/task/init.c | 4 +- 9 files changed, 75 insertions(+), 101 deletions(-) diff --git a/User/module/aimbot.c b/User/module/aimbot.c index d5745be..09e2c84 100644 --- a/User/module/aimbot.c +++ b/User/module/aimbot.c @@ -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, diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 0e5824a..3db59d7 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -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, ¶m->leg_length); PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length); - PID_Init(&c->pid.yaw, KPID_MODE_SET_D, target_freq, ¶m->yaw); + PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw); PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, ¶m->rotor); PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, ¶m->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控制 ==================== */ diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index b699c5a..f1af954 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -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; } diff --git a/User/module/config.c b/User/module/config.c index 1308e75..b16f7ed 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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 }, }, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 970b773..f81688f 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -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); diff --git a/User/module/gimbal.h b/User/module/gimbal.h index faaeaa4..abf7a61 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -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 -------------------------------------------- */ diff --git a/User/module/shoot.c b/User/module/shoot.c index 473f657..6470efd 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -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; } diff --git a/User/task/ctrl_shoot.c b/User/task/ctrl_shoot.c index 4c232b3..507f015 100644 --- a/User/task/ctrl_shoot.c +++ b/User/task/ctrl_shoot.c @@ -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); diff --git a/User/task/init.c b/User/task/init.c index f2466b9..1b29916 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -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);