robofish/RMUC2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: robofish:infantry
Branches Tags
robofish:main robofish:balance robofish:xr版本有超电hero robofish:infantry robofish:sentry_right robofish:无超电hero robofish:sentry_chassis robofish:hero robofish:sentry_left
..
compare: robofish:无超电hero
Branches Tags
robofish:balance robofish:main robofish:xr版本有超电hero robofish:infantry robofish:sentry_right robofish:无超电hero robofish:sentry_chassis robofish:hero robofish:sentry_left

5 Commits
infantry ... 无超电hero

Author SHA1 Message Date
ws
46ff0ff445 射速5350 2025-03-18 17:29:12 +08:00
RB
7aef65bf92 添加ref 2025-03-18 02:52:06 +08:00
RB
f05b2c120b 英雄啊 2025-03-17 12:51:00 +08:00
RB
143decc739 调pid 2025-03-12 19:24:00 +08:00
RB
8a3edebe9d hero 2025-03-09 18:04:03 +08:00
18 changed files with 10139 additions and 9964 deletions
Expand all files Collapse all files

31
MDK-ARM/DevC.uvoptx
View File

@@ -324,6 +324,11 @@
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U-O206 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@@ -347,7 +352,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name> <Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@@ -360,7 +365,7 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>chassis</ItemText> <ItemText>param_default</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
@@ -370,47 +375,47 @@
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>gimbal_out</ItemText> <ItemText>shoot</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>for_chassis</ItemText> <ItemText>param_hero</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>task_runtime</ItemText> <ItemText>ref</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>ref</ItemText> <ItemText>for_chassis</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>for_chassis</ItemText> <ItemText>cap_out,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>7</count> <count>7</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>for_cap</ItemText> <ItemText>pm01_od,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>8</count> <count>8</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>ref</ItemText> <ItemText>cap,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>9</count> <count>9</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>param_default</ItemText> <ItemText>gimbal</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>10</count> <count>10</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>shoot</ItemText> <ItemText>cmd</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<MemoryWindow4> <MemoryWindow4>
@@ -1283,7 +1288,7 @@
<Group> <Group>
<GroupName>User/bsp</GroupName> <GroupName>User/bsp</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@@ -1507,7 +1512,7 @@
<Group> <Group>
<GroupName>User/component</GroupName> <GroupName>User/component</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

8
MDK-ARM/DevC.uvprojx
View File

@@ -16,8 +16,8 @@
<TargetCommonOption> <TargetCommonOption>
<Device>STM32F407IGHx</Device> <Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor> <Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.2.15.0</PackID> <PackID>Keil.STM32F4xx_DFP.2.17.1</PackID>
<PackURL>http://www.keil.com/pack/</PackURL> <PackURL>https://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu> <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu>
<FlashUtilSpec></FlashUtilSpec> <FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile> <StartupFile></StartupFile>
@@ -1732,8 +1732,8 @@
<TargetCommonOption> <TargetCommonOption>
<Device>STM32F407IGHx</Device> <Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor> <Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.2.15.0</PackID> <PackID>Keil.STM32F4xx_DFP.2.17.1</PackID>
<PackURL>http://www.keil.com/pack/</PackURL> <PackURL>https://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu> <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu>
<FlashUtilSpec></FlashUtilSpec> <FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile> <StartupFile></StartupFile>

BIN
MDK-ARM/DevC/DevC.axf
View File

Binary file not shown.

18405
MDK-ARM/DevC/DevC.hex
View File

File diff suppressed because it is too large Load Diff

114
Middlewares/Third_Party/Protocol/protocol.h vendored
View File

@@ -7,15 +7,13 @@
#include <stdint.h> #include <stdint.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" extern "C" {
{
#endif #endif
#define AI_NOTICE_AUTOAIM (1 << 0) #define AI_NOTICE_AUTOAIM (1 << 0)
#define AI_NOTICE_HITBUFF (1 << 1) #define AI_NOTICE_HITBUFF (1 << 1)
#define AI_NOTICE_AUTOMATIC (1 << 2) #define AI_NOTICE_AUTOMATIC (1 << 2)
#define AI_NOTICE_FIRE (1 << 3) #define AI_NOTICE_FIRE (1 << 3)
#define AI_NOTICE_SEARCH_ARMOR (1 << 4)
#define AI_ID_MCU (0xC4) #define AI_ID_MCU (0xC4)
#define AI_ID_REF (0xA8) #define AI_ID_REF (0xA8)
@@ -23,70 +21,66 @@ extern "C"
#define AI_TEAM_RED (0x01) #define AI_TEAM_RED (0x01)
#define AI_TEAM_BLUE (0x02) #define AI_TEAM_BLUE (0x02)
typedef uint8_t Protocol_ID_t; typedef uint8_t Protocol_ID_t;
/* 电控 -> 视觉 MCU数据结构体*/ /* 电控 -> 视觉 MCU数据结构体*/
typedef struct __attribute__((packed)) typedef struct __attribute__((packed)) {
{ struct __attribute__((packed)) {
struct __attribute__((packed)) float q0;
{ float q1;
float q0; float q2;
float q1; float q3;
float q2; } quat; /* 四元数 */
float q3;
} quat; /* 四元数 */
struct __attribute__((packed))
{
float yaw;
float pit;
float rol;
} gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */
} Protocol_UpDataMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/ uint8_t notice; /* 控制命令 */
typedef struct __attribute__((packed))
{
uint16_t team; /* 本身队伍 */
uint16_t time; /* 比赛开始时间 */
} Protocol_UpDataReferee_t;
/* 视觉 -> 电控 数据结构体*/ float ball_speed; /* 子弹初速度 */
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
struct __attribute__((packed)) struct __attribute__((packed)) {
{ float left;
float vx; /* x轴移动速度 */ float right;
float vy; /* y轴移动速度 */ } distance; /* 左右距离(哨兵) */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
uint8_t notice; /* 控制命令 */
} Protocol_DownData_t;
typedef struct __attribute__((packed)) float chassis_speed; /* 底盘速度(哨兵) */
{ } Protocol_UpDataMCU_t;
Protocol_UpDataMCU_t data;
uint16_t crc16;
} Protocol_UpPackageMCU_t;
typedef struct __attribute__((packed)) /* 电控 -> 视觉 裁判系统数据结构体*/
{ typedef struct __attribute__((packed)) {
Protocol_UpDataReferee_t data; uint16_t team; /* 本身队伍 */
uint16_t crc16; uint16_t time; /* 比赛开始时间 */
} Protocol_UpPackageReferee_t; } Protocol_UpDataReferee_t;
typedef struct __attribute__((packed)) /* 视觉 -> 电控 数据结构体*/
{ typedef struct __attribute__((packed)) {
Protocol_DownData_t data; struct __attribute__((packed)) {
uint16_t crc16; float yaw; /* 偏航角（Yaw angle） */
} Protocol_DownPackage_t; float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */
struct __attribute__((packed)) {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
} Protocol_DownData_t;
typedef struct __attribute__((packed)) {
Protocol_UpDataMCU_t data;
uint16_t crc16;
} Protocol_UpPackageMCU_t;
typedef struct __attribute__((packed)) {
Protocol_UpDataReferee_t data;
uint16_t crc16;
} Protocol_UpPackageReferee_t;
typedef struct __attribute__((packed)) {
Protocol_DownData_t data;
uint16_t crc16;
} Protocol_DownPackage_t;
#ifdef __cplusplus #ifdef __cplusplus
} }

6
User/component/cmd.c
View File

@@ -86,7 +86,7 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->gimbal.delta_eulr.yaw = cmd->gimbal.delta_eulr.yaw =
(float)rc->mouse.x * dt_sec * cmd->param->sens_mouse; (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
cmd->gimbal.delta_eulr.pit = cmd->gimbal.delta_eulr.pit =
(float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse; (float)(rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f; cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
cmd->shoot.reverse_trig = false; cmd->shoot.reverse_trig = false;
@@ -170,7 +170,7 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->shoot.reverse_trig = true; cmd->shoot.reverse_trig = true;
} }
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) { if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35; cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
} }
/* 保存当前按下的键位状态 */ /* 保存当前按下的键位状态 */
cmd->key_last = rc->key; cmd->key_last = rc->key;
@@ -250,7 +250,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->chassis.ctrl_vec.vx = rc->ch_l_x; cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y; cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc; cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc; cmd->gimbal.delta_eulr.pit = -rc->ch_r_y * dt_sec * cmd->param->sens_rc;
} }
/** /**

19
User/component/user_math.c
View File

@@ -27,10 +27,6 @@ inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in); return (in < -limit) ? -limit : ((in > limit) ? limit : in);
} }
float fAbs(float in){
return (in > 0) ? in : -in;
}
inline void Clip(float *origin, float min, float max) { inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max; if (*origin > max) *origin = max;
if (*origin < min) *origin = min; if (*origin < min) *origin = min;
@@ -111,22 +107,9 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
} else { } else {
if (bullet_speed == 10.0f) return 4450.f; if (bullet_speed == 10.0f) return 4450.f;
if (bullet_speed == 16.0f) return 5800.f; if (bullet_speed == 16.0f) return 5800.f;
if (bullet_speed == 15.0f) return 5350.f;
} }
/* 不为裁判系统设定值时,计算转速 */ /* 不为裁判系统设定值时,计算转速 */
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
} }
/**
* @brief 断言失败处理
*
* @param file 文件名
* @param line 行号
*/
void VerifyFailed(const char *file, uint32_t line) {
UNUSED(file);
UNUSED(line);
while (1) {
__NOP();
}
}

52
User/component/user_math.h
View File

