diff --git a/CMakeLists.txt b/CMakeLists.txt index 530a8dd..6f0b735 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,7 +93,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/module/cmd/cmd_behavior.c User/module/cmd/cmd_example.c User/module/vision_bridge.c - + User/module/aimbot.c # User/task sources User/task/ai.c User/task/atti_esit.c diff --git a/User/bsp/uart.h b/User/bsp/uart.h index 80ffe91..f931e9d 100644 --- a/User/bsp/uart.h +++ b/User/bsp/uart.h @@ -30,6 +30,7 @@ typedef enum { BSP_UART_DR16, BSP_UART_VOFA, BSP_UART_REF, + BSP_UART_AI, BSP_UART_NUM, BSP_UART_ERR, } BSP_UART_t; diff --git a/User/module/aimbot.c b/User/module/aimbot.c new file mode 100644 index 0000000..d5745be --- /dev/null +++ b/User/module/aimbot.c @@ -0,0 +1,266 @@ +#include "module/aimbot.h" +#include "device/device.h" +#include "bsp/uart.h" +#include "component/crc16.h" +#include + +/* ===================================================================== + * FDCAN 帧索引(反馈方向:下层板?上层板) + * 注:上层板使用自己的IMU,下层板只发送射击数据与模式 + * ===================================================================== */ +#define FB_FRAME_DATA 0u /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B) */ + +/* ===================================================================== + * UART 通信接口(上层板 �?上位�?PC�? + * ===================================================================== */ + +int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai) { + if (BSP_UART_Receive(BSP_UART_AI, (uint8_t *)ai, sizeof(*ai), true) == DEVICE_OK) { + return DEVICE_OK; + } + return DEVICE_ERR; +} + +int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result) { + if (ai->head[0] != 'M' || ai->head[1] != 'R') { + return DEVICE_ERR; + } + if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) { + return DEVICE_ERR; + } + result->mode = ai->mode; + result->gimbal_t.setpoint.yaw = ai->yaw; + result->gimbal_t.vel.yaw = ai->yaw_vel; + result->gimbal_t.accl.yaw = ai->yaw_acc; + result->gimbal_t.setpoint.pit = ai->pitch; + result->gimbal_t.vel.pit = ai->pitch_vel; + result->gimbal_t.accl.pit = ai->pitch_acc; + return DEVICE_OK; +} + +/** + * @brief 打包 MCU 数据(UART 发给上位机格式),修正了原始实现中的字段错误�? + * 四元数来�?quat 参数;欧拉角、角速度来自 gimbal_fb->imu�? + */ +int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb, + const AHRS_Quaternion_t *quat, + float bullet_speed, uint16_t bullet_count, uint8_t mode) { + if (mcu == NULL || gimbal_fb == NULL || quat == NULL) { + return DEVICE_ERR_NULL; + } + mcu->head[0] = 'M'; + mcu->head[1] = 'R'; + mcu->mode = mode; + mcu->q[0] = quat->q0; + mcu->q[1] = quat->q1; + mcu->q[2] = quat->q2; + mcu->q[3] = quat->q3; + mcu->yaw = gimbal_fb->imu.eulr.yaw; + mcu->yaw_vel = gimbal_fb->imu.gyro.z; + mcu->pitch = gimbal_fb->imu.eulr.pit; + mcu->pitch_vel = gimbal_fb->imu.gyro.x; + mcu->bullet_speed = bullet_speed; + mcu->bullet_count = bullet_count; + mcu->crc16 = CRC16_Calc((const uint8_t *)mcu, + sizeof(*mcu) - sizeof(uint16_t), CRC16_INIT); + if (!CRC16_Verify((const uint8_t *)mcu, sizeof(*mcu))) { + return DEVICE_ERR; + } + return DEVICE_OK; +} + +int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu) { + if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)mcu, sizeof(*mcu), true) == DEVICE_OK) { + return DEVICE_OK; + } + return DEVICE_ERR; +} + +/* ===================================================================== + * CAN 通信接口(下层板 �?上层板) + * ===================================================================== */ + +/** + * @brief 初始�?Aimbot CAN 通信:注册指令接收队列和反馈收发队列�? + * 下层板只需注册 cmd_id;上层板只需注册 fb_base_id �?6 �?ID�? + * 本函数同时注册两侧所需 ID,上/下层板共用同一初始化流程即可�? + */ +int8_t Aimbot_Init(Aimbot_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_Init(); + + /* 注册 AI 指令帧队列(下层板接收/上层板发送) */ + BSP_FDCAN_RegisterId(param->can, param->cmd_id, + BSP_FDCAN_DEFAULT_QUEUE_SIZE); + + /* 注册反馈数据帧队列(上层板接收/下层板发送) */ + for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) { + BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i, + BSP_FDCAN_DEFAULT_QUEUE_SIZE); + } + + return DEVICE_OK; +} + +/** + * @brief �?Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体�? +/** + * @brief 【下层板】打包 CAN 反馈结构体。 + */ +int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb, + float bullet_speed, uint16_t bullet_count, + uint8_t mode) { + if (fb == NULL) { + return DEVICE_ERR_NULL; + } + fb->mode = mode; + fb->bullet_speed = bullet_speed; + fb->bullet_count = bullet_count; + return DEVICE_OK; +} + +/** + * @brief 【下层板】将反馈数据打成 1 个 CAN 标准帧发给上层板。 + * + * 帧格式(每帧 8 字节): + * 帧0: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B) + */ +void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param, + const Aimbot_FeedbackData_t *fb) { + if (param == NULL || fb == NULL) return; + + BSP_FDCAN_StdDataFrame_t frame; + frame.dlc = AIMBOT_CAN_DLC; + + /* 帧0: 弹速 + 弹数 + 模式 */ + frame.id = param->fb_base_id + FB_FRAME_DATA; + memcpy(&frame.data[0], &fb->bullet_speed, 4); + frame.data[4] = (uint8_t)(fb->bullet_count & 0xFFu); + frame.data[5] = (uint8_t)((fb->bullet_count >> 8u) & 0xFFu); + frame.data[6] = fb->mode; + frame.data[7] = 0u; + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); +} + +/** + * @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来�?AI 指令并解析�? + * + * 指令帧格式(8 字节,与 vision_bridge 一致)�? + * data[0] : mode (1B) + * data[1..3.5] : yaw (28bit 有符号定点数�?.1µrad/LSB) + * data[4.5..7] : pit (28bit 有符号定点数�?.1µrad/LSB) + * + * @return DEVICE_OK 成功解析到新指令 + * DEVICE_ERR 队列空,无新数据 + */ +int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param, + Aimbot_AIResult_t *result) { + if (param == NULL || result == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_Message_t msg; + int8_t ret = DEVICE_ERR; + + if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg, + BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + result->mode = msg.data[0]; + + /* 解析 yaw(高 28 位),符号扩展为 int32 */ + int32_t yaw_raw = (int32_t)(((uint32_t)msg.data[1] << 20u) | + ((uint32_t)msg.data[2] << 12u) | + ((uint32_t)msg.data[3] << 4u) | + ((uint32_t)(msg.data[4] >> 4u) & 0xFu)); + if (yaw_raw & 0x08000000) yaw_raw |= (int32_t)0xF0000000; + result->gimbal_t.setpoint.yaw = (float)yaw_raw / AIMBOT_ANGLE_SCALE; + + /* 解析 pit(低 28 位),符号扩展为 int32 */ + int32_t pit_raw = (int32_t)(((uint32_t)(msg.data[4] & 0xFu) << 24u) | + ((uint32_t)msg.data[5] << 16u) | + ((uint32_t)msg.data[6] << 8u) | + (uint32_t)msg.data[7]); + if (pit_raw & 0x08000000) pit_raw |= (int32_t)0xF0000000; + result->gimbal_t.setpoint.pit = (float)pit_raw / AIMBOT_ANGLE_SCALE; + ret = DEVICE_OK; + } + + if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 1, &msg, + BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + memcpy(&result->gimbal_t.vel.yaw, &msg.data[0], 4); + memcpy(&result->gimbal_t.vel.pit, &msg.data[4], 4); + ret = DEVICE_OK; + } + + if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 2, &msg, + BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + memcpy(&result->gimbal_t.accl.yaw, &msg.data[0], 4); + memcpy(&result->gimbal_t.accl.pit, &msg.data[4], 4); + ret = DEVICE_OK; + } + + return ret; +} + +/** + * @brief 【上层板】将 AI 指令通过 CAN 发送给下层板(单帧 8 字节)�? + * + * �?vision_bridge.c �?AI_SendCmdOnFDCAN 编码格式完全一致, + * 上层板可直接调用本函数替�?vision_bridge 中的同名函数�? + */ +void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param, + const Aimbot_AIResult_t *cmd) { + if (param == NULL || cmd == NULL) return; + + const int32_t yaw = (int32_t)(cmd->gimbal_t.setpoint.yaw * AIMBOT_ANGLE_SCALE); + const int32_t pit = (int32_t)(cmd->gimbal_t.setpoint.pit * AIMBOT_ANGLE_SCALE); + + BSP_FDCAN_StdDataFrame_t frame = {0}; + frame.id = param->cmd_id; + frame.dlc = AIMBOT_CAN_DLC; + + frame.data[0] = cmd->mode; + frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF); + frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF); + frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF); + frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF)); + frame.data[5] = (uint8_t)((pit >> 16) & 0xFF); + frame.data[6] = (uint8_t)((pit >> 8) & 0xFF); + frame.data[7] = (uint8_t)(pit & 0xFF); + + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); + + /* 第二帧:发速度 */ + frame.id = param->cmd_id + 1; + memcpy(&frame.data[0], &cmd->gimbal_t.vel.yaw, 4); + memcpy(&frame.data[4], &cmd->gimbal_t.vel.pit, 4); + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); + + /* 第三帧:发加速度 */ + frame.id = param->cmd_id + 2; + memcpy(&frame.data[0], &cmd->gimbal_t.accl.yaw, 4); + memcpy(&frame.data[4], &cmd->gimbal_t.accl.pit, 4); + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); +} + +/** + * @brief 【上层板】从 CAN 队列中非阻塞地解析下层板发来的反馈数据(共1帧)。 + * + * @return DEVICE_OK + */ +int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param, + Aimbot_FeedbackData_t *fb) { + if (param == NULL || fb == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_Message_t msg; + + /* 帧0: 弹速 + 弹数 + 模式 */ + if (BSP_FDCAN_GetMessage(param->can, + param->fb_base_id + FB_FRAME_DATA, + &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + memcpy(&fb->bullet_speed, &msg.data[0], 4); + fb->bullet_count = (uint16_t)(msg.data[4] | ((uint16_t)msg.data[5] << 8u)); + fb->mode = msg.data[6]; + } + + return DEVICE_OK; +} + diff --git a/User/module/aimbot.h b/User/module/aimbot.h new file mode 100644 index 0000000..1c87ce4 --- /dev/null +++ b/User/module/aimbot.h @@ -0,0 +1,174 @@ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "component\user_math.h" +#include "module/gimbal.h" +#include "bsp/fdcan.h" +#include + +// 数据包协议定义(UART 通信使用 head[2]={'M', 'R'} 标识) + +/* ============================ CAN 通信定义 ============================ + * 通路:上位机 <--串口--> 上层板 <--CAN--> 下层板 + * + * 注:上层板使用自己的IMU(atti_esti),下层板只需反馈电机和射击数据 + * + * 下层板 → 上层板:反馈数据,单帧 8 字节 + * 帧0 (fb_base_id+0): 弹速(4B) + 弹数(2B) + 模式(1B) + 保留(1B) + * + * 上层板 → 下层板:AI 指令,单帧 8 字节 + * 帧 (cmd_id): mode(1B) + yaw(28bit) + pit(28bit),定点数精度 0.1µrad + * ====================================================================== */ +#define AIMBOT_FB_FRAME_NUM 1u /* 反馈数据帧总数(仅下层三字段) */ +#define AIMBOT_CAN_DLC 8u /* CAN 帧数据长度 */ +#define AIMBOT_ANGLE_SCALE 10000000.0f /* 指令角度定点数比例(0.1µrad/LSB) */ + +/* CAN 通信参数结构体 */ +typedef struct { + BSP_FDCAN_t can; /* 使用的 CAN 总线实例 */ + uint32_t cmd_id; /* 上层板→下层板 AI 指令帧 CAN ID */ + uint32_t fb_base_id; /* 下层板→上层板 反馈数据起始 CAN ID */ +} Aimbot_Param_t; + +/* CAN 反馈数据结构体(下层板打包后经 CAN 发给上层板的内容) + * 注:下层板只反馈 bullet_speed / bullet_count / mode */ +typedef struct { + uint8_t mode; /* 下层板当前工作模式 */ + float bullet_speed; /* 弹速(m/s) */ + uint16_t bullet_count; /* 子弹累计发射次数 */ +} Aimbot_FeedbackData_t; + +/*-----------------------------to上位机---------------------------------*/ +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 数据包头: {'M', 'R'} + uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 + float q[4]; // 四元数 wxyz 顺序 + float yaw; // 偏航角 + float yaw_vel; // 偏航角速度 + float pitch; // 俯仰角 + float pitch_vel; // 俯仰角速度 + float bullet_speed; // 弹速 + uint16_t bullet_count; // 子弹累计发送次数 + uint16_t crc16; // CRC16 校验 +} Aimbot_MCU_t; + +// 裁判系统数据结构 +typedef struct __attribute__((packed)) +{ + uint16_t remain_hp; // 剩余血量 + uint8_t game_progress : 4; // 比赛进度 + uint16_t stage_remain_time; // 比赛剩余时间 +} Aimbot_RefereeData_t; + +typedef struct __attribute__((packed)) +{ + uint8_t id; // 数据包 ID: 0xA8 + Aimbot_RefereeData_t data; + uint16_t crc16; +} Aimbot_Referee_t; + +/*------------------------------------上位机back-------------------------------------*/ +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 数据包头: {'M', 'R'} + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火,2: 控制云台且开火 + float yaw; // 目标偏航角 + float yaw_vel; // 偏航角速度 + float yaw_acc; // 偏航角加速度 + float pitch; // 目标俯仰角 + float pitch_vel; // 俯仰角速度 + float pitch_acc; // 俯仰角加速度 + uint16_t crc16; // CRC16 校验 +} Aimbot_AI_t; + +typedef struct __attribute__((packed)) { + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 + struct{ +// Gimbal_CMD_t g_cmd; + struct{ + float yaw; // 目标偏航角 + float pit; // 目标俯仰角 + }setpoint; + + struct{ + float pit; // 俯仰角加速度 + float yaw; // 偏航角加速度 + }accl; + struct{ + float pit; // 俯仰角速度 + float yaw; // 偏航角速度 + }vel; + }gimbal_t; + + struct{ + float Vx; // X 方向速度 + float Vy; // Y 方向速度 + float Vw; // Z 方向角速度 + }chassis_t; + + uint8_t reserved; // 预留字段 +} Aimbot_AIResult_t; // 解析后的AI控制指令 + +/* ---------- UART 通信接口(上层板与上位机 PC 间) ---------- */ +int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb, + const AHRS_Quaternion_t *quat, + float bullet_speed, uint16_t bullet_count, uint8_t mode); +int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu); +int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result); +int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai); + +/* ---------- CAN 通信接口(下层板与上层板间) ---------- */ + +/** + * @brief 初始化 Aimbot CAN 通信,注册所有收发 CAN ID 队列 + * @param param CAN 通信参数 + * @return DEVICE_OK / DEVICE_ERR_NULL + */ +int8_t Aimbot_Init(Aimbot_Param_t *param); + +/** + * @brief 【下层板】打包反馈结构体 + * @param fb 输出:反馈数据 + * @param bullet_speed 弹速(m/s) + * @param bullet_count 已发射子弹数 + * @param mode 当前工作模式 + */ +int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb, + float bullet_speed, uint16_t bullet_count, + uint8_t mode); + +/** + * @brief 【下层板】将反馈数据经 CAN 发送给上层板(1 帧) + */ +void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param, + const Aimbot_FeedbackData_t *fb); + +/** + * @brief 【下层板】从 CAN 解析上层板发来的 AI 指令 + * @return DEVICE_OK 表示成功取到新数据;DEVICE_ERR 表示队列暂无数据 + */ +int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param, + Aimbot_AIResult_t *result); + +/** + * @brief 【上层板】通过 CAN 将 AI 指令发送给下层板(单帧 8 字节) + */ +void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param, + const Aimbot_AIResult_t *cmd); + +/** + * @brief 【上层板】从 CAN 解析下层板发来的反馈数据(逐帧读取,非阻塞) + * @return DEVICE_OK(部分或全部帧已更新均返回 OK) + */ +int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param, + Aimbot_FeedbackData_t *fb); + +#ifdef __cplusplus +} +#endif diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index c13f9e5..1b96cb0 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -228,10 +228,7 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { /* 根据AI模式决定射击行为 */ switch (ctx->input.nuc.mode) { case 0: - ctx->output.shoot.cmd.ready = false; - ctx->output.shoot.cmd.firecmd = false; - break; - case 1: + case 1: ctx->output.shoot.cmd.ready = true; ctx->output.shoot.cmd.firecmd = false; break; diff --git a/User/module/cmd/cmd_feature.h b/User/module/cmd/cmd_feature.h index e76a57a..0f9955e 100644 --- a/User/module/cmd/cmd_feature.h +++ b/User/module/cmd/cmd_feature.h @@ -16,8 +16,8 @@ /** PC 端键鼠输入 (通过 DR16 转发) */ #define CMD_ENABLE_SRC_PC 1 -/** NUC / AI 输入 (需要 vision_bridge 模块) */ -#define CMD_ENABLE_SRC_NUC 0 +/** NUC / AI 输入 */ +#define CMD_ENABLE_SRC_NUC 1 /** * 裁判系统数据中转开关 @@ -46,7 +46,7 @@ #define CMD_ENABLE_MODULE_ARM 0 /** 裁判系统UI命令模块 (需要 device/referee.h) */ -#define CMD_ENABLE_MODULE_REFUI 0 +#define CMD_ENABLE_MODULE_REFUI 1 /** 平衡底盘模块 (需要 module/balance_chassis.h) */ #define CMD_ENABLE_MODULE_BALANCE_CHASSIS 1 diff --git a/User/module/config.c b/User/module/config.c index ad087c9..2bc1baa 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -24,6 +24,11 @@ Config_RobotParam_t robot_config = { .width=1920, .height=1080, }, + .aimbot_param = { + .can = BSP_FDCAN_2, + .cmd_id = 0x520, /* 上层板→下层板 AI指令帧ID (共3帧: 0x520~0x522) */ + .fb_base_id = 0x524, /* 下层板→上层板 反馈起始ID */ + }, .gimbal_param = { .pid = { .yaw_omega = { diff --git a/User/module/config.h b/User/module/config.h index abdf5b0..cec9f02 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -16,6 +16,7 @@ extern "C" { #include "module/vision_bridge.h" #include "module/cmd/cmd.h" #include "device/referee_proto_types.h" +#include "module/aimbot.h" /** * @brief 机器人参数配置结构体 * @note 在此添加您的配置参数 @@ -28,6 +29,7 @@ typedef struct { AI_Param_t ai_param; CMD_Config_t cmd_param; Referee_Screen_t ref_screen; + Aimbot_Param_t aimbot_param; /* 下层板 ↔ 上层板 CAN 通信参数 */ /* USER CODE END Config_RobotParam */ } Config_RobotParam_t; diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 47daf19..bca9898 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -19,7 +19,8 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */ - +#define GIMBAL_YAW_INERTIA 0.05f +#define GIMBAL_PIT_INERTIA 0.03f /** * \brief 设置云台模式 * @@ -41,10 +42,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { PID_Reset(&g->pid.yaw_omega); PID_Reset(&g->pid.pit_angle); PID_Reset(&g->pid.pit_omega); + PID_Reset(&g->pid.aimbot.yaw_angle); + PID_Reset(&g->pid.aimbot.yaw_omega); + PID_Reset(&g->pid.aimbot.pit_angle); + PID_Reset(&g->pid.aimbot.pit_omega); LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); MOTOR_DM_Enable(&(g->param->yaw_motor)); + MOTOR_DM_Enable(&(g->param->pit_motor)); AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ @@ -91,6 +97,14 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, &(g->param->pid.pit_angle)); 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)); + PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.yaw_omega)); + PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.pit_angle)); + PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.pit_omega)); LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, g->param->low_pass_cutoff_freq.out); @@ -193,33 +207,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->limit.pit.max, g->limit.pit.min, M_2PI); } 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; + } /* 控制相关逻辑 */ float yaw_omega_set_point, pit_omega_set_point; switch (g->mode) { case GIMBAL_MODE_RELAX: g->out.yaw = 0.0f; g->out.pit = 0.0f; - break; - case GIMBAL_MODE_AI_CONTROL: - if (g_ai != NULL && g_ai->ctrl) { - g->setpoint.eulr.yaw = g_ai->yaw; - g->setpoint.eulr.pit = -g_ai->pit; - - /* 限位处理 */ - if (g->param->travel.yaw > 0) { - CircleAngleLimit(&g->setpoint.eulr.yaw, - g->feedback.motor.yaw.rotor_abs_angle, - g->feedback.imu.eulr.yaw, - g->limit.yaw.max, g->limit.yaw.min, M_2PI); - } - if (g->param->travel.pit > 0) { - CircleAngleLimit(&g->setpoint.eulr.pit, - g->feedback.motor.pit.rotor_abs_angle, - g->feedback.imu.eulr.rol, - g->limit.pit.max, g->limit.pit.min, M_2PI); - } - } /* fallthrough - AI控制模式也需要执行PID计算 */ case GIMBAL_MODE_ABSOLUTE: @@ -234,6 +233,28 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, g->feedback.imu.gyro.y, 0.f, g->dt); + break; + 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, + 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 + + /* --- PITCH --- */ + pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle), + g->setpoint.eulr.pit, + g->feedback.imu.eulr.pit, 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.x, 0.0f, g->dt) + + g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc break; } diff --git a/User/module/gimbal.h b/User/module/gimbal.h index e9a03e9..92ddd5f 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -42,6 +42,13 @@ typedef struct { Gimbal_Mode_t mode; float delta_yaw; float delta_pit; + /* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈(来自 NUC) */ + float setpoint_yaw; + float setpoint_pit; + float ff_vel_yaw; /* yaw 速度前馈(rad/s),叠加到内环角速度设定值 */ + float ff_vel_pit; /* pit 速度前馈(rad/s) */ + float ff_accl_yaw; /* yaw 加速度前馈(归一化),叠加到力矩输出 */ + float ff_accl_pit; /* pit 加速度前馈(归一化) */ } Gimbal_CMD_t; typedef struct { @@ -66,6 +73,12 @@ typedef struct { KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */ KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ + struct { + KPID_t yaw_angle; /* yaw轴角位置环PID */ + KPID_t yaw_omega; /* yaw轴角速度环PID */ + KPID_t pit_angle; /* pitch轴角位置环PID */ + KPID_t pit_omega; /* pitch轴角速度环PID */ + } aimbot; } pid; /* 低通滤波器截止频率 */ @@ -128,6 +141,12 @@ typedef struct { KPID_t yaw_omega; /* yaw轴角速度环PID */ KPID_t pit_angle; /* pitch轴角位置环PID */ KPID_t pit_omega; /* pitch轴角速度环PID */ + struct { + KPID_t yaw_angle; /* yaw轴角位置环PID */ + KPID_t yaw_omega; /* yaw轴角速度环PID */ + KPID_t pit_angle; /* pitch轴角位置环PID */ + KPID_t pit_omega; /* pitch轴角速度环PID */ + } aimbot; } pid; struct { diff --git a/User/module/shoot.c b/User/module/shoot.c index 1774165..847efa6 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -41,15 +41,18 @@ void Task(void *argument) { /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ #define MAX_FRIC_RPM 7000.0f -#define MAX_TRIG_RPM 7000.0f//这里可能也会影响最高发射频率,待测试 +#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率,待测试 /* 发射检测参数 */ #define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值,单位rpm */ #define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值,单位秒 */ +#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ static bool last_firecmd; + +float maxTrigrpm=4000.0f; /* Private function -------------------------------------------------------- */ /** @@ -180,7 +183,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) s->target_variable.fric_rpm=5000.0f; break; case SHOOT_PROJECTILE_42MM: - s->target_variable.fric_rpm=6500.0f;//6500 + s->target_variable.fric_rpm=5000.0f;//6500 break; } return SHOOT_OK; @@ -207,7 +210,7 @@ static int8_t Shoot_UpdateHeatControl(Shoot_t *s) s->heatcontrol.ncd = s->heatcontrol.Hcd / s->heatcontrol.Hgen; } else { s->heatcontrol.ncd = 0.0f; - } + } return SHOOT_OK; } @@ -235,6 +238,40 @@ static float Shoot_CalcAvgFricRpm(Shoot_t *s) return (fric_num > 0) ? (sum / fric_num) : 0.0f; } +/** + * \brief 发射检测:通过摩擦轮掉速检测发射 + * + * \param s 包含发射数据的结构体 + * + * \return 是否检测到掉速超过阈值 + */ +static bool Shoot_DetectShotByRpmDrop(Shoot_t *s) +{ + if (s == NULL) { + return false; + } + + /* 只在READY和FIRE状态下检测,避免停机时误判 */ + if (s->running_state != SHOOT_STATE_READY && + s->running_state != SHOOT_STATE_FIRE) { + return false; + } + + /* 计算当前摩擦轮平均转速 */ + s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); + + /* 计算当前转速与目标转速的差值(掉速量) */ + s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; + + /* 判断是否发生明显掉速 */ + if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) { + return true; + } + + return false; +} + + /** * \brief 热量数据融合:用裁判系统数据修正自主估计 * @@ -318,29 +355,18 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) /* 计算当前平均转速 */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - /* 检查摩擦轮是否初步达到设定速度 */ - if (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f) { + /* 检查摩擦轮是否达到目标转速的85%以上 */ + if (s->heatcontrol.avgFricRpm >= s->target_variable.fric_rpm * FRIC_READY_RPM_RATIO) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; - s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; // 记录初始基准转速 + s->heatcontrol.lastAvgFricRpm = s->heatcontrol.avgFricRpm; } } break; case SHOOT_HEAT_DETECT_READY: - /* 准备检测状态:监测摩擦轮相对当前的基准发生掉速 */ - s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - - /* 动态更新平稳时的基准转速:若当前转速比基准高,迅速成为新基准;若更低,基准非常缓慢地衰减,以适应正常波动 */ - if (s->heatcontrol.avgFricRpm > s->heatcontrol.baselineRpm) { - s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; - } else { - s->heatcontrol.baselineRpm = s->heatcontrol.baselineRpm * 0.999f + s->heatcontrol.avgFricRpm * 0.001f; - } - - s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; - - /* 检测掉速(当前转速低于动态基准超过阈值) */ - if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) { + /* 准备检测状态:监测摩擦轮掉速 */ + /* 检测掉速(当前转速低于目标转速超过阈值) */ + if (Shoot_DetectShotByRpmDrop(s)) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED; s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */ } @@ -353,47 +379,45 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) case SHOOT_HEAT_DETECT_SUSPECTED: /* 发射嫌疑状态:持续检测掉速 */ + /* 更新掉速量 */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; + s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; - /* 若掉速消失(转速刚掉下又马上弹回基准附近),回到准备状态 (阈值的 60%) */ - if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.6f) { + /* 若掉速消失(转速恢复正常),回到准备状态 */ + if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; } - /* 若掉速状态持续超过设定的嫌疑时间,确认这是一次发射 */ + /* 若嫌疑状态持续超过阈值时间,确认发射 */ else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED; } break; case SHOOT_HEAT_DETECT_CONFIRMED: - if (s->heatcontrol.unverified_shots > 0) { - s->heatcontrol.unverified_shots--; - } - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_RECOVERING; - s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; /* 记录进入此时的转速为谷底初值 */ - break; - - case SHOOT_HEAT_DETECT_RECOVERING: - - s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - - if (s->heatcontrol.avgFricRpm < s->heatcontrol.valleyRpm) { - s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; + /* 确认发射状态:增加热量并返回准备状态 */ + /* 根据弹丸类型增加估计热量 */ + switch (s->param->basic.projectileType) { + case SHOOT_PROJECTILE_17MM: + s->heatcontrol.Hnow_estimated += 10.0f; + break; + case SHOOT_PROJECTILE_42MM: + s->heatcontrol.Hnow_estimated += 100.0f; + break; + default: + s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen; + break; } - if ((s->heatcontrol.avgFricRpm - s->heatcontrol.valleyRpm > 30.0f) || - (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f)) { - - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; - - s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; + /* 限制估计热量不超过最大值 */ + if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { + s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; } - /* 如果摩擦轮停止 */ - if (s->running_state == SHOOT_STATE_IDLE) { - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE; - } + /* 增加发射计数 */ + s->heatcontrol.shots_detected++; + + /* 返回准备检测状态 */ + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; break; default: @@ -401,7 +425,7 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) break; } - /* 热量冷却*/ + /* 善后工作:热量冷却(每个周期都执行) */ if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) { s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt; if (s->heatcontrol.Hnow_estimated < 0.0f) { @@ -409,36 +433,6 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) } } - /* 空弹链检测与修正:拨盘触发后超过0.4秒内仍未有对应的摩擦轮掉速确认 */ - if (s->heatcontrol.unverified_shots > 0 && (s->timer.now - s->var_trig.time_lastShoot) > 0.4f) { - /* 这些弹丸确定未射出(空发) */ - float refund_heat = 0.0f; - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: refund_heat = 10.0f; break; - case SHOOT_PROJECTILE_42MM: refund_heat = 100.0f; break; - default: refund_heat = s->heatcontrol.Hgen; break; - } - refund_heat *= s->heatcontrol.unverified_shots; - - s->heatcontrol.Hnow_estimated -= refund_heat; - if (s->heatcontrol.Hnow_estimated < 0.0f) { - s->heatcontrol.Hnow_estimated = 0.0f; - } - - if (s->var_trig.num_shooted >= s->heatcontrol.unverified_shots) { - s->var_trig.num_shooted -= s->heatcontrol.unverified_shots; - } else { - s->var_trig.num_shooted = 0; - } - - if (s->heatcontrol.shots_detected >= s->heatcontrol.unverified_shots) { - s->heatcontrol.shots_detected -= s->heatcontrol.unverified_shots; - } else { - s->heatcontrol.shots_detected = 0; - } - - s->heatcontrol.unverified_shots = 0; - } /* 数据融合 */ Shoot_FuseHeatData(s); @@ -508,22 +502,18 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s) */ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { - if (s == NULL) { + if (s == NULL || s->var_trig.num_toShoot == 0) { return SHOOT_ERR_NULL; } /* 更新热量控制数据 */ Shoot_UpdateHeatControl(s); - if (s->var_trig.num_toShoot == 0) { - return SHOOT_OK; /* 即使没有弹丸待发,热量更新照常执行 */ - } - /* 根据热量控制计算实际射频 */ float actual_freq = Shoot_CaluFreqByHeat(s); float dt = s->timer.now - s->var_trig.time_lastShoot; - float dpos; + float dpos; dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); /* 使用热量控制计算出的射频,而不是配置的固定射频 */ @@ -531,25 +521,10 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { s->var_trig.time_lastShoot=s->timer.now; CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); - s->var_trig.num_toShoot--; - s->var_trig.num_shooted++; - - /* 预测性发射:拨盘拨动即认为弹丸会射出,立即增加热量与发射计数 */ - float add_heat = 0.0f; - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: add_heat = 10.0f; break; - case SHOOT_PROJECTILE_42MM: add_heat = 100.0f; break; - default: add_heat = s->heatcontrol.Hgen; break; - } - s->heatcontrol.Hnow_estimated += add_heat; - if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { - s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; - } - s->heatcontrol.shots_detected++; - - /* 增加等待摩擦轮掉速确认的弹丸数 */ - s->heatcontrol.unverified_shots++; + s->var_trig.num_toShoot--; + s->var_trig.num_shooted++; } + return SHOOT_OK; } @@ -630,7 +605,7 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s) s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl; } s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed); - s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / MAX_TRIG_RPM; + s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm; if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f; if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f; @@ -652,17 +627,16 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) return SHOOT_ERR_NULL; // 参数错误 } uint8_t fric_num = s->param->basic.fric_num; - + static float pos; if(s->mode==SHOOT_MODE_SAFE){ for(int i=0;iparam->motor.fric[i].param); } - MOTOR_RM_Relax(&s->param->motor.trig); - s->target_variable.trig_angle=s->var_trig.trig_agl; + MOTOR_RM_Relax(&s->param->motor.trig);\ + pos=s->target_variable.trig_angle=s->var_trig.trig_agl; } else{ - Shoot_CaluTargetAngle(s, cmd); switch(s->running_state) { case SHOOT_STATE_IDLE:/*熄火等待*/ @@ -675,7 +649,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]); } - s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.trig_angle,s->var_trig.trig_agl,0,s->timer.dt); + s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt); s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig); @@ -719,7 +693,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) } /* 设置拨弹电机输出 */ s->output.outagl_trig =PID_Calc(&s->pid.trig, - s->target_variable.trig_angle, + pos, s->var_trig.trig_agl, 0, s->timer.dt); @@ -761,6 +735,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) break; case SHOOT_STATE_FIRE:/*射击*/ + Shoot_CaluTargetAngle(s, cmd); for(int i=0;iparam->motor.fric[i].level-1; @@ -805,7 +780,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) if(!cmd->firecmd) { s->running_state=SHOOT_STATE_READY; - s->target_variable.trig_angle=s->var_trig.trig_agl; + pos=s->var_trig.trig_agl; s->var_trig.num_toShoot=0; } break; @@ -991,10 +966,10 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd) s->timer.now = BSP_TIME_Get_us() / 1000000.0f; s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f; s->timer.lask_wakeup = BSP_TIME_Get_us(); - Shoot_CaluTargetRPM(s,233); + // Shoot_CaluTargetRPM(s,233); /* 运行热量检测状态机 */ - Shoot_HeatDetectionFSM(s); + // Shoot_HeatDetectionFSM(s); /* 运行卡弹检测状态机 */ Shoot_JamDetectionFSM(s, cmd); @@ -1013,3 +988,7 @@ void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui) { ui->fire = s->running_state; } +void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) { + ai->num_shooted=s->var_trig.num_shooted; + ai->bullet_speed=12.0f; +} diff --git a/User/module/shoot.h b/User/module/shoot.h index cd50342..0f14916 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -35,8 +35,7 @@ typedef enum { SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */ SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */ SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */ - SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */ - SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */ + SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */ }Shoot_HeatDetectionFSM_State_t; typedef enum { SHOOT_STATE_IDLE = 0,/* 熄火 */ @@ -52,6 +51,11 @@ typedef enum { SHOOT_MODE_NUM }Shoot_Mode_t; +typedef struct { + uint16_t num_shooted;/* 已发射弹数 */ + float bullet_speed;/* 目标弹速 */ +}Shoot_AI_t; + /* UI 导出结构(供 referee 系统绘制) */ typedef struct { Shoot_Mode_t mode; @@ -115,8 +119,7 @@ typedef struct { Shoot_HeatDetectionFSM_State_t detectState; // 检测状态 float detectTime; // 检测计时器 float avgFricRpm; // 摩擦轮平均转速 - float baselineRpm; // 动态基准转速,用于连发掉速检测的参照 - float valleyRpm; // 掉速谷底转速,用于判断掉速回升 + float lastAvgFricRpm; // 上次摩擦轮平均转速 float rpmDrop; // 转速下降量 bool shotDetected; // 检测到发射标志 @@ -125,7 +128,6 @@ typedef struct { float Hnow_fused; // 融合后的热量值 uint16_t shots_detected; // 检测到的发射数 uint16_t shots_available;// 当前热量可发射弹数 - uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数 }Shoot_HeatControl_t; typedef struct { @@ -219,6 +221,7 @@ typedef struct { struct { float fric_rpm; /* 目标摩擦轮转速 */ float trig_angle;/* 目标拨弹位置 */ + float bullet_speed;/* 目标弹速 */ }target_variable; /* 反馈控制用的PID */ @@ -295,7 +298,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd); */ void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui); - +void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) ; #ifdef __cplusplus } #endif diff --git a/User/task/ai.c b/User/task/ai.c index ea31f72..3929bf7 100644 --- a/User/task/ai.c +++ b/User/task/ai.c @@ -1,15 +1,16 @@ /* - ai Task - + AI Task - 下层板 */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "bsp/fdcan.h" +#include "module/aimbot.h" #include "module/config.h" #include "module/gimbal.h" -#include "module/vision_bridge.h" +#include "module/shoot.h" +#include "device/referee_proto_types.h" +#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -17,20 +18,20 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -AI_cmd_t cmd_ai; -AI_t ai; -Gimbal_AI_t ai_for_gimbal; +static Aimbot_Param_t aimbot_can; /* CAN 通信参数(从 config 拷贝) */ +static Aimbot_FeedbackData_t lower_board_fb; /* 打包好发给上层板的数据 */ +static Aimbot_AIResult_t ai_cmd_from_can; /* 上层板下发并传到底板的指令 */ +static AI_cmd_t for_cmdnuc; /* 转换后发往 cmd.nuc 的指令 */ + +static Referee_ForAI_t ai_ref; /* 裁判系统转发给 AI 的数据 */ +static Shoot_AI_t raw_shoot; /* 发射机构反馈 */ /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* USER PRIVATE CODE BEGIN */ - -/* USER PRIVATE CODE END */ /* Exported functions ------------------------------------------------------- */ void Task_ai(void *argument) { (void)argument; /* 未使用argument,消除警告 */ - /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ; @@ -38,25 +39,49 @@ void Task_ai(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - AI_Init(&ai, &Config_GetRobotParam()->ai_param); - /* 注册CAN接收ID */ + aimbot_can = Config_GetRobotParam()->aimbot_param; + Aimbot_Init(&aimbot_can); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - AI_ParseCmdFromCan( &ai,&cmd_ai); - if (cmd_ai.mode != 0){ - ai_for_gimbal.ctrl = true; - } else { - ai_for_gimbal.ctrl = false; + + /* 1. 读取裁判系统数据(获知机器人系统状态/弹速等) */ + osMessageQueueGet(task_runtime.msgq.ai.ref, &ai_ref, NULL, 0); + + /* 2. 读取发射机构反馈数据(非阻塞),以获取弹速、弹数等信息 + todo: 检查是否有单独发弹速消息队列 + */ + osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromShoot, &raw_shoot, NULL, 0); + + /* 3. 打包当前底层板的数据(弹速、弹数、模式),通过CAN发送给上层板 + 由于现在只能拿到一部分,预留测试值,实车时接入射击模块返回的信息 + */ + // TODO: 使用实际读取到的 bullet_speed 和 bullet_count + float current_bullet_speed = raw_shoot.bullet_speed; + uint16_t current_bullet_count = raw_shoot.num_shooted; + uint8_t current_mode = 1; + + Aimbot_PackFeedback(&lower_board_fb, current_bullet_speed, current_bullet_count, current_mode); + Aimbot_SendFeedbackOnCAN(&aimbot_can, &lower_board_fb); + + /* 4. 解析上层板发来的 AI 指令(非阻塞提取) */ + if (Aimbot_ParseCmdFromCAN(&aimbot_can, &ai_cmd_from_can) == DEVICE_OK) { + /* 将 Aimbot_AIResult_t 转换为 AI_cmd_t 并推送到 cmd.nuc 队列 */ + for_cmdnuc.mode = ai_cmd_from_can.mode; + for_cmdnuc.gimbal.setpoint.yaw = ai_cmd_from_can.gimbal_t.setpoint.yaw; + for_cmdnuc.gimbal.setpoint.pit = ai_cmd_from_can.gimbal_t.setpoint.pit; + for_cmdnuc.gimbal.vel.yaw = ai_cmd_from_can.gimbal_t.vel.yaw; + for_cmdnuc.gimbal.vel.pit = ai_cmd_from_can.gimbal_t.vel.pit; + for_cmdnuc.gimbal.accl.yaw = ai_cmd_from_can.gimbal_t.accl.yaw; + for_cmdnuc.gimbal.accl.pit = ai_cmd_from_can.gimbal_t.accl.pit; + + osMessageQueueReset(task_runtime.msgq.cmd.nuc); + osMessageQueuePut(task_runtime.msgq.cmd.nuc, &for_cmdnuc, 0, 0); } - ai_for_gimbal.yaw = cmd_ai.gimbal.setpoint.yaw; - ai_for_gimbal.pit = cmd_ai.gimbal.setpoint.pit; - osMessageQueueReset(task_runtime.msgq.gimbal.ai_cmd); - osMessageQueuePut(task_runtime.msgq.gimbal.ai_cmd, &ai_for_gimbal, 0, 0); + /* USER CODE END */ osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */ } - } \ No newline at end of file diff --git a/User/task/cmd.c b/User/task/cmd.c index 58f338f..9f2193c 100644 --- a/User/task/cmd.c +++ b/User/task/cmd.c @@ -13,7 +13,7 @@ #include "module/shoot.h" #include "module/cmd/cmd.h" #include "bsp/fdcan.h" -#include "module/vision_bridge.h" + //#define DEAD_AREA 0.05f /* USER INCLUDE END */ @@ -28,7 +28,7 @@ DR16_t cmd_dr16; #elif CMD_RCTypeTable_Index == 1 AT9S_t cmd_at9s; #endif -// AI_cmd_t cmd_ai; +AI_cmd_t cmd_ai; #if CMD_ENABLE_SRC_REF CMD_RawInput_REF_t cmd_ref; @@ -53,8 +53,6 @@ void Task_cmd(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); - // AI_Init(&ai, &Config_GetRobotParam()->ai_param); - /* 注册CAN接收ID */ /* USER CODE INIT END */ while (1) { @@ -65,10 +63,7 @@ void Task_cmd(void *argument) { #elif CMD_RCTypeTable_Index == 1 osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); #endif - - // /* 从CAN2接收AI命令 */ - // AI_ParseCmdFromCan( &ai,&cmd_ai); - // #error "弄好自瞄之后统一改" + osMessageQueueGet(task_runtime.msgq.cmd.nuc, &cmd_ai, NULL, 0); #if CMD_ENABLE_SRC_REF /* 从裁判系统读取最新数据(非阻塞) */ diff --git a/User/task/init.c b/User/task/init.c index a58c4e2..6093a57 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -61,7 +61,12 @@ 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); - /* 裁判系统 */ + + /* 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); + task_runtime.msgq.ai.rawdata_FromShoot = osMessageQueueNew(2u, sizeof(Shoot_AI_t), NULL); + /* 裁判系统 */ task_runtime.msgq.referee.tocmd.ai= osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL); task_runtime.msgq.referee.tocmd.cap= osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL); task_runtime.msgq.referee.tocmd.chassis= osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL); diff --git a/User/task/user_task.h b/User/task/user_task.h index 082c1e9..5ada786 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -87,14 +87,13 @@ typedef struct { struct { osMessageQueueId_t rc; osMessageQueueId_t ref; - osMessageQueueId_t ai; + osMessageQueueId_t nuc; }cmd; - struct { - osMessageQueueId_t quat; - osMessageQueueId_t move_vec; - osMessageQueueId_t eulr; - osMessageQueueId_t fire; - osMessageQueueId_t ref; + struct{ + osMessageQueueId_t rawdata_FromGimbal; + osMessageQueueId_t rawdata_FromShoot; + osMessageQueueId_t rawdata_FromIMU; + osMessageQueueId_t ref; /* Referee_ForAI_t, cmd转发 */ }ai; struct{ osMessageQueueId_t ref; /* Referee_ForCap_t, cmd转发 */