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

保存

Browse Source
This commit is contained in:
xxxxm 2026-03-14 00:22:13 +08:00
parent 6dc460e80c
commit 96da4ad6dc
14 changed files with 163 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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

16
User/component/user_math.c
View File

@ -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 */

1
User/component/user_math.h
View File

@ -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 */

2
User/module/balance_chassis.c
View File

@ -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应该是跟随云台的那个角,我没找着在哪"
}

2
User/module/cmd/cmd_feature.h
View File

@ -24,7 +24,7 @@
* 1 (比赛模式): cmd 将 referee 数据转发到各模块的 .ref 队列
* 0 (调试模式): cmd 不转发，裁判系统可断开，不影响其他功能
*/
#define CMD_ENABLE_SRC_REF 0
#define CMD_ENABLE_SRC_REF 1
/* ========================================================================== */
/* 输出模块使能开关 */

1
User/module/cmd/cmd_types.h
View File

@ -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;

6
User/module/config.c
View File

@ -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 */
};

2
User/module/config.h
View File

@ -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;

18
User/task/cmd.c
View File

@ -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,7 +53,7 @@ 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);
// AI_Init(&ai, &Config_GetRobotParam()->ai_param);
/* 注册CAN接收ID */
/* USER CODE INIT END */
@ -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

10
User/task/ctrl_chassis.c
View File

@ -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 */

1
User/task/init.c
View File

@ -47,6 +47,7 @@ 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 */

99
User/task/ref_main.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

5
User/task/user_task.c
View File

@ -69,3 +69,8 @@ const osThreadAttr_t attr_cmd = {
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_referee = {
.name = "referee",
.priority = osPriorityNormal,
.stack_size = 512 * 4,
};

10
User/task/user_task.h
View File

@ -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
}