@@ -51,8 +51,6 @@ float InvSqrt(float x);
float AbsClip(float in, float limit); float AbsClip(float in, float limit);
float fAbs(float in);
void Clip(float *origin, float min, float max); void Clip(float *origin, float min, float max);
float Sign(float in); float Sign(float in);
@@ -106,53 +104,3 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define ASSERT(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 未定DEBUG，表达式不会运行，断言被忽略
*
*/
#define ASSERT(expr) ((void)(0))
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define VERIFY(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 表达式会运行，忽略表达式结果
*
*/
#define VERIFY(expr) ((void)(expr))
#endif
/**
* @brief 断言失败处理
*
* @param file 文件名
* @param line 行号
*/
void VerifyFailed(const char *file, uint32_t line);

57
User/device/ai.c
View File

@@ -1,5 +1,5 @@
/* /*
AI AI
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@@ -12,45 +12,31 @@ AI
#include "component\crc16.h" #include "component\crc16.h"
#include "component\crc8.h" #include "component\crc8.h"
#include "component\user_math.h" #include "component\user_math.h"
#include "component\filter.h"
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0; static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[AI_LEN_RX_BUFF]; static osThreadId_t thread_alert;
static bool inited = false; static bool inited = false;
static osThreadId_t thread_alert;
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) { static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY); osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
} }
static void Ai_IdleLineCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) { int8_t AI_Init(AI_t *ai) {
UNUSED(ai); if (ai == NULL) return DEVICE_ERR_NULL;
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED; if (inited) return DEVICE_ERR_INITED;
VERIFY((thread_alert = osThreadGetId()) != NULL); if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback); Ai_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
Ai_IdleLineCallback);
inited = true; inited = true;
return 0; return 0;
} }
@@ -62,9 +48,9 @@ int8_t AI_Restart(void) {
} }
int8_t AI_StartReceiving(AI_t *ai) { int8_t AI_StartReceiving(AI_t *ai) {
UNUSED(ai); if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf, (uint8_t *)&(ai->form_host),
AI_LEN_RX_BUFF) == HAL_OK) sizeof(ai->form_host)) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
@@ -74,12 +60,17 @@ bool AI_WaitDmaCplt(void) {
SIGNAL_AI_RAW_REDY); SIGNAL_AI_RAW_REDY);
} }
int8_t AI_ParseHost(AI_t *ai) { int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host))) (void)cmd_host;
if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
goto error; goto error;
ai->ai_online = true; cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host)); cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
memset(rxbuf, 0, AI_LEN_RX_BUFF); cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
return DEVICE_OK; return DEVICE_OK;
error: error:
@@ -87,20 +78,11 @@ error:
return DEVICE_ERR; return DEVICE_ERR;
} }
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
}
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) { int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (ai == NULL) return DEVICE_ERR_NULL; if (ai == NULL) return DEVICE_ERR_NULL;
if (cmd_host == NULL) return DEVICE_ERR_NULL; if (cmd_host == NULL) return DEVICE_ERR_NULL;
ai->ai_online = false;
memset(&(ai->from_host), 0, sizeof(ai->from_host)); memset(&(ai->form_host), 0, sizeof(ai->form_host));
memset(cmd_host, 0, sizeof(*cmd_host)); memset(cmd_host, 0, sizeof(*cmd_host));
return 0; return 0;
} }
@@ -149,4 +131,3 @@ int8_t AI_StartSend(AI_t *ai, bool ref_update) {
return DEVICE_ERR; return DEVICE_ERR;
} }
} }

10
User/device/ai.h
View File

@@ -16,7 +16,6 @@ extern "C" {
#include "component\ahrs.h" #include "component\ahrs.h"
#include "component\cmd.h" #include "component\cmd.h"
#include "component\user_math.h" #include "component\user_math.h"
#include "component\filter.h"
#include "device\device.h" #include "device\device.h"
#include "device\referee.h" #include "device\referee.h"
#include "protocol.h" #include "protocol.h"
@@ -38,7 +37,7 @@ typedef struct __packed {
typedef struct __packed { typedef struct __packed {
osThreadId_t thread_alert; osThreadId_t thread_alert;
Protocol_DownPackage_t from_host; Protocol_DownPackage_t form_host;
struct { struct {
AI_UpPackageReferee_t ref; AI_UpPackageReferee_t ref;
@@ -46,7 +45,6 @@ typedef struct __packed {
} to_host; } to_host;
CMD_AI_Status_t status; CMD_AI_Status_t status;
bool ai_online;
} AI_t; } AI_t;
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
@@ -55,15 +53,11 @@ int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai); int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void); bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai); int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host); int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat); int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref); int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
int8_t AI_StartSend(AI_t *ai, bool option); int8_t AI_StartSend(AI_t *ai, bool option);
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

292
User/device/can.c
View File

@@ -68,7 +68,8 @@ static bool inited = false;
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
static void CAN_Motor_Decode(CAN_MotorFeedback_t *feedback, static void CAN_Motor_Decode(CAN_MotorFeedback_t *feedback,
const uint8_t *raw) { const uint8_t *raw)
{
uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]); uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]); int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);
@@ -79,36 +80,44 @@ static void CAN_Motor_Decode(CAN_MotorFeedback_t *feedback,
feedback->temp = raw[6]; feedback->temp = raw[6];
} }
void CAN_Cap_Decode(CAN_CapFeedback_t *feedback, const uint8_t *raw) { void CAN_Cap_Decode(CAN_CapFeedback_t *feedback, const uint8_t *raw)
{
feedback->input_volt = (float)((raw[1] << 8) | raw[0]) / (float)CAN_CAP_RES; feedback->input_volt = (float)((raw[1] << 8) | raw[0]) / (float)CAN_CAP_RES;
feedback->cap_volt = (float)((raw[3] << 8) | raw[2]) / (float)CAN_CAP_RES; feedback->cap_volt = (float)((raw[3] << 8) | raw[2]) / (float)CAN_CAP_RES;
feedback->input_curr = (float)((raw[5] << 8) | raw[4]) / (float)CAN_CAP_RES; feedback->input_curr = (float)((raw[5] << 8) | raw[4]) / (float)CAN_CAP_RES;
feedback->target_power = (float)((raw[7] << 8) | raw[6]) / (float)CAN_CAP_RES; feedback->target_power = (float)((raw[7] << 8) | raw[6]) / (float)CAN_CAP_RES;
} }
void CAN_Tof_Decode(CAN_Tof_t *tof, const uint8_t *raw) { void CAN_Tof_Decode(CAN_Tof_t *tof, const uint8_t *raw)
{
tof->dist = (float)((raw[2] << 16) | (raw[1] << 8) | raw[0]) / 1000.0f; tof->dist = (float)((raw[2] << 16) | (raw[1] << 8) | raw[0]) / 1000.0f;
tof->status = raw[3]; tof->status = raw[3];
tof->signal_strength = (raw[5] << 8) | raw[4]; tof->signal_strength = (raw[5] << 8) | raw[4];
} }
static void CAN_CAN1RxFifoMsgPendingCallback(void) { static void CAN_CAN1RxFifoMsgPendingCallback(void)
{
HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_MOTOR_RX_FIFO, HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_MOTOR_RX_FIFO,
&raw_rx1.rx_header, raw_rx1.rx_data); &raw_rx1.rx_header, raw_rx1.rx_data);
osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0); osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0);
} }
static void CAN_CAN2RxFifoMsgPendingCallback(void) { static void CAN_CAN2RxFifoMsgPendingCallback(void)
{
HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_CAP_RX_FIFO, HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_CAP_RX_FIFO,
&raw_rx2.rx_header, raw_rx2.rx_data); &raw_rx2.rx_header, raw_rx2.rx_data);
osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0); osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
} }
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) { int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param)
if (can == NULL) return DEVICE_ERR_NULL; {
if (inited) return DEVICE_ERR_INITED; if (can == NULL)
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; return DEVICE_ERR_NULL;
if (inited)
return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL)
return DEVICE_ERR_NULL;
can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL); can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL);
@@ -150,158 +159,178 @@ int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
} }
int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output, int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
CAN_t *can) { CAN_t *can)
if (output == NULL) return DEVICE_ERR_NULL; {
if (output == NULL)
return DEVICE_ERR_NULL;
int16_t motor1, motor2, motor3, motor4; int16_t motor1, motor2, motor3, motor4;
int16_t yaw_motor, pit_motor; int16_t yaw_motor, pit_motor;
int16_t fric1_motor, fric2_motor, trig_motor; int16_t fric1_motor, fric2_motor, fric3_motor, trig_motor;
switch (group) { switch (group)
case CAN_MOTOR_GROUT_CHASSIS: {
motor1 = case CAN_MOTOR_GROUT_CHASSIS:
(int16_t)(output->chassis.named.m1 * (float)CAN_M3508_MAX_ABS_LSB); motor1 =
motor2 = (int16_t)(output->chassis.named.m1 * (float)CAN_M3508_MAX_ABS_LSB);
(int16_t)(output->chassis.named.m2 * (float)CAN_M3508_MAX_ABS_LSB); motor2 =
motor3 = (int16_t)(output->chassis.named.m2 * (float)CAN_M3508_MAX_ABS_LSB);
(int16_t)(output->chassis.named.m3 * (float)CAN_M3508_MAX_ABS_LSB); motor3 =
motor4 = (int16_t)(output->chassis.named.m3 * (float)CAN_M3508_MAX_ABS_LSB);
(int16_t)(output->chassis.named.m4 * (float)CAN_M3508_MAX_ABS_LSB); motor4 =
(int16_t)(output->chassis.named.m4 * (float)CAN_M3508_MAX_ABS_LSB);
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_BASE; raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_BASE;
raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF); raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF); raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF); raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF); raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF); raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF); raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF); raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF); raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis), HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis),
&raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.chassis)); &(can->mailbox.chassis));
break; break;
case CAN_MOTOR_GROUT_GIMBAL1: case CAN_MOTOR_GROUT_GIMBAL1:
case CAN_MOTOR_GROUT_GIMBAL2: case CAN_MOTOR_GROUT_GIMBAL2:
yaw_motor = yaw_motor =
(int16_t)(output->gimbal.named.yaw * (float)CAN_GM6020_MAX_ABS_LSB); (int16_t)(output->gimbal.named.yaw * (float)CAN_GM6020_MAX_ABS_LSB);
pit_motor = pit_motor =
(int16_t)(output->gimbal.named.pit * (float)CAN_GM6020_MAX_ABS_LSB); (int16_t)(output->gimbal.named.pit * (float)CAN_GM6020_MAX_ABS_LSB);
raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_EXTAND; raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_EXTAND;
raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((yaw_motor >> 8) & 0xFF); raw_tx.tx_data[0] = (uint8_t)((yaw_motor >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(yaw_motor & 0xFF); raw_tx.tx_data[1] = (uint8_t)(yaw_motor & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((pit_motor >> 8) & 0xFF); raw_tx.tx_data[2] = (uint8_t)((pit_motor >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(pit_motor & 0xFF); raw_tx.tx_data[3] = (uint8_t)(pit_motor & 0xFF);
raw_tx.tx_data[4] = 0; raw_tx.tx_data[4] = 0;
raw_tx.tx_data[5] = 0; raw_tx.tx_data[5] = 0;
raw_tx.tx_data[6] = 0; raw_tx.tx_data[6] = 0;
raw_tx.tx_data[7] = 0; raw_tx.tx_data[7] = 0;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal), HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal.yaw),
&raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.gimbal)); &(can->mailbox.gimbal));
// HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot), HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal.pitch),
// &raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
// &(can->mailbox.gimbal)); &(can->mailbox.gimbal));
break; // HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
// &raw_tx.tx_header, raw_tx.tx_data,
// &(can->mailbox.gimbal));
break;
case CAN_MOTOR_GROUT_SHOOT1: case CAN_MOTOR_GROUT_SHOOT1:
case CAN_MOTOR_GROUT_SHOOT2: case CAN_MOTOR_GROUT_SHOOT2:
fric1_motor = fric1_motor =
(int16_t)(output->shoot.named.fric1 * (float)CAN_M3508_MAX_ABS_LSB); (int16_t)(output->shoot.named.fric1 * (float)CAN_M3508_MAX_ABS_LSB);
fric2_motor = fric2_motor =
(int16_t)(output->shoot.named.fric2 * (float)CAN_M3508_MAX_ABS_LSB); (int16_t)(output->shoot.named.fric2 * (float)CAN_M3508_MAX_ABS_LSB);
trig_motor = fric3_motor =
(int16_t)(output->shoot.named.trig * (float)CAN_M2006_MAX_ABS_LSB); (int16_t)(output->shoot.named.fric3 * (float)CAN_M3508_MAX_ABS_LSB);
trig_motor =
(int16_t)(output->shoot.named.trig * (float)CAN_M2006_MAX_ABS_LSB);
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_EXTAND; raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_EXTAND;
raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((fric1_motor >> 8) & 0xFF); raw_tx.tx_data[0] = (uint8_t)((fric1_motor >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(fric1_motor & 0xFF); raw_tx.tx_data[1] = (uint8_t)(fric1_motor & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((fric2_motor >> 8) & 0xFF); raw_tx.tx_data[2] = (uint8_t)((fric2_motor >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(fric2_motor & 0xFF); raw_tx.tx_data[3] = (uint8_t)(fric2_motor & 0xFF);
raw_tx.tx_data[4] = (uint8_t)((trig_motor >> 8) & 0xFF); raw_tx.tx_data[4] = (uint8_t)((fric3_motor >> 8) & 0xFF);
raw_tx.tx_data[5] = (uint8_t)(trig_motor & 0xFF); raw_tx.tx_data[5] = (uint8_t)(fric3_motor & 0xFF);
raw_tx.tx_data[6] = 0; raw_tx.tx_data[6] = (uint8_t)((trig_motor >> 8) & 0xFF);
raw_tx.tx_data[7] = 0; raw_tx.tx_data[7] = (uint8_t)(trig_motor & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot), HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
&raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.shoot)); &(can->mailbox.shoot));
break; HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->trig),
&raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.shoot));
break;
default: default:
break; break;
} }
return DEVICE_OK; return DEVICE_OK;
} }
int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx)
if (can == NULL) return DEVICE_ERR_NULL; {
if (can_rx == NULL) return DEVICE_ERR_NULL; if (can == NULL)
return DEVICE_ERR_NULL;
if (can_rx == NULL)
return DEVICE_ERR_NULL;
uint32_t index; uint32_t index;
switch (can_rx->rx_header.StdId) { switch (can_rx->rx_header.StdId)
case CAN_M3508_M1_ID: {
case CAN_M3508_M2_ID: case CAN_M3508_M1_ID:
case CAN_M3508_M3_ID: case CAN_M3508_M2_ID:
case CAN_M3508_M4_ID: case CAN_M3508_M3_ID:
index = can_rx->rx_header.StdId - CAN_M3508_M1_ID; case CAN_M3508_M4_ID:
CAN_Motor_Decode(&(can->motor.chassis.as_array[index]), can_rx->rx_data); index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
can->recive_flag |= 1 << index; CAN_Motor_Decode(&(can->motor.chassis.as_array[index]), can_rx->rx_data);
break; can->recive_flag |= 1 << index;
break;
case CAN_M3508_FRIC1_ID: case CAN_M3508_FRIC1_ID:
case CAN_M3508_FRIC2_ID: case CAN_M3508_FRIC2_ID:
case CAN_M2006_TRIG_ID: case CAN_M3508_FRIC3_ID:
index = can_rx->rx_header.StdId - CAN_M3508_FRIC1_ID; case CAN_M2006_TRIG_ID:
can->recive_flag |= 1 << (index + 6); index = can_rx->rx_header.StdId - CAN_M3508_FRIC1_ID;
CAN_Motor_Decode(&(can->motor.shoot.as_array[index]), can_rx->rx_data); can->recive_flag |= 1 << (index + 6);
break; CAN_Motor_Decode(&(can->motor.shoot.as_array[index]), can_rx->rx_data);
break;
case CAN_GM6020_YAW_ID: case CAN_GM6020_YAW_ID:
case CAN_GM6020_PIT_ID: case CAN_GM6020_PIT_ID:
index = can_rx->rx_header.StdId - CAN_GM6020_YAW_ID; index = can_rx->rx_header.StdId - CAN_GM6020_YAW_ID;
can->recive_flag |= 1 << (index + 4); can->recive_flag |= 1 << (index + 4);
CAN_Motor_Decode(&(can->motor.gimbal.as_array[index]), can_rx->rx_data); CAN_Motor_Decode(&(can->motor.gimbal.as_array[index]), can_rx->rx_data);
break; break;
case CAN_CAP_FB_ID_BASE: case CAN_CAP_FB_ID_BASE:
can->recive_flag |= 1 << 9; can->recive_flag |= 1 << 9;
CAN_Cap_Decode(&(can->cap.cap_feedback), can_rx->rx_data); CAN_Cap_Decode(&(can->cap.cap_feedback), can_rx->rx_data);
break; break;
case CAN_TOF_ID_BASE: case CAN_TOF_ID_BASE:
can->recive_flag |= 1 << 10; can->recive_flag |= 1 << 10;
CAN_Tof_Decode(&(can->tof), can_rx->rx_data); CAN_Tof_Decode(&(can->tof), can_rx->rx_data);
break; break;
default: default:
break; break;
} }
return DEVICE_OK; return DEVICE_OK;
} }
bool CAN_CheckFlag(CAN_t *can, uint32_t flag) { bool CAN_CheckFlag(CAN_t *can, uint32_t flag)
if (can == NULL) return false; {
if (can == NULL)
return false;
return (can->recive_flag & flag) == flag; return (can->recive_flag & flag) == flag;
} }
int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag) { int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag)
if (can == NULL) return DEVICE_ERR_NULL; {
if (can == NULL)
return DEVICE_ERR_NULL;
can->recive_flag &= ~flag; can->recive_flag &= ~flag;
return DEVICE_OK; return DEVICE_OK;
} }
int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can) { int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can)
{
float power_limit = output->power_limit; float power_limit = output->power_limit;
uint16_t cap = (uint16_t)(power_limit * CAN_CAP_RES); uint16_t cap = (uint16_t)(power_limit * CAN_CAP_RES);
@@ -320,7 +349,8 @@ int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can) {
} }
void CAN_CAP_HandleOffline(CAN_Capacitor_t *cap, CAN_CapOutput_t *cap_out, void CAN_CAP_HandleOffline(CAN_Capacitor_t *cap, CAN_CapOutput_t *cap_out,
float power_chassis) { float power_chassis)
{
cap->cap_status = CAN_CAP_STATUS_OFFLINE; cap->cap_status = CAN_CAP_STATUS_OFFLINE;
cap_out->power_limit = power_chassis; cap_out->power_limit = power_chassis;
} }

23
User/device/can.h
View File

@@ -22,9 +22,10 @@ extern "C" {
#define CAN_MOTOR_GIMBAL_PIT_RECV (1 << 5) #define CAN_MOTOR_GIMBAL_PIT_RECV (1 << 5)
#define CAN_MOTOR_SHOOT_FRIC1_RECV (1 << 6) #define CAN_MOTOR_SHOOT_FRIC1_RECV (1 << 6)
#define CAN_MOTOR_SHOOT_FRIC2_RECV (1 << 7) #define CAN_MOTOR_SHOOT_FRIC2_RECV (1 << 7)
#define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 8) #define CAN_MOTOR_SHOOT_FRIC3_RECV (1 << 8)
#define CAN_MOTOR_CAP_RECV (1 << 9) #define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 9)
#define CAN_TOF_RECV (1 << 10) #define CAN_MOTOR_CAP_RECV (1 << 10)
#define CAN_TOF_RECV (1 << 11)
#define CAN_REC_CHASSIS_FINISHED \ #define CAN_REC_CHASSIS_FINISHED \
(CAN_MOTOR_CHASSIS_1_RECV | CAN_MOTOR_CHASSIS_2_RECV | \ (CAN_MOTOR_CHASSIS_1_RECV | CAN_MOTOR_CHASSIS_2_RECV | \
@@ -33,7 +34,7 @@ extern "C" {
(CAN_MOTOR_GIMBAL_YAW_RECV | CAN_MOTOR_GIMBAL_PIT_RECV) (CAN_MOTOR_GIMBAL_YAW_RECV | CAN_MOTOR_GIMBAL_PIT_RECV)
#define CAN_REC_SHOOT_FINISHED \ #define CAN_REC_SHOOT_FINISHED \
(CAN_MOTOR_SHOOT_FRIC1_RECV | CAN_MOTOR_SHOOT_FRIC2_RECV | \ (CAN_MOTOR_SHOOT_FRIC1_RECV | CAN_MOTOR_SHOOT_FRIC2_RECV | \
CAN_MOTOR_SHOOT_TRIG_RECV) CAN_MOTOR_SHOOT_FRIC3_RECV |CAN_MOTOR_SHOOT_TRIG_RECV)
#define CAN_REC_CAP_FINISHED CAN_MOTOR_CAP_RECV #define CAN_REC_CAP_FINISHED CAN_MOTOR_CAP_RECV
#define CAN_REC_TOF_FINISHED CAN_TOF_RECV #define CAN_REC_TOF_FINISHED CAN_TOF_RECV
@@ -72,16 +73,23 @@ typedef enum {
CAN_M3508_FRIC1_ID = 0x205, /* 5 */ CAN_M3508_FRIC1_ID = 0x205, /* 5 */
CAN_M3508_FRIC2_ID = 0x206, /* 6 */ CAN_M3508_FRIC2_ID = 0x206, /* 6 */
CAN_M2006_TRIG_ID = 0x207, /* 7 */ CAN_M3508_FRIC3_ID = 0x207, /* 7 */
CAN_M2006_TRIG_ID = 0x208, /* 8 */
CAN_GM6020_YAW_ID = 0x209, /* 5 */ CAN_GM6020_YAW_ID = 0x209, /* 5 */
CAN_GM6020_PIT_ID = 0x20A, /* 6 */ CAN_GM6020_PIT_ID = 0x20A, /* 6 */
} CAN_MotorId_t; } CAN_MotorId_t;
typedef struct {
BSP_CAN_t yaw;
BSP_CAN_t pitch;
} Gimbal_CAN_t;
typedef struct { typedef struct {
BSP_CAN_t chassis; BSP_CAN_t chassis;
BSP_CAN_t gimbal; Gimbal_CAN_t gimbal;
BSP_CAN_t shoot; BSP_CAN_t shoot;
BSP_CAN_t trig;
BSP_CAN_t cap; BSP_CAN_t cap;
} CAN_Params_t; } CAN_Params_t;
@@ -110,6 +118,7 @@ enum CAN_MotorGimbal_e {
enum CAN_MotorShoot_e { enum CAN_MotorShoot_e {
CAN_MOTOR_SHOOT_FRIC1 = 0, CAN_MOTOR_SHOOT_FRIC1 = 0,
CAN_MOTOR_SHOOT_FRIC2, CAN_MOTOR_SHOOT_FRIC2,
CAN_MOTOR_SHOOT_FRIC3,
CAN_MOTOR_SHOOT_TRIG, CAN_MOTOR_SHOOT_TRIG,
CAN_MOTORSHOOT_NUM, CAN_MOTORSHOOT_NUM,
}; };
@@ -147,6 +156,7 @@ typedef union {
struct { struct {
float fric1; float fric1;
float fric2; float fric2;
float fric3;
float trig; float trig;
} named; } named;
} CAN_ShootOutput_t; } CAN_ShootOutput_t;
@@ -200,6 +210,7 @@ typedef union {
struct { struct {
CAN_MotorFeedback_t fric1; CAN_MotorFeedback_t fric1;
CAN_MotorFeedback_t fric2; CAN_MotorFeedback_t fric2;
CAN_MotorFeedback_t fric3;
CAN_MotorFeedback_t trig; CAN_MotorFeedback_t trig;
} named; } named;
} CAN_ShootMotor_t; } CAN_ShootMotor_t;

