diff --git a/CMakeLists.txt b/CMakeLists.txt index 09f217d..530a8dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,6 +108,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/task/cli.c User/task/user_task.c User/task/vofa.c + User/task/cmd.c + User/task/ref_main.c ) # Add include paths diff --git a/User/component/user_math.c b/User/component/user_math.c index 5e0b0c4..def1999 100644 --- a/User/component/user_math.c +++ b/User/component/user_math.c @@ -128,7 +128,21 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { // __NOP(); // } // } - +/* USER FUNCTION BEGIN */ + /** + * @brief 按比例缩放 + * + * @param a 传入值1 + * @param b 传入值2 + */ +inline void ScaleSumTo1(float *a, float *b) { + float sum = *a + *b; + if (sum > 1.0f) { + float scale = 1.0f / sum; + *a *= scale; + *b *= scale; + } +} /* USER FUNCTION BEGIN */ /* USER FUNCTION END */ \ No newline at end of file diff --git a/User/component/user_math.h b/User/component/user_math.h index 6e61ca2..2d36599 100644 --- a/User/component/user_math.h +++ b/User/component/user_math.h @@ -174,6 +174,7 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); // */ // void VerifyFailed(const char *file, uint32_t line); +void ScaleSumTo1(float *a, float *b); /* USER FUNCTION BEGIN */ /* USER FUNCTION END */ \ No newline at end of file diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 1c657f0..60d1fb6 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1108,5 +1108,5 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui) { ui->mode = c->mode; // ui->angle = c->feedback.yaw.rotor_abs_angle - c->mech_zero; - #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪" + // #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪" } \ No newline at end of file diff --git a/User/module/cmd/cmd_feature.h b/User/module/cmd/cmd_feature.h index e8bbdea..e76a57a 100644 --- a/User/module/cmd/cmd_feature.h +++ b/User/module/cmd/cmd_feature.h @@ -24,7 +24,7 @@ * 1 (比赛模式): cmd 将 referee 数据转发到各模块的 .ref 队列 * 0 (调试模式): cmd 不转发,裁判系统可断开,不影响其他功能 */ -#define CMD_ENABLE_SRC_REF 0 +#define CMD_ENABLE_SRC_REF 1 /* ========================================================================== */ /* 输出模块使能开关 */ diff --git a/User/module/cmd/cmd_types.h b/User/module/cmd/cmd_types.h index 9c3a238..818a593 100644 --- a/User/module/cmd/cmd_types.h +++ b/User/module/cmd/cmd_types.h @@ -197,6 +197,7 @@ typedef struct { } CMD_RawInput_NUC_t; #if CMD_ENABLE_SRC_REF +#include "device/referee_proto_types.h" /* 裁判系统原始输入,包含需转发给各模块的完整子集 */ typedef struct { Referee_ForChassis_t chassis; diff --git a/User/module/config.c b/User/module/config.c index 809272c..0ee13be 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -20,7 +20,10 @@ */ Config_RobotParam_t robot_config = { /* USER CODE BEGIN robot_config */ - + .ref_screen={ + .width=1920, + .height=1080, + }, .gimbal_param = { .pid = { .yaw_omega = { @@ -464,6 +467,7 @@ Config_RobotParam_t robot_config = { #endif }, }, + /* USER CODE END robot_config */ }; diff --git a/User/module/config.h b/User/module/config.h index ab4f578..abdf5b0 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -15,6 +15,7 @@ extern "C" { #include "module/gimbal.h" #include "module/vision_bridge.h" #include "module/cmd/cmd.h" +#include "device/referee_proto_types.h" /** * @brief 机器人参数配置结构体 * @note 在此添加您的配置参数 @@ -26,6 +27,7 @@ typedef struct { Gimbal_Params_t gimbal_param; AI_Param_t ai_param; CMD_Config_t cmd_param; + Referee_Screen_t ref_screen; /* USER CODE END Config_RobotParam */ } Config_RobotParam_t; diff --git a/User/task/cmd.c b/User/task/cmd.c index 8ac4ac7..58f338f 100644 --- a/User/task/cmd.c +++ b/User/task/cmd.c @@ -7,12 +7,10 @@ #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "device/dr16.h" -#include "device/AT9S_Pro.h" #include "module/config.h" #include "module/balance_chassis.h" #include "module/gimbal.h" #include "module/shoot.h" -#include "module/track.h" #include "module/cmd/cmd.h" #include "bsp/fdcan.h" #include "module/vision_bridge.h" @@ -22,14 +20,15 @@ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ -/* Private variables -------------------------------------------------------- */ +/* Private variables ----------- +--------------------------------------------- */ /* USER STRUCT BEGIN */ #if CMD_RCTypeTable_Index == 0 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; @@ -37,7 +36,7 @@ CMD_RawInput_REF_t cmd_ref; static CMD_t cmd; -AI_t ai; +// AI_t ai; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -54,8 +53,8 @@ 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 */ + // AI_Init(&ai, &Config_GetRobotParam()->ai_param); + /* 注册CAN接收ID */ /* USER CODE INIT END */ while (1) { @@ -69,7 +68,7 @@ void Task_cmd(void *argument) { // /* 从CAN2接收AI命令 */ // AI_ParseCmdFromCan( &ai,&cmd_ai); - #error "弄好自瞄之后统一改" + // #error "弄好自瞄之后统一改" #if CMD_ENABLE_SRC_REF /* 从裁判系统读取最新数据(非阻塞) */ @@ -96,10 +95,7 @@ void Task_cmd(void *argument) { #if CMD_ENABLE_MODULE_SHOOT Shoot_CMD_t *cmd_for_shoot = CMD_GetShootCmd(&cmd); - #if CMD_ENABLE_MODULE_GIMBAL - /* 发射时添加pitch前馈,抵消后坐力 */ - cmd_for_gimbal->feedforward_pit = cmd_for_shoot->firecmd ? -1.0f : 0.0f; - #endif + osMessageQueueReset(task_runtime.msgq.shoot.cmd); osMessageQueuePut(task_runtime.msgq.shoot.cmd, cmd_for_shoot, 0, 0); #endif diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 5e9b2df..1d14ecd 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -30,6 +30,7 @@ Chassis_CMD_t chassis_cmd = { .height = 0.0f, }; Chassis_IMU_t chassis_imu; +Referee_ForChassis_t chassis_ref; /* 裁判系统底盘数据 */ /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -95,10 +96,19 @@ void Task_ctrl_chassis(void *argument) { chassis.feedback.imu = chassis_imu; } osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.yaw, NULL, 0); - + osMessageQueueGet(task_runtime.msgq.chassis.ref, &chassis_ref, NULL, 0); + Chassis_UpdateFeedback(&chassis); Chassis_Control(&chassis, &chassis_cmd); + + // /* 功率限制:裁判系统在线时使用下发上限,否则使用保守默认值 */ + // float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING) + // ? chassis_ref.chassis_power_limit + // : 500.0f; + // Chassis_Power_Control(&chassis, power_limit); + + osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列,防止阻塞 osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0); /* USER CODE END */ diff --git a/User/task/init.c b/User/task/init.c index e6d7c22..a58c4e2 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -47,8 +47,9 @@ void Task_Init(void *argument) { task_runtime.thread.cli = osThreadNew(Task_cli, NULL, &attr_cli); // task_runtime.thread.debug = osThreadNew(Task_debug, NULL, &attr_debug); task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd); + task_runtime.thread.referee = osThreadNew(Task_referee, NULL, &attr_referee); - // 创建消息队列 + // 创建消息队列 /* USER MESSAGE BEGIN */ task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL); diff --git a/User/task/ref_main.c b/User/task/ref_main.c new file mode 100644 index 0000000..d38fcda --- /dev/null +++ b/User/task/ref_main.c @@ -0,0 +1,99 @@ +/* + referee Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "device/referee.h" + #include "module/config.h" +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +Referee_t ref; +Referee_UI_t ui; +Referee_UI_CMD_t ref_cmd; +Referee_ForCap_t for_cap; +Referee_ForAI_t for_ai; +Referee_ForChassis_t for_chassis; +Referee_ForShoot_t for_shoot; +uint8_t send_data[6]={1,2,3,4}; +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* USER PRIVATE CODE BEGIN */ + +/* USER PRIVATE CODE END */ +/* Exported functions ------------------------------------------------------- */ +void Task_referee(void *argument) { + (void)argument; /* 未使用argument,消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / REFEREE_FREQ; + + osDelay(REFEREE_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + uint32_t last_online_tick = 0; + /* 初始化裁判系统 */ + + Referee_Init(&ref, &ui, &Config_GetRobotParam()->ref_screen); + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + Referee_StartReceiving(&ref); + + if (osThreadFlagsWait(SIGNAL_REFEREE_RAW_REDY, osFlagsWaitAll, 10) != + SIGNAL_REFEREE_RAW_REDY) { + if (osKernelGetTickCount() - last_online_tick > 2500) + Referee_HandleOffline(&ref); + } else { + Referee_Parse(&ref); + last_online_tick = osKernelGetTickCount(); + } + Referee_PackCap(&for_cap, &ref); + Referee_PackAI(&for_ai, &ref); + Referee_PackShoot(&for_shoot, &ref); + Referee_PackChassis(&for_chassis, &ref); + { + /* 裁判系统数据读取 */ + osMessageQueueReset(task_runtime.msgq.referee.tocmd.cap); + osMessageQueuePut(task_runtime.msgq.referee.tocmd.cap, &for_cap, 0, 0); + osMessageQueueReset(task_runtime.msgq.referee.tocmd.ai); + osMessageQueuePut(task_runtime.msgq.referee.tocmd.ai, &for_ai, 0, 0); + osMessageQueueReset(task_runtime.msgq.referee.tocmd.chassis); + osMessageQueuePut(task_runtime.msgq.referee.tocmd.chassis, &for_chassis, 0, 0); + osMessageQueueReset(task_runtime.msgq.referee.tocmd.shoot); + osMessageQueuePut(task_runtime.msgq.referee.tocmd.shoot, &for_shoot, 0, 0); + /* UI数据获取 */ + osMessageQueueGet(task_runtime.msgq.referee.ui.tocap, &(ui.cap_ui), NULL, 0); + osMessageQueueGet(task_runtime.msgq.referee.ui.tochassis, &(ui.chassis_ui), NULL,0); + osMessageQueueGet(task_runtime.msgq.referee.ui.togimbal, &(ui.gimbal_ui), NULL, 0); + osMessageQueueGet(task_runtime.msgq.referee.ui.toshoot, &(ui.shoot_ui), NULL, 0); + osMessageQueueGet(task_runtime.msgq.referee.ui.tocmd, &(ui.cmd_pc), NULL, 0); + Referee_UIRefresh(&ui); + + while (osMessageQueueGet(task_runtime.msgq.referee.ui.frcmd, &ref_cmd, NULL, + 0) == osOK) { + ref_cmd=UI_AUTO_AIM_START; + Referee_PraseCmd(&ui, ref_cmd); + // Referee_StartSend(send_data, sizeof(send_data)); + } + + Referee_PackUI(&ui, &ref); + } + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/user_task.c b/User/task/user_task.c index 8d40472..7669e78 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -68,4 +68,9 @@ const osThreadAttr_t attr_cmd = { .name = "cmd", .priority = osPriorityNormal, .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_referee = { + .name = "referee", + .priority = osPriorityNormal, + .stack_size = 512 * 4, }; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h index 737c09f..082c1e9 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -23,6 +23,7 @@ extern "C" { #define VOFA_FREQ (500.0) #define DEBUG_FREQ (10.0) #define CMD_FREQ (500.0) +#define REFEREE_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) @@ -38,6 +39,7 @@ extern "C" { #define CLI_INIT_DELAY (0) #define DEBUG_INIT_DELAY (0) #define CMD_INIT_DELAY (0) +#define REFEREE_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -59,6 +61,8 @@ typedef struct { osThreadId_t cli; osThreadId_t debug; osThreadId_t cmd; + osThreadId_t referee; + } thread; /* USER MESSAGE BEGIN */ @@ -139,6 +143,8 @@ typedef struct { UBaseType_t cli; UBaseType_t debug; UBaseType_t cmd; + UBaseType_t referee; + } stack_water_mark; /* 各任务运行频率 */ @@ -153,6 +159,7 @@ typedef struct { float vofa; float debug; float cmd; + float referee; } freq; /* 任务最近运行时间 */ @@ -167,6 +174,7 @@ typedef struct { float vofa; float debug; float cmd; + float referee; } last_up_time; } Task_Runtime_t; @@ -188,6 +196,7 @@ extern const osThreadAttr_t attr_vofa; extern const osThreadAttr_t attr_cli; extern const osThreadAttr_t attr_debug; extern const osThreadAttr_t attr_cmd; +extern const osThreadAttr_t attr_referee; /* 任务函数声明 */ void Task_Init(void *argument); @@ -203,6 +212,7 @@ void Task_vofa(void *argument); void Task_cli(void *argument); void Task_debug(void *argument); void Task_cmd(void *argument); +void Task_referee(void *argument); #ifdef __cplusplus }