98
User/device/referee.c
View File

@@ -116,23 +116,27 @@ void Referee_HandleOffline(Referee_t *referee) {
} }
int8_t Referee_Parse(Referee_t *ref) { int8_t Referee_Parse(Referee_t *ref) {
// 将裁判系统状态切换为运行状态
REF_SWITCH_STATUS(*ref, REF_STATUS_RUNNING); REF_SWITCH_STATUS(*ref, REF_STATUS_RUNNING);
uint32_t data_length =
REF_LEN_RX_BUFF - // 计算接收到的数据长度
__HAL_DMA_GET_COUNTER(BSP_UART_GetHandle(BSP_UART_REF)->hdmarx); uint32_t data_length = REF_LEN_RX_BUFF - __HAL_DMA_GET_COUNTER(BSP_UART_GetHandle(BSP_UART_REF)->hdmarx);
uint8_t index = 0; uint8_t index = 0;
uint8_t packet_shift; uint8_t packet_shift;
uint8_t packet_length; uint8_t packet_length;
// 遍历接收到的数据包
while (index < data_length && rxbuf[index] == REF_HEADER_SOF) { while (index < data_length && rxbuf[index] == REF_HEADER_SOF) {
packet_shift = index; packet_shift = index;
Referee_Header_t *header = (Referee_Header_t *)(rxbuf + index); Referee_Header_t *header = (Referee_Header_t *)(rxbuf + index);
index += sizeof(*header); index += sizeof(*header);
if (index - packet_shift >= data_length) goto error; if (index - packet_shift >= data_length) goto error;
// 验证CRC8校验
if (!CRC8_Verify((uint8_t *)header, sizeof(*header))) goto error; if (!CRC8_Verify((uint8_t *)header, sizeof(*header))) goto error;
// 验证数据包头
if (header->sof != REF_HEADER_SOF) goto error; if (header->sof != REF_HEADER_SOF) goto error;
Referee_CMDID_t *cmd_id = (Referee_CMDID_t *)(rxbuf + index); Referee_CMDID_t *cmd_id = (Referee_CMDID_t *)(rxbuf + index);
@@ -143,6 +147,7 @@ int8_t Referee_Parse(Referee_t *ref) {
void *origin; void *origin;
size_t size; size_t size;
// 根据命令ID解析数据包内容
switch (*cmd_id) { switch (*cmd_id) {
case REF_CMD_ID_GAME_STATUS: case REF_CMD_ID_GAME_STATUS:
origin = &(ref->game_status); origin = &(ref->game_status);
@@ -156,25 +161,10 @@ int8_t Referee_Parse(Referee_t *ref) {
origin = &(ref->game_robot_hp); origin = &(ref->game_robot_hp);
size = sizeof(ref->game_robot_hp); size = sizeof(ref->game_robot_hp);
break; break;
case REF_CMD_ID_DART_STATUS:
origin = &(ref->dart_status);
size = sizeof(ref->dart_status);
break;
case REF_CMD_ID_ICRA_ZONE_STATUS:
origin = &(ref->icra_zone);
size = sizeof(ref->icra_zone);
break;
case REF_CMD_ID_FIELD_EVENTS: case REF_CMD_ID_FIELD_EVENTS:
origin = &(ref->field_event); origin = &(ref->field_event);
size = sizeof(ref->field_event); size = sizeof(ref->field_event);
break; break;
case REF_CMD_ID_SUPPLY_ACTION:
origin = &(ref->supply_action);
size = sizeof(ref->supply_action);
break;
case REF_CMD_ID_REQUEST_SUPPLY:
origin = &(ref->request_supply);
size = sizeof(ref->request_supply);
case REF_CMD_ID_WARNING: case REF_CMD_ID_WARNING:
origin = &(ref->warning); origin = &(ref->warning);
size = sizeof(ref->warning); size = sizeof(ref->warning);
@@ -199,10 +189,6 @@ int8_t Referee_Parse(Referee_t *ref) {
origin = &(ref->robot_buff); origin = &(ref->robot_buff);
size = sizeof(ref->robot_buff); size = sizeof(ref->robot_buff);
break; break;
case REF_CMD_ID_DRONE_ENERGY:
origin = &(ref->drone_energy);
size = sizeof(ref->drone_energy);
break;
case REF_CMD_ID_ROBOT_DMG: case REF_CMD_ID_ROBOT_DMG:
origin = &(ref->robot_danage); origin = &(ref->robot_danage);
size = sizeof(ref->robot_danage); size = sizeof(ref->robot_danage);
@@ -223,26 +209,71 @@ int8_t Referee_Parse(Referee_t *ref) {
origin = &(ref->dart_client); origin = &(ref->dart_client);
size = sizeof(ref->dart_client); size = sizeof(ref->dart_client);
break; break;
case REF_CMD_ID_ROBOT_POS_TO_SENTRY:
origin = &(ref->pos_sentry);
size = sizeof(ref->pos_sentry);
break;
case REF_CMD_ID_RADAR_MARK:
origin = &(ref->radar_mark);
size = sizeof(ref->radar_mark);
break;
case REF_CMD_ID_SENTRY_DECISION:
origin = &(ref->sentry_decision);
size = sizeof(ref->sentry_decision);
break;
case REF_CMD_ID_RADAR_DECISION:
origin = &(ref->radar_decision);
size = sizeof(ref->radar_decision);
break;
case REF_CMD_ID_INTER_STUDENT:
origin = &(ref->interactive_data);
size = sizeof(ref->interactive_data);
break;
case REF_CMD_ID_INTER_STUDENT_CUSTOM: case REF_CMD_ID_INTER_STUDENT_CUSTOM:
origin = &(ref->custom); origin = &(ref->custom_robot_data);
size = sizeof(ref->custom); size = sizeof(ref->custom_robot_data);
break;
case REF_CMD_ID_CLIENT_MAP: case REF_CMD_ID_CLIENT_MAP:
origin = &(ref->client_map); origin = &(ref->client_map);
size = sizeof(ref->client_map); size = sizeof(ref->client_map);
break;
case REF_CMD_ID_KEYBOARD_MOUSE: case REF_CMD_ID_KEYBOARD_MOUSE:
origin = &(ref->keyboard_mouse); origin = &(ref->keyboard_mouse);
size = sizeof(ref->keyboard_mouse); size = sizeof(ref->keyboard_mouse);
break;
case REF_CMD_ID_MAP_ROBOT_DATA:
origin = &(ref->map_robot_data);
size = sizeof(ref->map_robot_data);
break;
case REF_CMD_ID_CUSTOM_KEYBOARD_MOUSE:
origin = &(ref->custom_keyboard_mouse);
size = sizeof(ref->custom_keyboard_mouse);
break;
case REF_CMD_ID_SENTRY_POS_DATA:
origin = &(ref->sentry_pos_data);
size = sizeof(ref->sentry_pos_data);
break;
case REF_CMD_ID_ROBOT_POS_DATA:
origin = &(ref->custom_info);
size = sizeof(ref->custom_info);
break;
case REF_CMD_ID_DATA:
origin = &(ref->custom_data);
size = sizeof(ref->custom_data);
break;
default: default:
return DEVICE_ERR; return DEVICE_ERR;
} }
packet_length = sizeof(Referee_Header_t) + sizeof(Referee_CMDID_t) + size +
sizeof(Referee_Tail_t); // 计算数据包长度
packet_length = sizeof(Referee_Header_t) + sizeof(Referee_CMDID_t) + size + sizeof(Referee_Tail_t);
index += size; index += size;
if (index - packet_shift >= data_length) goto error; if (index - packet_shift >= data_length) goto error;
index += sizeof(Referee_Tail_t); index += sizeof(Referee_Tail_t);
if (index - packet_shift != packet_length) goto error; if (index - packet_shift != packet_length) goto error;
// 验证CRC16校验并复制数据
if (CRC16_Verify((uint8_t *)(rxbuf + packet_shift), packet_length)) if (CRC16_Verify((uint8_t *)(rxbuf + packet_shift), packet_length))
memcpy(origin, target, size); memcpy(origin, target, size);
else else
@@ -272,8 +303,12 @@ int8_t Referee_MoveData(void *data, void *tmp, uint32_t len) {
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header, int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id) { Referee_StudentCMDID_t cmd_id, uint8_t sender_id) {
// 设置数据命令ID
header->data_cmd_id = cmd_id; header->data_cmd_id = cmd_id;
if (sender_id <= REF_BOT_RED_RADER) switch (sender_id) {
// 根据发送者ID设置发送者和接收者ID
if (sender_id <= REF_BOT_RED_RADER) {
switch (sender_id) {
case REF_BOT_RED_HERO: case REF_BOT_RED_HERO:
header->sender_ID = REF_BOT_RED_HERO; header->sender_ID = REF_BOT_RED_HERO;
header->receiver_ID = REF_CL_RED_HERO; header->receiver_ID = REF_CL_RED_HERO;
@@ -305,9 +340,9 @@ int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
header->sender_ID = REF_BOT_RED_RADER; header->sender_ID = REF_BOT_RED_RADER;
break; break;
default: default:
return -1; return -1; // 无效的发送者ID
} }
else } else {
switch (sender_id) { switch (sender_id) {
case REF_BOT_BLU_HERO: case REF_BOT_BLU_HERO:
header->sender_ID = REF_BOT_BLU_HERO; header->sender_ID = REF_BOT_BLU_HERO;
@@ -340,9 +375,10 @@ int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
header->sender_ID = REF_BOT_BLU_RADER; header->sender_ID = REF_BOT_BLU_RADER;
break; break;
default: default:
return -1; return -1; // 无效的发送者ID
} }
return 0; }
return 0; // 成功
} }
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref) { int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref) {

417
User/device/referee.h
View File

@@ -21,11 +21,11 @@ extern "C" {
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat)) #define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
#define CHASSIS_POWER_MAX_WITHOUT_REF 50.0f /* 裁判系统离线底盘最大功率 */ #define CHASSIS_POWER_MAX_WITHOUT_REF 2000.0f /* 裁判系统离线底盘最大功率 */
#define REF_UI_MAX_GRAPIC_NUM (7) #define REF_UI_MAX_GRAPIC_NUM (7) /* 最大图形数量 */
#define REF_UI_MAX_STRING_NUM (7) #define REF_UI_MAX_STRING_NUM (7) /* 最大字符串数量 */
#define REF_UI_MAX_DEL_NUM (3) #define REF_UI_MAX_DEL_NUM (3) /* 最大删除数量 */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef struct __packed { typedef struct __packed {
@@ -38,30 +38,39 @@ typedef struct __packed {
typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t; typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t;
typedef enum { typedef enum {
REF_CMD_ID_GAME_STATUS = 0x0001, REF_CMD_ID_GAME_STATUS = 0x0001, //比赛状态数据，固定以1Hz频率发送
REF_CMD_ID_GAME_RESULT = 0x0002, REF_CMD_ID_GAME_RESULT = 0x0002, //比赛结果数据，比赛结束触发发送
REF_CMD_ID_GAME_ROBOT_HP = 0x0003, REF_CMD_ID_GAME_ROBOT_HP = 0x0003, //机器人血量数据，固定以3Hz频率发送
REF_CMD_ID_DART_STATUS = 0x0004, //REF_CMD_ID_DART_STATUS = 0x0004,
REF_CMD_ID_ICRA_ZONE_STATUS = 0x0005, // REF_CMD_ID_ICRA_ZONE_STATUS = 0x0005,
REF_CMD_ID_FIELD_EVENTS = 0x0101, REF_CMD_ID_FIELD_EVENTS = 0x0101, //场地事件数据，固定以1Hz频率发送
REF_CMD_ID_SUPPLY_ACTION = 0x0102, //REF_CMD_ID_SUPPLY_ACTION = 0x0102,
REF_CMD_ID_REQUEST_SUPPLY = 0x0103, //REF_CMD_ID_REQUEST_SUPPLY = 0x0103,
REF_CMD_ID_WARNING = 0x0104, REF_CMD_ID_WARNING = 0x0104, //裁判警告数据
REF_CMD_ID_DART_COUNTDOWN = 0x0105, REF_CMD_ID_DART_COUNTDOWN = 0x0105, //飞镖发射相关数据
REF_CMD_ID_ROBOT_STATUS = 0x0201, REF_CMD_ID_ROBOT_STATUS = 0x0201, //机器人性能体系数据
REF_CMD_ID_POWER_HEAT_DATA = 0x0202, REF_CMD_ID_POWER_HEAT_DATA = 0x0202, //实时底盘缓冲能量和射击热量数据
REF_CMD_ID_ROBOT_POS = 0x0203, REF_CMD_ID_ROBOT_POS = 0x0203, //机器人位置数据
REF_CMD_ID_ROBOT_BUFF = 0x0204, REF_CMD_ID_ROBOT_BUFF = 0x0204, //机器人增益和底盘能量数据
REF_CMD_ID_DRONE_ENERGY = 0x0205, //REF_CMD_ID_DRONE_ENERGY = 0x0205,
REF_CMD_ID_ROBOT_DMG = 0x0206, REF_CMD_ID_ROBOT_DMG = 0x0206, //伤害状态数据
REF_CMD_ID_SHOOT_DATA = 0x0207, REF_CMD_ID_SHOOT_DATA = 0x0207, //实时射击数据
REF_CMD_ID_BULLET_REMAINING = 0x0208, REF_CMD_ID_BULLET_REMAINING = 0x0208, //允许发弹量
REF_CMD_ID_RFID = 0x0209, REF_CMD_ID_RFID = 0x0209, //机器人RFID模块状态
REF_CMD_ID_DART_CLIENT = 0x020A, REF_CMD_ID_DART_CLIENT = 0x020A, //飞镖选手端指令数据
REF_CMD_ID_INTER_STUDENT = 0x0301, REF_CMD_ID_ROBOT_POS_TO_SENTRY = 0X020B, //地面机器人位置数据
REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, REF_CMD_ID_RADAR_MARK = 0X020C, //雷达标记进度数据
REF_CMD_ID_CLIENT_MAP = 0x0303, REF_CMD_ID_SENTRY_DECISION = 0x020D, //哨兵自主决策相关信息同步
REF_CMD_ID_KEYBOARD_MOUSE = 0x0304, REF_CMD_ID_RADAR_DECISION = 0x020E, //雷达自主决策相关信息同步
REF_CMD_ID_INTER_STUDENT = 0x0301, //机器人交互数据
REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, //自定义控制器与机器人交互数据
REF_CMD_ID_CLIENT_MAP = 0x0303, //选手端小地图交互数据
REF_CMD_ID_KEYBOARD_MOUSE = 0x0304, //键鼠遥控数据
REF_CMD_ID_MAP_ROBOT_DATA = 0x0305, //选手端小地图接收雷达数据
REF_CMD_ID_CUSTOM_KEYBOARD_MOUSE = 0X0306, //自定义控制器与选手端交互数据
REF_CMD_ID_SENTRY_POS_DATA = 0x0307, //选手端小地图接收哨兵数据
REF_CMD_ID_ROBOT_POS_DATA = 0x0308, //选手端小地图接受机器人消息
REF_CMD_ID_DATA = 0x0309, //自定义控制器接收机器人数据
} Referee_CMDID_t; } Referee_CMDID_t;
typedef struct __packed { typedef struct __packed {
@@ -69,19 +78,18 @@ typedef struct __packed {
uint8_t game_progress : 4; uint8_t game_progress : 4;
uint16_t stage_remain_time; uint16_t stage_remain_time;
uint64_t sync_time_stamp; uint64_t sync_time_stamp;
} Referee_GameStatus_t; } Referee_GameStatus_t;/* 0x0001 */
typedef struct __packed { typedef struct __packed {
uint8_t winner; uint8_t winner;
} Referee_GameResult_t; } Referee_GameResult_t;/* 0x0002 */
typedef struct __packed { typedef struct __packed {
uint16_t red_1; uint16_t red_1;
uint16_t red_2; uint16_t red_2;
uint16_t red_3; uint16_t red_3;
uint16_t red_4; uint16_t red_4;
uint16_t red_5; uint16_t reserved;
uint16_t red_6;
uint16_t red_7; uint16_t red_7;
uint16_t red_outpose; uint16_t red_outpose;
uint16_t red_base; uint16_t red_base;
@@ -89,12 +97,11 @@ typedef struct __packed {
uint16_t blue_2; uint16_t blue_2;
uint16_t blue_3; uint16_t blue_3;
uint16_t blue_4; uint16_t blue_4;
uint16_t blue_5; uint16_t reserved1;
uint16_t blue_6;
uint16_t blue_7; uint16_t blue_7;
uint16_t blue_outpose; uint16_t blue_outpose;
uint16_t blue_base; uint16_t blue_base;
} Referee_GameRobotHP_t; } Referee_GameRobotHP_t;/* 0x0003 */
typedef struct __packed { typedef struct __packed {
uint8_t dart_belong; uint8_t dart_belong;
@@ -121,51 +128,44 @@ typedef struct __packed {
} Referee_ICRAZoneStatus_t; } Referee_ICRAZoneStatus_t;
typedef struct __packed { typedef struct __packed {
uint8_t copter_pad : 2; uint32_t exchange_nooverlap_status : 1;
uint8_t energy_mech : 2; uint32_t exchange_overlap_status : 1;
uint8_t virtual_shield : 1; uint32_t supply_status : 1;
uint32_t res : 27; uint32_t energy_mech_small_status : 1;
} Referee_FieldEvents_t; uint32_t energy_mech_big_status : 1;
uint32_t highland_annular : 2;
uint32_t highland_trapezium_1 : 2;
uint32_t last_hit_time : 9;
uint32_t last_hit_target : 3;
uint32_t activation_status : 2;
uint32_t res : 9;
} Referee_FieldEvents_t;/* 0x0101 */
typedef struct __packed { typedef struct __packed {
uint8_t supply_id; uint8_t supply_id;
uint8_t robot_id; uint8_t robot_id;
uint8_t supply_step; uint8_t supply_step;
uint8_t supply_sum; uint8_t supply_sum;
} Referee_SupplyAction_t; } Referee_SupplyAction_t;/* 0x0102 */
typedef struct __packed { typedef struct __packed {
uint8_t place_holder; /* TODO */ uint8_t place_holder; /* TODO */
} Referee_RequestSupply_t; } Referee_RequestSupply_t;/* 0x0103 */
typedef struct __packed { typedef struct __packed {
uint8_t level; uint8_t level;
uint8_t robot_id; uint8_t robot_id;
} Referee_Warning_t; uint8_t count;
} Referee_Warning_t;/* 0x0104 */
typedef struct __packed { typedef struct __packed {
uint8_t countdown; uint8_t countdown;
} Referee_DartCountdown_t; uint16_t dart_last_target : 3;
uint16_t attack_count : 2;
uint16_t dart_target : 2;
uint16_t res : 8;
} Referee_DartCountdown_t;/* 0x0105 */
// typedef struct __packed {
// uint8_t robot_id;
// uint8_t robot_level;
// uint16_t remain_hp;
// uint16_t max_hp;
// uint16_t shoot_id1_17_cooling_rate;
// uint16_t shoot_id1_17_heat_limit;
// uint16_t shoot_id1_17_speed_limit;
// uint16_t shoot_id2_17_cooling_rate;
// uint16_t shoot_id2_17_heat_limit;
// uint16_t shoot_id2_17_speed_limit;
// uint16_t shoot_42_cooling_rate;
// uint16_t shoot_42_heat_limit;
// uint16_t shoot_42_speed_limit;
// uint16_t chassis_power_limit;
// uint8_t power_gimbal_output : 1;
// uint8_t power_chassis_output : 1;
// uint8_t power_shoot_output : 1;
// } Referee_RobotStatus_t;
typedef struct __packed { typedef struct __packed {
uint8_t robot_id; uint8_t robot_id;
@@ -188,22 +188,15 @@ typedef struct __packed {
uint16_t shoot_id1_17_heat; uint16_t shoot_id1_17_heat;
uint16_t shoot_id2_17_heat; uint16_t shoot_id2_17_heat;
uint16_t shoot_42_heat; uint16_t shoot_42_heat;
} Referee_PowerHeat_t; } Referee_PowerHeat_t; /* 0x0202 */
typedef struct __packed { typedef struct __packed {
float x; float x;
float y; float y;
float z; float z;
float yaw; float yaw;
} Referee_RobotPos_t; } Referee_RobotPos_t; /* 0x0203 */
// typedef struct __packed {
// uint8_t healing : 1;
// uint8_t cooling_acc : 1;
// uint8_t defense_buff : 1;
// uint8_t attack_buff : 1;
// uint8_t res : 4;
// } Referee_RobotBuff_t;
typedef struct __packed { typedef struct __packed {
uint8_t healing_buff; uint8_t healing_buff;
@@ -211,16 +204,17 @@ typedef struct __packed {
uint8_t defense_buff; uint8_t defense_buff;
uint8_t vulnerability_buff; uint8_t vulnerability_buff;
uint16_t attack_buff; uint16_t attack_buff;
} Referee_RobotBuff_t; uint8_t remaining_energy;
} Referee_RobotBuff_t; /* 0x0204 */
typedef struct __packed { typedef struct __packed {
uint8_t attack_countdown; uint8_t attack_countdown;
} Referee_DroneEnergy_t; } Referee_DroneEnergy_t; /* 0x0205 *///删
typedef struct __packed { typedef struct __packed {
uint8_t armor_id : 4; uint8_t armor_id : 4;
uint8_t damage_type : 4; uint8_t damage_type : 4;
} Referee_RobotDamage_t; } Referee_RobotDamage_t; /* 0x0206 */
typedef struct __packed { typedef struct __packed {
uint8_t bullet_type; uint8_t bullet_type;
@@ -233,38 +227,106 @@ typedef struct __packed {
uint16_t bullet_17_remain; uint16_t bullet_17_remain;
uint16_t bullet_42_remain; uint16_t bullet_42_remain;
uint16_t coin_remain; uint16_t coin_remain;
} Referee_BulletRemain_t; } Referee_BulletRemain_t;/* 0x0208 */
typedef struct __packed { typedef struct __packed {
uint8_t base : 1; uint32_t own_base : 1; //0
uint8_t high_ground : 1; uint32_t own_highland_annular : 1; //1
uint8_t energy_mech : 1; uint32_t enemy_highland_annular : 1; //2
uint8_t slope : 1; uint32_t own_trapezium_R3B3 : 1; //3
uint8_t outpose : 1; uint32_t enemy_trapezium_R3B3 : 1; //4
uint8_t resource : 1; uint32_t own_slope_before : 1; //坡道前5
uint8_t healing_card : 1; uint32_t own_slope_after : 1; //坡道后6
uint32_t res : 24; uint32_t enemy_slope_before : 1; //坡道前7
} Referee_RFID_t; uint32_t enemy_slope_after : 1; //坡道后8
uint32_t own_crosses_above : 1; //增益点 上方9
uint32_t own_crosses_below : 1; //增益点 下方10
uint32_t enemy_crosses_above : 1; //增益点 上方11
uint32_t enemy_crosses_below : 1; //增益点 下方12
uint32_t own_highway_above : 1; //公路 上方13
uint32_t own_highway_below : 1; //公路 下方14
uint32_t enemy_highway_above : 1; //公路 上方15
uint32_t enemy_highway_below : 1; //公路 下方16
uint32_t own_fortress : 1; //己方堡垒增益点17
uint32_t own_outpost : 1; //己方前哨站增益点18
uint32_t own_RMUL : 1; //己方RMUL增益点19
uint32_t enemy_RMDL : 1; //敌方RMDL增益点20
uint32_t own_island : 1; //己方资源岛 21
uint32_t enemy_island : 1; //敌方资源岛 22
uint32_t centre : 1; //中心 23
uint32_t res : 8;
} Referee_RFID_t;/* 0x0209 */
typedef struct __packed { typedef struct __packed {
uint8_t opening; uint8_t opening_status;
uint8_t target; uint8_t target;
uint8_t target_changable_countdown; uint16_t target_changable_countdown;
uint8_t dart1_speed;
uint8_t dart2_speed;
uint8_t dart3_speed;
uint8_t dart4_speed;
uint16_t last_dart_launch_time;
uint16_t operator_cmd_launch_time; uint16_t operator_cmd_launch_time;
} Referee_DartClient_t; } Referee_DartClient_t;/* 0x020A */
typedef struct __packed {
float hero_x;
float hero_y;
float engineer_x;
float engineer_y;
float standard_3_x;
float standard_3_y;
float standard_4_x;
float standard_4_y;
float reserved;
float reserved1;
} Referee_Robot_Position_t;/* 0x020B */
typedef struct __packed
{
uint8_t mark_hero_progress;
uint8_t mark_engineer_progress;
uint8_t mark_standard_3_progress;
uint8_t mark_standard_4_progress;
uint8_t mark_sentry_progress;
}Referee_Radar_mark_data_t; /* 0x020C */
typedef struct __packed
{
uint32_t exchanged_bullet_num : 11;
uint32_t exchanged_bullet_times : 4;
uint32_t exchanged_blood_times : 4;
uint32_t Confirmation_resurrection : 1;
uint32_t now_resurrection : 1;
uint32_t money : 10;
uint32_t res : 1;
uint16_t state : 1;
uint16_t quantity : 11;
uint16_t res1 : 4;
} Referee_Sentry_Info_t; /* 0x020D */
typedef struct __packed
{
uint8_t qualification : 2;
uint8_t enemy_status : 1;
uint8_t res : 5;
} Referee_Radar_Info_t; /* 0x020E */
typedef struct __packed
{
uint16_t data_cmd_id; /* 子内容ID */
uint16_t sender_id;
uint16_t receiver_id;
uint8_t user_data[112];
} Referee_Interaction_Data_t; /* 0x0301 */
typedef struct __packed
{
uint8_t data[30];
} Referee_custom_robot_data_t; /* 0x0302 */
typedef struct __packed { typedef struct __packed {
float position_x; float position_x;
float position_y; float position_y;
float position_z;
uint8_t commd_keyboard; uint8_t commd_keyboard;
uint16_t robot_id; uint8_t robot_id;
} Referee_ClientMap_t; uint8_t cmd_source;
} Referee_ClientMap_t;/* 0x0303 */
typedef struct __packed { typedef struct __packed {
int16_t mouse_x; int16_t mouse_x;
@@ -274,10 +336,74 @@ typedef struct __packed {
int8_t button_r; int8_t button_r;
uint16_t keyboard_value; uint16_t keyboard_value;
uint16_t res; uint16_t res;
} Referee_KeyboardMouse_t; } Referee_KeyboardMouse_t;/* 0x0304 */
typedef struct __packed {
uint16_t hero_position_x;
uint16_t hero_position_y;
uint16_t engineer_position_x;
uint16_t engineer_position_y;
uint16_t infantry_3_position_x;
uint16_t infantry_3_position_y;
uint16_t infantry_4_position_x;
uint16_t infantry_4_position_y;
uint16_t infantry_5_position_x;
uint16_t infantry_5_position_y;
uint16_t sentry_position_x;
uint16_t sentry_position_y;
} Referee_map_robot_data_t;/* 0x0305 */
typedef struct __packed {
uint16_t key_value;
uint16_t x_position : 12;
uint16_t mouse_left : 4;
uint16_t y_position : 12;
uint16_t mouse_right : 4;
uint16_t res;
} Referee_custom_client_data_t; /* 0x0306 */
typedef struct __packed {
uint8_t intention;
uint16_t start_position_x;
uint16_t start_position_y;
int8_t delta_x[49];
int8_t delta_y[49];
uint16_t sender_id;
} Referee_map_data_t;/* 0x0307 */
typedef struct __packed
{
uint16_t sender_id;
uint16_t receiver_id;
uint8_t user_data[30];
}Referee_custom_info_t; /* 0x0308 */
typedef struct __packed
{
uint8_t data[30];
}Referee_robot_custom_data_t; /* 0x0309*/
typedef uint16_t Referee_Tail_t; typedef uint16_t Referee_Tail_t;
// typedef enum {
// REF_BOT_RED_HERO = 1,
// REF_BOT_RED_ENGINEER = 2,
// REF_BOT_RED_INFANTRY_1 = 3,
// REF_BOT_RED_INFANTRY_2 = 4,
// REF_BOT_RED_INFANTRY_3 = 5,
// REF_BOT_RED_DRONE = 6,
// REF_BOT_RED_SENTRY = 7,
// REF_BOT_RED_RADER = 9,
// REF_BOT_BLU_HERO = 101,
// REF_BOT_BLU_ENGINEER = 102,
// REF_BOT_BLU_INFANTRY_1 = 103,
// REF_BOT_BLU_INFANTRY_2 = 104,
// REF_BOT_BLU_INFANTRY_3 = 105,
// REF_BOT_BLU_DRONE = 106,
// REF_BOT_BLU_SENTRY = 107,
// REF_BOT_BLU_RADER = 109,
// } Referee_RobotID_t;//
typedef enum { typedef enum {
REF_BOT_RED_HERO = 1, REF_BOT_RED_HERO = 1,
REF_BOT_RED_ENGINEER = 2, REF_BOT_RED_ENGINEER = 2,
@@ -286,7 +412,10 @@ typedef enum {
REF_BOT_RED_INFANTRY_3 = 5, REF_BOT_RED_INFANTRY_3 = 5,
REF_BOT_RED_DRONE = 6, REF_BOT_RED_DRONE = 6,
REF_BOT_RED_SENTRY = 7, REF_BOT_RED_SENTRY = 7,
REF_BOT_RED_DART = 8,
REF_BOT_RED_RADER = 9, REF_BOT_RED_RADER = 9,
REF_BOT_RED_OUTPOST = 10,
REF_BOT_RED_BASE = 11,
REF_BOT_BLU_HERO = 101, REF_BOT_BLU_HERO = 101,
REF_BOT_BLU_ENGINEER = 102, REF_BOT_BLU_ENGINEER = 102,
REF_BOT_BLU_INFANTRY_1 = 103, REF_BOT_BLU_INFANTRY_1 = 103,
@@ -294,9 +423,27 @@ typedef enum {
REF_BOT_BLU_INFANTRY_3 = 105, REF_BOT_BLU_INFANTRY_3 = 105,
REF_BOT_BLU_DRONE = 106, REF_BOT_BLU_DRONE = 106,
REF_BOT_BLU_SENTRY = 107, REF_BOT_BLU_SENTRY = 107,
REF_BOT_BLU_DART = 108,
REF_BOT_BLU_RADER = 109, REF_BOT_BLU_RADER = 109,
} Referee_RobotID_t; REF_BOT_BLU_OUTPOST = 110,
REF_BOT_BLU_BASE = 111,
} Referee_RobotID_t;//
// typedef enum {
// REF_CL_RED_HERO = 0x0101,
// REF_CL_RED_ENGINEER = 0x0102,
// REF_CL_RED_INFANTRY_1 = 0x0103,
// REF_CL_RED_INFANTRY_2 = 0x0104,
// REF_CL_RED_INFANTRY_3 = 0x0105,
// REF_CL_RED_DRONE = 0x0106,
// REF_CL_BLU_HERO = 0x0165,
// REF_CL_BLU_ENGINEER = 0x0166,
// REF_CL_BLU_INFANTRY_1 = 0x0167,
// REF_CL_BLU_INFANTRY_2 = 0x0168,
// REF_CL_BLU_INFANTRY_3 = 0x0169,
// REF_CL_BLU_DRONE = 0x016A,
// } Referee_ClientID_t;//选手端ID
typedef enum { typedef enum {
REF_CL_RED_HERO = 0x0101, REF_CL_RED_HERO = 0x0101,
REF_CL_RED_ENGINEER = 0x0102, REF_CL_RED_ENGINEER = 0x0102,
@@ -310,8 +457,20 @@ typedef enum {
REF_CL_BLU_INFANTRY_2 = 0x0168, REF_CL_BLU_INFANTRY_2 = 0x0168,
REF_CL_BLU_INFANTRY_3 = 0x0169, REF_CL_BLU_INFANTRY_3 = 0x0169,
REF_CL_BLU_DRONE = 0x016A, REF_CL_BLU_DRONE = 0x016A,
} Referee_ClientID_t; REF_CL_REFEREE_SERVER = 0x8080, /* 裁判系统服务器，用于哨兵和雷达自主决策 */
} Referee_ClientID_t;//选手端ID
// typedef enum {
// REF_STDNT_CMD_ID_UI_DEL = 0x0100,
// REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101,
// REF_STDNT_CMD_ID_UI_DRAW2 = 0x0102,
// REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103,
// REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104,
// REF_STDNT_CMD_ID_UI_STR = 0x0110,
// REF_STDNT_CMD_ID_CUSTOM = 0x0200,
// } Referee_StudentCMDID_t;
typedef enum { typedef enum {
REF_STDNT_CMD_ID_UI_DEL = 0x0100, REF_STDNT_CMD_ID_UI_DEL = 0x0100,
REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101, REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101,
@@ -319,8 +478,9 @@ typedef enum {
REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103, REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103,
REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104, REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104,
REF_STDNT_CMD_ID_UI_STR = 0x0110, REF_STDNT_CMD_ID_UI_STR = 0x0110,
REF_STDNT_CMD_ID_CUSTOM = 0x0200, REF_STDNT_CMD_ID_CUSTOM = 0x0200,
REF_STDNT_CMD_ID_SENTRY_CMD = 0X0120,
REF_STDNT_CMD_ID_RADAR_CMD = 0X0121,
} Referee_StudentCMDID_t; } Referee_StudentCMDID_t;
typedef struct __packed { typedef struct __packed {
@@ -336,29 +496,40 @@ typedef struct __packed {
typedef struct { typedef struct {
Referee_Status_t ref_status; Referee_Status_t ref_status;
Referee_GameStatus_t game_status; Referee_GameStatus_t game_status; /* 0x0001 */
Referee_GameResult_t game_result; Referee_GameResult_t game_result; /* 0x0002 */
Referee_GameRobotHP_t game_robot_hp; Referee_GameRobotHP_t game_robot_hp; /* 0x0003 */
Referee_DartStatus_t dart_status; // Referee_DartStatus_t dart_status;
Referee_ICRAZoneStatus_t icra_zone; // Referee_ICRAZoneStatus_t icra_zone;
Referee_FieldEvents_t field_event; Referee_FieldEvents_t field_event; /* 0x0101 */
Referee_SupplyAction_t supply_action; //Referee_SupplyAction_t supply_action; /* 0x0102 */
Referee_RequestSupply_t request_supply; Referee_RequestSupply_t request_supply; /* 0x0103 */
Referee_Warning_t warning; Referee_Warning_t warning; /* 0x0104 */
Referee_DartCountdown_t dart_countdown; Referee_DartCountdown_t dart_countdown; /* 0x0105 */
Referee_RobotStatus_t robot_status; Referee_RobotStatus_t robot_status; /* 0x0201 */
Referee_PowerHeat_t power_heat; Referee_PowerHeat_t power_heat; /* 0x0202 */
Referee_RobotPos_t robot_pos; Referee_RobotPos_t robot_pos; /* 0x0203 */
Referee_RobotBuff_t robot_buff; Referee_RobotBuff_t robot_buff; /* 0x0204 */
Referee_DroneEnergy_t drone_energy; //Referee_DroneEnergy_t drone_energy; /* 0x0205 */
Referee_RobotDamage_t robot_danage; Referee_RobotDamage_t robot_danage; /* 0x0206 */
Referee_ShootData_t shoot_data; Referee_ShootData_t shoot_data; /* 0x0207 */
Referee_BulletRemain_t bullet_remain; Referee_BulletRemain_t bullet_remain; /* 0x0208 */
Referee_RFID_t rfid; Referee_RFID_t rfid; /* 0x0209 */
Referee_DartClient_t dart_client; Referee_DartClient_t dart_client; /* 0x020A */
Referee_InterStudent_Custom_t custom; Referee_Robot_Position_t pos_sentry; /* 0x020B */
Referee_ClientMap_t client_map; Referee_Radar_mark_data_t radar_mark; /* 0x020C */
Referee_KeyboardMouse_t keyboard_mouse; Referee_Sentry_Info_t sentry_decision; /* 0x020D */
Referee_Radar_Info_t radar_decision; /* 0x020E */
// Referee_InterStudent_Custom_t custom;
Referee_Interaction_Data_t interactive_data; /* 0x0301 */
Referee_custom_robot_data_t custom_robot_data; /* 0x0302 */
Referee_ClientMap_t client_map; /* 0x0303 */
Referee_KeyboardMouse_t keyboard_mouse; /* 0x0304 */
Referee_map_robot_data_t map_robot_data; /* 0x0305 */
Referee_custom_client_data_t custom_keyboard_mouse; /* 0x0306 */
Referee_map_data_t sentry_pos_data; /* 0x0307 */
Referee_custom_info_t custom_info; /* 0x0308 */
Referee_robot_custom_data_t custom_data; /* 0x0309*/
osTimerId_t ui_fast_timer_id; osTimerId_t ui_fast_timer_id;
osTimerId_t ui_slow_timer_id; osTimerId_t ui_slow_timer_id;

97
User/module/config.c
View File

@@ -188,19 +188,25 @@ static const Config_RobotParam_t param_default = {
.cover_open_duty = 0.10f, .cover_open_duty = 0.10f,
.cover_close_duty = 0.050f, .cover_close_duty = 0.050f,
.model = SHOOT_MODEL_17MM, .model = SHOOT_MODEL_17MM,
.bullet_speed = 25.f, .bullet_speed = 30.f,
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f), .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
}, /* shoot */ }, /* shoot */
.can = { .can = {
.chassis = BSP_CAN_1, .chassis = BSP_CAN_1,
.gimbal = BSP_CAN_2, .gimbal.yaw = BSP_CAN_2,
.gimbal.pitch = BSP_CAN_2,
.shoot = BSP_CAN_2, .shoot = BSP_CAN_2,
.trig = BSP_CAN_2,
.cap = BSP_CAN_1, .cap = BSP_CAN_1,
}, /* can */ }, /* can */
}; /* param_default */ }; /* param_default */
#ifdef DEBUG
Config_RobotParam_t param_hero = {
#else
static const Config_RobotParam_t param_hero = { static const Config_RobotParam_t param_hero = {
#endif
.model = ROBOT_MODEL_HERO, .model = ROBOT_MODEL_HERO,
.chassis = { /* 底盘模块参数 */ .chassis = { /* 底盘模块参数 */
@@ -234,7 +240,7 @@ static const Config_RobotParam_t param_hero = {
}, },
.reverse = { .reverse = {
.yaw = true, .yaw = false,
}, },
}, /* chassis */ }, /* chassis */
@@ -242,28 +248,28 @@ static const Config_RobotParam_t param_hero = {
.pid = { .pid = {
{ {
/* GIMBAL_PID_YAW_OMEGA_IDX */ /* GIMBAL_PID_YAW_OMEGA_IDX */
.k = 0.45f, .k = 0.4f,
.p = 1.0f, .p = 1.0f,
.i = 6.0f, .i = 0.5f,
.d = 0.0008f, .d = 0.0f,
.i_limit = 1.0f, .i_limit = 1.0f,
.out_limit = 1.0f, .out_limit = 1.0f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = -1.0f, .range = -1.0f,
}, { }, {
/* GIMBAL_PID_YAW_ANGLE_IDX */ /* GIMBAL_PID_YAW_ANGLE_IDX */
.k = 20.0f, .k = 10.0f,
.p = 1.0f, .p = 1.0f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.05f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 10.0f, .out_limit = 10.0f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = M_2PI, .range = M_2PI,
}, { }, {
/* GIMBAL_PID_PIT_OMEGA_IDX */ /* GIMBAL_PID_PIT_OMEGA_IDX */
.k = 0.25f, .k = 2.0f,
.p = 1.0f, .p = 0.8f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 1.0f, .i_limit = 1.0f,
@@ -272,9 +278,9 @@ static const Config_RobotParam_t param_hero = {
.range = -1.0f, .range = -1.0f,
}, { }, {
/* GIMBAL_PID_PIT_ANGLE_IDX */ /* GIMBAL_PID_PIT_ANGLE_IDX */
.k = 12.0f, .k = 4.0f,
.p = 1.0f, .p = 6.0f,
.i = 0.0f, .i = 1.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 10.0f, .out_limit = 10.0f,
@@ -283,7 +289,7 @@ static const Config_RobotParam_t param_hero = {
}, },
}, /* pid */ }, /* pid */
.pitch_travel_rad = 1.07685447f, .pitch_travel_rad = 0.9685447f,
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@@ -291,8 +297,8 @@ static const Config_RobotParam_t param_hero = {
}, },
.reverse = { .reverse = {
.yaw = true, .yaw = false,
.pit = true, .pit = false,
}, },
}, /* gimbal */ }, /* gimbal */
@@ -309,7 +315,7 @@ static const Config_RobotParam_t param_hero = {
}, },
.trig_pid_param = { .trig_pid_param = {
.k = 10.0f, .k = 10.0f,
.p = 1.0f, .p = 0.95f,
.i = 0.0f, .i = 0.0f,
.d = 0.032f, .d = 0.032f,
.i_limit = 1.0f, .i_limit = 1.0f,
@@ -327,8 +333,8 @@ static const Config_RobotParam_t param_hero = {
.trig = -1.0f, .trig = -1.0f,
}, },
}, },
.num_trig_tooth = 6.0f, .num_trig_tooth = 5.0f,
.trig_gear_ratio = 3591.0f / 187.0f, .trig_gear_ratio = 19.0f,
.fric_radius = 0.03f, .fric_radius = 0.03f,
.cover_open_duty = 0.125f, .cover_open_duty = 0.125f,
.cover_close_duty = 0.075f, .cover_close_duty = 0.075f,
@@ -339,8 +345,10 @@ static const Config_RobotParam_t param_hero = {
.can = { .can = {
.chassis = BSP_CAN_1, .chassis = BSP_CAN_1,
.gimbal = BSP_CAN_2, .gimbal.yaw = BSP_CAN_1,
.gimbal.pitch = BSP_CAN_2,
.shoot = BSP_CAN_2, .shoot = BSP_CAN_2,
.trig = BSP_CAN_1,
.cap = BSP_CAN_1, .cap = BSP_CAN_1,
}, /* can */ }, /* can */
}; /* param_hero */ }; /* param_hero */
@@ -349,8 +357,8 @@ static const Config_RobotParam_t param_hero = {
static const Config_PilotCfg_t cfg_qs = { static const Config_PilotCfg_t cfg_qs = {
.param = { .param = {
.sens_mouse = 0.06f, .sens_mouse = 0.01f,
.sens_rc = 6.0f, .sens_rc = 4.0f,
.map = { .map = {
.key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W}, .key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
.key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S}, .key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
@@ -430,17 +438,21 @@ static const Config_PilotCfgMap_t pilot_cfg_map[] = {
* *
* \param cfg 配置信息 * \param cfg 配置信息
*/ */
void Config_Get(Config_t *cfg) { void Config_Get(Config_t *cfg)
{
BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg)); BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
cfg->pilot_cfg = Config_GetPilotCfg(cfg->pilot_cfg_name); cfg->pilot_cfg = Config_GetPilotCfg(cfg->pilot_cfg_name);
cfg->robot_param = Config_GetRobotParam(cfg->robot_param_name); cfg->robot_param = Config_GetRobotParam(cfg->robot_param_name);
/* 防止第一次烧写后访问NULL指针 */ /* 防止第一次烧写后访问NULL指针 */
if (cfg->robot_param == NULL) cfg->robot_param = &param_default; if (cfg->robot_param == NULL)
if (cfg->pilot_cfg == NULL) cfg->pilot_cfg = &cfg_qs; cfg->robot_param = &param_default;
if (cfg->pilot_cfg == NULL)
cfg->pilot_cfg = &cfg_qs;
/* 防止擦除后全为1 */ /* 防止擦除后全为1 */
if ((uint32_t)(cfg->robot_param) == UINT32_MAX) if ((uint32_t)(cfg->robot_param) == UINT32_MAX)
cfg->robot_param = &param_default; cfg->robot_param = &param_default;
if ((uint32_t)(cfg->pilot_cfg) == UINT32_MAX) cfg->pilot_cfg = &cfg_qs; if ((uint32_t)(cfg->pilot_cfg) == UINT32_MAX)
cfg->pilot_cfg = &cfg_qs;
} }
/** /**
@@ -448,7 +460,8 @@ void Config_Get(Config_t *cfg) {
* *
* \param cfg 配置信息 * \param cfg 配置信息
*/ */
void Config_Set(Config_t *cfg) { void Config_Set(Config_t *cfg)
{
osKernelLock(); osKernelLock();
BSP_Flash_EraseSector(11); BSP_Flash_EraseSector(11);
BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg)); BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
@@ -461,10 +474,14 @@ void Config_Set(Config_t *cfg) {
* @param robot_param_name 机器人参数名称 * @param robot_param_name 机器人参数名称
* @return const Config_RobotParam_t* 机器人参数的指针 * @return const Config_RobotParam_t* 机器人参数的指针
*/ */
const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name) { const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name)
if (robot_param_name == NULL) return NULL; {
for (size_t j = 0; robot_param_map[j].name != NULL; j++) { if (robot_param_name == NULL)
if (strcmp(robot_param_map[j].name, robot_param_name) == 0) { return NULL;
for (size_t j = 0; robot_param_map[j].name != NULL; j++)
{
if (strcmp(robot_param_map[j].name, robot_param_name) == 0)
{
return robot_param_map[j].param; return robot_param_map[j].param;
} }
} }
@@ -477,20 +494,26 @@ const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name) {
* @param pilot_cfg_name 操作手配置名称 * @param pilot_cfg_name 操作手配置名称
* @return const Config_PilotCfg_t* 操作手配置的指针 * @return const Config_PilotCfg_t* 操作手配置的指针
*/ */
const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name) { const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name)
if (pilot_cfg_name == NULL) return NULL; {
for (size_t j = 0; pilot_cfg_map[j].name != NULL; j++) { if (pilot_cfg_name == NULL)
if (strcmp(pilot_cfg_map[j].name, pilot_cfg_name) == 0) { return NULL;
for (size_t j = 0; pilot_cfg_map[j].name != NULL; j++)
{
if (strcmp(pilot_cfg_map[j].name, pilot_cfg_name) == 0)
{
return pilot_cfg_map[j].param; return pilot_cfg_map[j].param;
} }
} }
return NULL; /* No match. */ return NULL; /* No match. */
} }
const Config_PilotCfgMap_t *Config_GetPilotNameMap(void) { const Config_PilotCfgMap_t *Config_GetPilotNameMap(void)
{
return pilot_cfg_map; return pilot_cfg_map;
} }
const Config_RobotParamMap_t *Config_GetRobotNameMap(void) { const Config_RobotParamMap_t *Config_GetRobotNameMap(void)
{
return robot_param_map; return robot_param_map;
} }

41
User/module/shoot.c
View File

@@ -14,8 +14,8 @@
#define HEAT_INCREASE_42MM (100.f) /* 每发射一颗42mm弹丸增加100热量 */ #define HEAT_INCREASE_42MM (100.f) /* 每发射一颗42mm弹丸增加100热量 */
#define HEAT_INCREASE_17MM (10.f) /* 每发射一颗17mm弹丸增加10热量 */ #define HEAT_INCREASE_17MM (10.f) /* 每发射一颗17mm弹丸增加10热量 */
#define BULLET_SPEED_LIMIT_42MM (16.0) #define BULLET_SPEED_LIMIT_42MM (15.0)
#define BULLET_SPEED_LIMIT_17MM (25.0) #define BULLET_SPEED_LIMIT_17MM (30.0)
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
@@ -35,7 +35,7 @@ static int8_t Shoot_SetMode(Shoot_t *s, CMD_ShootMode_t mode) {
if (mode == s->mode) return SHOOT_OK; if (mode == s->mode) return SHOOT_OK;
/* 切换模式后重置PID和滤波器 */ /* 切换模式后重置PID和滤波器 */
for (uint8_t i = 0; i < 2; i++) { for (uint8_t i = 0; i < 3; i++) {
PID_Reset(s->pid.fric + i); PID_Reset(s->pid.fric + i);
LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f); LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f);
LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f); LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f);
@@ -84,7 +84,7 @@ static int8_t Shoot_HeatLimit(Shoot_t *s, Referee_ForShoot_t *s_ref) {
/* 检测热量更新后,计算可发射弹丸 */ /* 检测热量更新后,计算可发射弹丸 */
if ((hc->heat != hc->last_heat) || (hc->heat == 0)) { if ((hc->heat != hc->last_heat) || (hc->heat == 0)) {
hc->available_shot = hc->available_shot =
(uint32_t)floorf((hc->heat_limit - hc->heat) / hc->heat_increase) - 2; (uint32_t)floorf((hc->heat_limit - hc->heat) / hc->heat_increase);
hc->last_heat = hc->heat; hc->last_heat = hc->heat;
} }
/* 计算已发射弹丸 */ /* 计算已发射弹丸 */
@@ -117,7 +117,7 @@ int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq) {
s->param = param; /* 初始化参数 */ s->param = param; /* 初始化参数 */
s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */ s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */
for (uint8_t i = 0; i < 2; i++) { for (uint8_t i = 0; i < 3; i++) {
/* PI控制器初始化PID */ /* PI控制器初始化PID */
PID_Init(s->pid.fric + i, KPID_MODE_NO_D, target_freq, PID_Init(s->pid.fric + i, KPID_MODE_NO_D, target_freq,
&(param->fric_pid_param)); &(param->fric_pid_param));
@@ -154,7 +154,7 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can) {
if (s == NULL) return -1; if (s == NULL) return -1;
if (can == NULL) return -1; if (can == NULL) return -1;
for (uint8_t i = 0; i < 2; i++) { for (uint8_t i = 0; i < 3; i++) {
s->feedback.fric_rpm[i] = can->motor.shoot.as_array[i].rotor_speed; s->feedback.fric_rpm[i] = can->motor.shoot.as_array[i].rotor_speed;
} }
@@ -222,11 +222,10 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
} }
break; break;
} }
case FIRE_MODE_CONT: { /* Recy模式 */ case FIRE_MODE_CONT: { /* 持续开火模式 */
// float shoot_freq = HeatLimit_ShootFreq( float shoot_freq = HeatLimit_ShootFreq(
// s->heat_ctrl.heat, s->heat_ctrl.heat_limit, s->heat_ctrl.cooling_rate, s->heat_ctrl.heat, s->heat_ctrl.heat_limit, s->heat_ctrl.cooling_rate,
// s->heat_ctrl.heat_increase, s->param->model == SHOOT_MODEL_17MM); s->heat_ctrl.heat_increase, s->param->model == SHOOT_MODEL_17MM);
float shoot_freq = 20.0f; //射频
s->fire_ctrl.period_ms = s->fire_ctrl.period_ms =
(shoot_freq == 0.0f) ? UINT32_MAX : (uint32_t)(1000.f / shoot_freq); (shoot_freq == 0.0f) ? UINT32_MAX : (uint32_t)(1000.f / shoot_freq);
break; break;
@@ -246,20 +245,26 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
} }
/* 计算摩擦轮转速的目标值 */ /* 计算摩擦轮转速的目标值 */
s->setpoint.fric_rpm[1] = // s->setpoint.fric_rpm[1] =
CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius, // CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
(s->param->model == SHOOT_MODEL_17MM)); // (s->param->model == SHOOT_MODEL_17MM));
s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1]; // s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
(s->param->model == SHOOT_MODEL_17MM));
s->setpoint.fric_rpm[0] = -fric_rpm;
s->setpoint.fric_rpm[1] = fric_rpm;
s->setpoint.fric_rpm[2] = -fric_rpm;
/* 计算拨弹电机位置的目标值 */ /* 计算拨弹电机位置的目标值 */
if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) && if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&
(s_cmd->fire)) { (s_cmd->fire)) {
/* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */ /* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
if (s_cmd->reverse_trig) { /* 反转拨弹 */ if (s_cmd->reverse_trig) { /* 反转拨弹 */
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth, CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
M_2PI); M_2PI);
} else { } else {
CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth, CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
M_2PI); M_2PI);
s->fire_ctrl.shooted++; s->fire_ctrl.shooted++;
s->fire_ctrl.last_shoot = now; s->fire_ctrl.last_shoot = now;
@@ -286,7 +291,7 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
s->out[SHOOT_ACTR_TRIG_IDX] = LowPassFilter2p_Apply( s->out[SHOOT_ACTR_TRIG_IDX] = LowPassFilter2p_Apply(
&(s->filter.out.trig), s->out[SHOOT_ACTR_TRIG_IDX]); &(s->filter.out.trig), s->out[SHOOT_ACTR_TRIG_IDX]);
for (uint8_t i = 0; i < 2; i++) { for (uint8_t i = 0; i < 3; i++) {
/* 控制摩擦轮 */ /* 控制摩擦轮 */
s->feedback.fric_rpm[i] = LowPassFilter2p_Apply( s->feedback.fric_rpm[i] = LowPassFilter2p_Apply(
&(s->filter.in.fric[i]), s->feedback.fric_rpm[i]); &(s->filter.in.fric[i]), s->feedback.fric_rpm[i]);

350
User/module/shoot.h
View File

@@ -2,208 +2,210 @@
* 射击模组 * 射击模组
*/ */
#pragma once #pragma once
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h> #include <cmsis_os2.h>
#include "component\cmd.h" #include "component\cmd.h"
#include "component\filter.h" #include "component\filter.h"
#include "component\pid.h" #include "component\pid.h"
#include "device\can.h" #include "device\can.h"
#include "device\referee.h" #include "device\referee.h"
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
#define SHOOT_OK (0) /* 运行正常 */ #define SHOOT_OK (0) /* 运行正常 */
#define SHOOT_ERR (-1) /* 运行时发现了其他错误 */ #define SHOOT_ERR (-1) /* 运行时发现了其他错误 */
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */ #define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ShootMode_t */ #define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ShootMode_t */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* 用enum组合所有PID，方便访问，配合数组使用 */ /* 用enum组合所有PID，方便访问，配合数组使用 */
enum Shoot_Acuator_e { enum Shoot_Acuator_e {
SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */ SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */ SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */ SHOOT_ACTR_FRIC3_IDX, /* 3号摩擦轮相关的索引值 */
SHOOT_ACTR_NUM, /* 总共的动作器数量 */ SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */
}; SHOOT_ACTR_NUM, /* 总共的动作器数量 */
};
/* 发射机构型号 */ /* 发射机构型号 */
typedef enum { typedef enum {
SHOOT_MODEL_17MM = 0, /* 17mm发射机构 */ SHOOT_MODEL_17MM = 0, /* 17mm发射机构 */
SHOOT_MODEL_42MM, /* 42mm发射机构 */ SHOOT_MODEL_42MM, /* 42mm发射机构 */
} Shoot_Model_t; } Shoot_Model_t;
/* 射击参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/ /* 射击参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
typedef struct { typedef struct {
KPID_Params_t fric_pid_param; /* 摩擦轮电机控制PID的参数 */ KPID_Params_t fric_pid_param; /* 摩擦轮电机控制PID的参数 */
KPID_Params_t trig_pid_param; /* 扳机电机控制PID的参数 */ KPID_Params_t trig_pid_param; /* 扳机电机控制PID的参数 */
/* 低通滤波器截止频率 */ /* 低通滤波器截止频率 */
struct { struct {
/* 输入 */ /* 输入 */
struct { struct {
float fric; /* 摩擦轮电机 */ float fric; /* 摩擦轮电机 */
float trig; /* 扳机电机 */ float trig; /* 扳机电机 */
} in; } in;
/* 输出 */ /* 输出 */
struct { struct {
float fric; /* 摩擦轮电机 */ float fric; /* 摩擦轮电机 */
float trig; /* 扳机电机 */ float trig; /* 扳机电机 */
} out; } out;
} low_pass_cutoff_freq; } low_pass_cutoff_freq;
float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */ float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */
float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */ float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */
float fric_radius; /* 摩擦轮半径，单位：米 */ float fric_radius; /* 摩擦轮半径，单位：米 */
float cover_open_duty; /* 弹舱盖打开时舵机PWM占空比 */ float cover_open_duty; /* 弹舱盖打开时舵机PWM占空比 */
float cover_close_duty; /* 弹舱盖关闭时舵机PWM占空比 */ float cover_close_duty; /* 弹舱盖关闭时舵机PWM占空比 */
Shoot_Model_t model; /* 发射机构型号 */ Shoot_Model_t model; /* 发射机构型号 */
float bullet_speed; /* 弹丸初速度 */ float bullet_speed; /* 弹丸初速度 */
uint32_t min_shoot_delay; /* 通过设置最小射击间隔来设置最大射频 */ uint32_t min_shoot_delay; /* 通过设置最小射击间隔来设置最大射频 */
} Shoot_Params_t; } Shoot_Params_t;
typedef struct { typedef struct {
float heat; /* 现在热量水平 */ float heat; /* 现在热量水平 */
float last_heat; /* 之前的热量水平 */ float last_heat; /* 之前的热量水平 */
float heat_limit; /* 热量上限 */ float heat_limit; /* 热量上限 */
float speed_limit; /* 弹丸初速是上限 */ float speed_limit; /* 弹丸初速是上限 */
float cooling_rate; /* 冷却速率 */ float cooling_rate; /* 冷却速率 */
float heat_increase; /* 每发热量增加值 */ float heat_increase; /* 每发热量增加值 */
float last_bullet_speed; /* 之前的弹丸速度 */ float last_bullet_speed; /* 之前的弹丸速度 */
uint32_t available_shot; /* 热量范围内还可以发射的数量 */ uint32_t available_shot; /* 热量范围内还可以发射的数量 */
} Shoot_HeatCtrl_t; } Shoot_HeatCtrl_t;
typedef struct { typedef struct {
uint32_t last_shoot; /* 上次射击时间 单位：ms */ uint32_t last_shoot; /* 上次射击时间 单位：ms */
bool last_fire; /* 上次开火状态 */ bool last_fire; /* 上次开火状态 */
bool first_fire; /* 第一次收到开火指令 */ bool first_fire; /* 第一次收到开火指令 */
uint32_t shooted; /* 已经发射的弹丸 */ uint32_t shooted; /* 已经发射的弹丸 */
uint32_t to_shoot; /* 计划发射的弹丸 */ uint32_t to_shoot; /* 计划发射的弹丸 */
float bullet_speed; /* 弹丸初速度 */ float bullet_speed; /* 弹丸初速度 */
uint32_t period_ms; /* 弹丸击发延迟 */ uint32_t period_ms; /* 弹丸击发延迟 */
CMD_FireMode_t fire_mode; CMD_FireMode_t fire_mode;
} Shoot_FireCtrl_t; } Shoot_FireCtrl_t;
/* /*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。 * 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
* 包含了初始化参数，中间变量，输出变量。 * 包含了初始化参数，中间变量，输出变量。
*/ */
typedef struct { typedef struct {
uint32_t lask_wakeup; uint32_t lask_wakeup;
float dt; float dt;
const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */ const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */
/* 模块通用 */ /* 模块通用 */
CMD_ShootMode_t mode; /* 射击模式 */ CMD_ShootMode_t mode; /* 射击模式 */
/* 反馈信息 */ /* 反馈信息 */
struct { struct {
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */ float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */ float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */
float trig_angle; /* 拨弹转盘角度，单位：弧度 */ float trig_angle; /* 拨弹转盘角度，单位：弧度 */
} feedback; } feedback;
/* PID计算的目标值 */ /* PID计算的目标值 */
struct { struct {
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */ float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
float trig_angle; /* 拨弹电机角度，单位：弧度 */ float trig_angle; /* 拨弹电机角度，单位：弧度 */
} setpoint; } setpoint;
/* 反馈控制用的PID */ /* 反馈控制用的PID */
struct { struct {
KPID_t fric[2]; /* 控制摩擦轮 */ KPID_t fric[3]; /* 控制摩擦轮 */
KPID_t trig; /* 控制拨弹电机 */ KPID_t trig; /* 控制拨弹电机 */
} pid; } pid;
/* 过滤器 */ /* 过滤器 */
struct { struct {
/* 反馈值滤波器 */ /* 反馈值滤波器 */
struct { struct {
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */ LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
LowPassFilter2p_t trig; /* 过滤拨弹电机 */ LowPassFilter2p_t trig; /* 过滤拨弹电机 */
} in; } in;
/* 输出值滤波器 */ /* 输出值滤波器 */
struct { struct {
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */ LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
LowPassFilter2p_t trig; /* 过滤拨弹电机 */ LowPassFilter2p_t trig; /* 过滤拨弹电机 */
} out; } out;
} filter; } filter;
Shoot_HeatCtrl_t heat_ctrl; Shoot_HeatCtrl_t heat_ctrl;
Shoot_FireCtrl_t fire_ctrl; Shoot_FireCtrl_t fire_ctrl;
float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */ float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */
} Shoot_t; } Shoot_t;
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
/** /**
* \brief 初始化射击 * \brief 初始化射击
* *
* \param s 包含射击数据的结构体 * \param s 包含射击数据的结构体
* \param param 包含射击参数的结构体指针 * \param param 包含射击参数的结构体指针
* \param target_freq 任务预期的运行频率 * \param target_freq 任务预期的运行频率
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq); int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq);
/** /**
* \brief 更新射击的反馈信息 * \brief 更新射击的反馈信息
* *
* \param s 包含射击数据的结构体 * \param s 包含射击数据的结构体
* \param can CAN设备结构体 * \param can CAN设备结构体
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can); int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can);
/** /**
* \brief 运行射击控制逻辑 * \brief 运行射击控制逻辑
* *
* \param s 包含射击数据的结构体 * \param s 包含射击数据的结构体
* \param s_cmd 射击控制指令 * \param s_cmd 射击控制指令
* \param s_ref 裁判系统数据 * \param s_ref 裁判系统数据
* \param dt_sec 两次调用的时间间隔 * \param dt_sec 两次调用的时间间隔
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd, int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
Referee_ForShoot_t *s_ref, uint32_t now); Referee_ForShoot_t *s_ref, uint32_t now);
/** /**
* \brief 复制射击输出值 * \brief 复制射击输出值
* *
* \param s 包含射击数据的结构体 * \param s 包含射击数据的结构体
* \param out CAN设备射击输出结构体 * \param out CAN设备射击输出结构体
*/ */
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out); void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out);
/** /**
* \brief 清空输出值 * \brief 清空输出值
* *
* \param output 要清空的结构体 * \param output 要清空的结构体
*/ */
void Shoot_ResetOutput(CAN_ShootOutput_t *output); void Shoot_ResetOutput(CAN_ShootOutput_t *output);
/** /**
* @brief 导出射击UI数据 * @brief 导出射击UI数据
* *
* @param s 射击结构体 * @param s 射击结构体
* @param ui UI结构体 * @param ui UI结构体
*/ */
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui); void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui);
#ifdef __cplusplus
}
#endif
#ifdef __cplusplus
}
#endif

15
User/task/ai.c
View File

@@ -42,7 +42,6 @@ void Task_Ai(void *argument) {
AI_Init(&ai); AI_Init(&ai);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
uint32_t last_online_tick = tick;
while (1) { while (1) {
#ifdef DEBUG #ifdef DEBUG
task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
@@ -52,23 +51,17 @@ void Task_Ai(void *argument) {
AI_StartReceiving(&ai); AI_StartReceiving(&ai);
if (AI_WaitDmaCplt()) { if (AI_WaitDmaCplt()) {
AI_ParseHost(&ai); AI_ParseHost(&ai, &cmd_host);
last_online_tick = tick;
} else { } else {
if (tick - last_online_tick > 300) AI_HandleOffline(&ai,&cmd_host); AI_HandleOffline(&ai, &cmd_host);
}
if (ai.status != AI_STATUS_STOP && ai.ai_online){
AI_PackCmd(&ai, &cmd_host);
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
} }
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0); osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0); osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai, bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
&(referee_ai), NULL, 0) == osOK); &(referee_ai), NULL, 0) == osOK);
AI_PackMCU(&ai, &quat); AI_PackMCU(&ai, &quat);
if (ref_update) AI_PackRef(&ai, &(referee_ai)); if (ref_update) AI_PackRef(&ai, &(referee_ai));