Compare commits
4 Commits
main
...
auto_sentr
| Author | SHA1 | Date | |
|---|---|---|---|
|
88a1f2bae4 | ||
|
|
180b6d728d | ||
|
|
b6e1393f4d | ||
|
|
772c44f3d9 |
18
.vscode/c_cpp_properties.json
vendored
Normal file
18
.vscode/c_cpp_properties.json
vendored
Normal file
@ -0,0 +1,18 @@
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"name": "windows-gcc-x64",
|
||||
"includePath": [
|
||||
"${workspaceFolder}/**"
|
||||
],
|
||||
"compilerPath": "gcc",
|
||||
"cStandard": "${default}",
|
||||
"cppStandard": "${default}",
|
||||
"intelliSenseMode": "windows-gcc-x64",
|
||||
"compilerArgs": [
|
||||
""
|
||||
]
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
24
.vscode/launch.json
vendored
Normal file
24
.vscode/launch.json
vendored
Normal file
@ -0,0 +1,24 @@
|
||||
{
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
{
|
||||
"name": "C/C++ Runner: Debug Session",
|
||||
"type": "cppdbg",
|
||||
"request": "launch",
|
||||
"args": [],
|
||||
"stopAtEntry": false,
|
||||
"externalConsole": true,
|
||||
"cwd": "c:/Users/lvzucheng/Documents/XRobot/User/device",
|
||||
"program": "c:/Users/lvzucheng/Documents/XRobot/User/device/build/Debug/outDebug",
|
||||
"MIMode": "gdb",
|
||||
"miDebuggerPath": "gdb",
|
||||
"setupCommands": [
|
||||
{
|
||||
"description": "Enable pretty-printing for gdb",
|
||||
"text": "-enable-pretty-printing",
|
||||
"ignoreFailures": true
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
59
.vscode/settings.json
vendored
Normal file
59
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,59 @@
|
||||
{
|
||||
"C_Cpp_Runner.cCompilerPath": "gcc",
|
||||
"C_Cpp_Runner.cppCompilerPath": "g++",
|
||||
"C_Cpp_Runner.debuggerPath": "gdb",
|
||||
"C_Cpp_Runner.cStandard": "",
|
||||
"C_Cpp_Runner.cppStandard": "",
|
||||
"C_Cpp_Runner.msvcBatchPath": "C:/Program Files/Microsoft Visual Studio/VR_NR/Community/VC/Auxiliary/Build/vcvarsall.bat",
|
||||
"C_Cpp_Runner.useMsvc": false,
|
||||
"C_Cpp_Runner.warnings": [
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Wpedantic",
|
||||
"-Wshadow",
|
||||
"-Wformat=2",
|
||||
"-Wcast-align",
|
||||
"-Wconversion",
|
||||
"-Wsign-conversion",
|
||||
"-Wnull-dereference"
|
||||
],
|
||||
"C_Cpp_Runner.msvcWarnings": [
|
||||
"/W4",
|
||||
"/permissive-",
|
||||
"/w14242",
|
||||
"/w14287",
|
||||
"/w14296",
|
||||
"/w14311",
|
||||
"/w14826",
|
||||
"/w44062",
|
||||
"/w44242",
|
||||
"/w14905",
|
||||
"/w14906",
|
||||
"/w14263",
|
||||
"/w44265",
|
||||
"/w14928"
|
||||
],
|
||||
"C_Cpp_Runner.enableWarnings": true,
|
||||
"C_Cpp_Runner.warningsAsError": false,
|
||||
"C_Cpp_Runner.compilerArgs": [],
|
||||
"C_Cpp_Runner.linkerArgs": [],
|
||||
"C_Cpp_Runner.includePaths": [],
|
||||
"C_Cpp_Runner.includeSearch": [
|
||||
"*",
|
||||
"**/*"
|
||||
],
|
||||
"C_Cpp_Runner.excludeSearch": [
|
||||
"**/build",
|
||||
"**/build/**",
|
||||
"**/.*",
|
||||
"**/.*/**",
|
||||
"**/.vscode",
|
||||
"**/.vscode/**"
|
||||
],
|
||||
"C_Cpp_Runner.useAddressSanitizer": false,
|
||||
"C_Cpp_Runner.useUndefinedSanitizer": false,
|
||||
"C_Cpp_Runner.useLeakSanitizer": false,
|
||||
"C_Cpp_Runner.showCompilationTime": false,
|
||||
"C_Cpp_Runner.useLinkTimeOptimization": false,
|
||||
"C_Cpp_Runner.msvcSecureNoWarnings": false
|
||||
}
|
||||
@ -347,7 +347,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<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 -FO15 -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 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -400,17 +400,22 @@
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd</ItemText>
|
||||
<ItemText>for_chassis</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc</ItemText>
|
||||
<ItemText>for_shoot</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_host</ItemText>
|
||||
<ItemText>cmd</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>host</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -663,7 +668,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Application/User/USB_DEVICE/App</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1219,7 +1224,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Middlewares/USB_Device_Library</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1275,7 +1280,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/bsp</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1499,7 +1504,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/component</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1675,7 +1680,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/device</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1755,7 +1760,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/module</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -2044,7 +2044,7 @@
|
||||
<uC99>0</uC99>
|
||||
<uGnu>0</uGnu>
|
||||
<useXO>0</useXO>
|
||||
<v6Lang>6</v6Lang>
|
||||
<v6Lang>5</v6Lang>
|
||||
<v6LangP>3</v6LangP>
|
||||
<vShortEn>1</vShortEn>
|
||||
<vShortWch>1</vShortWch>
|
||||
|
||||
Binary file not shown.
19242
MDK-ARM/DevC/DevC.hex
19242
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
114
Middlewares/Third_Party/Protocol/protocol.h
vendored
114
Middlewares/Third_Party/Protocol/protocol.h
vendored
@ -7,7 +7,8 @@
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#define AI_NOTICE_AUTOAIM (1 << 0)
|
||||
@ -21,67 +22,70 @@ extern "C" {
|
||||
#define AI_TEAM_RED (0x01)
|
||||
#define AI_TEAM_BLUE (0x02)
|
||||
|
||||
typedef uint8_t Protocol_ID_t;
|
||||
|
||||
typedef uint8_t Protocol_ID_t;
|
||||
/* 电控 -> 视觉 MCU数据结构体*/
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
struct __attribute__((packed))
|
||||
{
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3;
|
||||
} quat; /* 四元数 */
|
||||
struct __attribute__((packed))
|
||||
{
|
||||
float yaw;
|
||||
float pit;
|
||||
float rol;
|
||||
} gimbal; /* 欧拉角 */
|
||||
uint8_t notice; /* 控制命令 */
|
||||
} Protocol_UpDataMCU_t;
|
||||
|
||||
/* 电控 -> 视觉 MCU数据结构体*/
|
||||
typedef struct __attribute__((packed)) {
|
||||
struct __attribute__((packed)) {
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3;
|
||||
} quat; /* 四元数 */
|
||||
/* 电控 -> 视觉 裁判系统数据结构体*/
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint16_t team; /* 本身队伍 */
|
||||
uint16_t time; /* 比赛开始时间 */
|
||||
} Protocol_UpDataReferee_t;
|
||||
|
||||
uint8_t notice; /* 控制命令 */
|
||||
/* 视觉 -> 电控 数据结构体*/
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
struct __attribute__((packed))
|
||||
{
|
||||
float yaw; /* 偏航角（Yaw angle） */
|
||||
float pit; /* 俯仰角（Pitch angle） */
|
||||
float rol; /* 翻滚角（Roll angle） */
|
||||
} gimbal; /* 欧拉角 */
|
||||
|
||||
float ball_speed; /* 子弹初速度 */
|
||||
struct __attribute__((packed))
|
||||
{
|
||||
float vx; /* x轴移动速度 */
|
||||
float vy; /* y轴移动速度 */
|
||||
float wz; /* z轴转动速度 */
|
||||
} chassis_move_vec; /* 底盘移动向量 */
|
||||
uint8_t notice; /* 控制命令 */
|
||||
} Protocol_DownData_t;
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float left;
|
||||
float right;
|
||||
} distance; /* 左右距离(哨兵) */
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
Protocol_UpDataMCU_t data;
|
||||
uint16_t crc16;
|
||||
} Protocol_UpPackageMCU_t;
|
||||
|
||||
float chassis_speed; /* 底盘速度(哨兵) */
|
||||
} Protocol_UpDataMCU_t;
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
Protocol_UpDataReferee_t data;
|
||||
uint16_t crc16;
|
||||
} Protocol_UpPackageReferee_t;
|
||||
|
||||
/* 电控 -> 视觉 裁判系统数据结构体*/
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint16_t team; /* 本身队伍 */
|
||||
uint16_t time; /* 比赛开始时间 */
|
||||
} Protocol_UpDataReferee_t;
|
||||
|
||||
/* 视觉 -> 电控 数据结构体*/
|
||||
typedef struct __attribute__((packed)) {
|
||||
struct __attribute__((packed)) {
|
||||
float yaw; /* 偏航角（Yaw angle） */
|
||||
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;
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
Protocol_DownData_t data;
|
||||
uint16_t crc16;
|
||||
} Protocol_DownPackage_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -12,9 +12,9 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
|
||||
if (huart->Instance == USART3)
|
||||
return BSP_UART_DR16;
|
||||
else if (huart->Instance == USART1)
|
||||
return BSP_UART_AI;
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_REF;
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_AI;
|
||||
/*
|
||||
else if (huart->Instance == USARTX)
|
||||
return BSP_UART_XXX;
|
||||
@ -110,9 +110,9 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
|
||||
case BSP_UART_DR16:
|
||||
return &huart3;
|
||||
case BSP_UART_REF:
|
||||
return &huart6;
|
||||
case BSP_UART_AI:
|
||||
return &huart1;
|
||||
case BSP_UART_AI:
|
||||
return &huart6;
|
||||
/*
|
||||
case BSP_UART_XXX:
|
||||
return &huartX;
|
||||
|
||||
@ -6,7 +6,6 @@
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include <device\ai.h>
|
||||
/**
|
||||
* @brief 行为转换为对应按键
|
||||
*
|
||||
@ -15,12 +14,14 @@
|
||||
* @return uint16_t 行为对应的按键
|
||||
*/
|
||||
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
|
||||
CMD_Behavior_t behavior) {
|
||||
CMD_Behavior_t behavior)
|
||||
{
|
||||
return cmd->param->map.key_map[behavior].key;
|
||||
}
|
||||
|
||||
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
|
||||
CMD_Behavior_t behavior) {
|
||||
CMD_Behavior_t behavior)
|
||||
{
|
||||
return cmd->param->map.key_map[behavior].active;
|
||||
}
|
||||
|
||||
@ -33,43 +34,53 @@ static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
|
||||
* @return true 按下
|
||||
* @return false 未按下
|
||||
*/
|
||||
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
|
||||
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key)
|
||||
{
|
||||
/* 按下按键为鼠标左、右键 */
|
||||
if (key == CMD_L_CLICK) {
|
||||
if (key == CMD_L_CLICK)
|
||||
{
|
||||
return rc->mouse.l_click;
|
||||
}
|
||||
if (key == CMD_R_CLICK) {
|
||||
if (key == CMD_R_CLICK)
|
||||
{
|
||||
return rc->mouse.r_click;
|
||||
}
|
||||
return rc->key & (1u << key);
|
||||
}
|
||||
|
||||
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
|
||||
CMD_Behavior_t behavior) {
|
||||
CMD_Behavior_t behavior)
|
||||
{
|
||||
CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
|
||||
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
|
||||
|
||||
bool now_key_pressed, last_key_pressed;
|
||||
|
||||
/* 按下按键为鼠标左、右键 */
|
||||
if (key == CMD_L_CLICK) {
|
||||
if (key == CMD_L_CLICK)
|
||||
{
|
||||
now_key_pressed = rc->mouse.l_click;
|
||||
last_key_pressed = cmd->mouse_last.l_click;
|
||||
} else if (key == CMD_R_CLICK) {
|
||||
}
|
||||
else if (key == CMD_R_CLICK)
|
||||
{
|
||||
now_key_pressed = rc->mouse.r_click;
|
||||
last_key_pressed = cmd->mouse_last.r_click;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
now_key_pressed = rc->key & (1u << key);
|
||||
last_key_pressed = cmd->key_last & (1u << key);
|
||||
}
|
||||
|
||||
switch (active) {
|
||||
case CMD_ACTIVE_PRESSING:
|
||||
return now_key_pressed && !last_key_pressed;
|
||||
case CMD_ACTIVE_RASING:
|
||||
return !now_key_pressed && last_key_pressed;
|
||||
case CMD_ACTIVE_PRESSED:
|
||||
return now_key_pressed;
|
||||
switch (active)
|
||||
{
|
||||
case CMD_ACTIVE_PRESSING:
|
||||
return now_key_pressed && !last_key_pressed;
|
||||
case CMD_ACTIVE_RASING:
|
||||
return !now_key_pressed && last_key_pressed;
|
||||
case CMD_ACTIVE_PRESSED:
|
||||
return now_key_pressed;
|
||||
}
|
||||
}
|
||||
|
||||
@ -80,7 +91,8 @@ static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
|
||||
* @param cmd 主结构体
|
||||
* @param dt_sec 两次解析的间隔
|
||||
*/
|
||||
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
|
||||
/* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
|
||||
@ -92,85 +104,111 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
cmd->shoot.reverse_trig = false;
|
||||
|
||||
/* 按键行为映射相关逻辑 */
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
|
||||
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
|
||||
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE))
|
||||
{
|
||||
/* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire = true;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 切换至准备模式，停止射击 */
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire = false;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE))
|
||||
{
|
||||
/* 每按一次依次切换开火下一个模式 */
|
||||
cmd->shoot.fire_mode++;
|
||||
cmd->shoot.fire_mode %= FIRE_MODE_NUM;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR))
|
||||
{
|
||||
/* 切换到小陀螺模式 */
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER))
|
||||
{
|
||||
/* 每按一次开、关弹舱盖 */
|
||||
cmd->shoot.cover_open = !cmd->shoot.cover_open;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
|
||||
if (cmd->ai_status == AI_STATUS_HITSWITCH) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF))
|
||||
{
|
||||
if (cmd->ai_status == AI_STATUS_HITSWITCH)
|
||||
{
|
||||
/* 停止ai的打符模式，停用host控制 */
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
|
||||
cmd->host_overwrite = false;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
} else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
|
||||
}
|
||||
else if (cmd->ai_status == AI_STATUS_AUTOAIM)
|
||||
{
|
||||
/* 自瞄模式中切换失败提醒 */
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
/* ai切换至打符模式，启用host控制 */
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
|
||||
cmd->ai_status = AI_STATUS_HITSWITCH;
|
||||
cmd->host_overwrite = true;
|
||||
}
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
|
||||
if (cmd->ai_status == AI_STATUS_AUTOAIM) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM))
|
||||
{
|
||||
if (cmd->ai_status == AI_STATUS_AUTOAIM)
|
||||
{
|
||||
/* 停止ai的自瞄模式，停用host控制 */
|
||||
cmd->host_overwrite = false;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
/* ai切换至自瞄模式，启用host控制 */
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd->host_overwrite = false;
|
||||
// TODO: 修复逻辑
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG))
|
||||
{
|
||||
/* 按下拨弹反转 */
|
||||
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;
|
||||
}
|
||||
/* 保存当前按下的键位状态 */
|
||||
@ -185,82 +223,69 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
* @param cmd 主结构体
|
||||
* @param dt_sec 两次解析的间隔
|
||||
*/
|
||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
switch (rc->sw_l) {
|
||||
/* 左拨杆相应行为选择和解析 */
|
||||
case CMD_SW_UP:
|
||||
cmd->chassis.mode = CHASSIS_MODE_BREAK;
|
||||
// cmd->host_overwrite = false;
|
||||
// cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
switch (rc->sw_l)
|
||||
{
|
||||
/* 左拨杆相应行为选择和解析 */
|
||||
case CMD_SW_UP:
|
||||
cmd->chassis.mode = CHASSIS_MODE_BREAK;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
cmd->host_overwrite = false;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
// cmd->host_overwrite = true;
|
||||
// cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
|
||||
// cmd->host_overwrite = true;
|
||||
// cmd->shoot.ai_fire = true;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_ERR:
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
// cmd->host_overwrite = false;
|
||||
// cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
case CMD_SW_ERR:
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
cmd->host_overwrite = false;
|
||||
break;
|
||||
}
|
||||
switch (rc->sw_r) {
|
||||
/* 右拨杆相应行为选择和解析*/
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
switch (rc->sw_r)
|
||||
{
|
||||
/* 右拨杆相应行为选择和解析*/
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
/*
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
*/
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
// cmd->host_overwrite = false;
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
}
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
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.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
|
||||
if (cmd->ai_status == AI_STATUS_STOP || cmd->host_overwrite == false)
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
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.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -268,7 +293,8 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
*
|
||||
* @param cmd 主结构体
|
||||
*/
|
||||
static void CMD_RcLostLogic(CMD_t *cmd) {
|
||||
static void CMD_RcLostLogic(CMD_t *cmd)
|
||||
{
|
||||
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
@ -282,10 +308,13 @@ static void CMD_RcLostLogic(CMD_t *cmd) {
|
||||
* @param param 参数
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
|
||||
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param)
|
||||
{
|
||||
/* 指针检测 */
|
||||
if (cmd == NULL) return -1;
|
||||
if (param == NULL) return -1;
|
||||
if (cmd == NULL)
|
||||
return -1;
|
||||
if (param == NULL)
|
||||
return -1;
|
||||
|
||||
/* 设置机器人的命令参数，初始化控制方式为rc控制 */
|
||||
cmd->pc_ctrl = false;
|
||||
@ -311,10 +340,13 @@ inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
|
||||
* @param dt_sec 两次解析的间隔
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
/* 指针检测 */
|
||||
if (rc == NULL) return -1;
|
||||
if (cmd == NULL) return -1;
|
||||
if (rc == NULL)
|
||||
return -1;
|
||||
if (cmd == NULL)
|
||||
return -1;
|
||||
|
||||
/* 在pc控制和rc控制间切换 */
|
||||
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
|
||||
@ -325,12 +357,18 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
|
||||
cmd->pc_ctrl = false;
|
||||
/*c当rc丢控时，恢复机器人至默认状态 */
|
||||
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
|
||||
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR))
|
||||
{
|
||||
CMD_RcLostLogic(cmd);
|
||||
} else {
|
||||
if (cmd->pc_ctrl) {
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cmd->pc_ctrl)
|
||||
{
|
||||
CMD_PcLogic(rc, cmd, dt_sec);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
CMD_RcLogic(rc, cmd, dt_sec);
|
||||
}
|
||||
}
|
||||
@ -345,21 +383,27 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
* @param dt_sec 两次解析的间隔
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
|
||||
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
(void)dt_sec; /* 未使用dt_sec，消除警告 */
|
||||
/* 指针检测 */
|
||||
if (host == NULL) return -1;
|
||||
if (cmd == NULL) return -1;
|
||||
if (host == NULL)
|
||||
return -1;
|
||||
if (cmd == NULL)
|
||||
return -1;
|
||||
|
||||
/* 云台欧拉角设置为host相应的变化的欧拉角 */
|
||||
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
|
||||
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
|
||||
|
||||
/* host射击命令，设置不同的射击频率和弹丸初速度 */
|
||||
if (host->fire) {
|
||||
if (host->fire)
|
||||
{
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire = true;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
}
|
||||
return 0;
|
||||
@ -372,11 +416,14 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
|
||||
* @param cmd 要添加的命令
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
|
||||
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd)
|
||||
{
|
||||
/* 指针检测 */
|
||||
if (ref == NULL) return -1;
|
||||
if (ref == NULL)
|
||||
return -1;
|
||||
/* 越界检测 */
|
||||
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
|
||||
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0)
|
||||
return -1;
|
||||
|
||||
/* 添加机器人当前行为状态到画图的命令队列中 */
|
||||
ref->cmd[ref->counter] = cmd;
|
||||
|
||||
@ -21,9 +21,8 @@ typedef enum {
|
||||
ROBOT_MODEL_HERO, /* 英雄机器人 */
|
||||
ROBOT_MODEL_ENGINEER, /* 工程机器人 */
|
||||
ROBOT_MODEL_DRONE, /* 空中机器人 */
|
||||
ROBOT_MODEL_SENTRY, /* 哨兵机器人 */
|
||||
ROBOT_MODEL_SENTRY_GIMBAL, /* 哨兵云台 */
|
||||
ROBOT_MODEL_SENTRY_CHASSIS, /* 哨兵底盘 */
|
||||
ROBOT_MODEL_SENTRY_LEFT, /* 哨兵机器人左头 */
|
||||
ROBOT_MODEL_SENTRY_RIGHT, /* 哨兵机器人右头 */
|
||||
ROBOT_MODEL_NUM, /* 型号数量 */
|
||||
} CMD_RobotModel_t;
|
||||
|
||||
@ -84,7 +83,6 @@ typedef struct {
|
||||
CMD_ShootMode_t mode; /* 射击运行模式 */
|
||||
CMD_FireMode_t fire_mode; /* 开火模式 */
|
||||
bool fire; /*开火*/
|
||||
bool ai_fire; /* AI开火状态 */
|
||||
bool cover_open; /* 弹舱盖开关 */
|
||||
bool reverse_trig; /* 拨弹电机状态 */
|
||||
} CMD_ShootCmd_t;
|
||||
@ -213,6 +211,7 @@ typedef struct {
|
||||
} mouse_last; /* 鼠标值 */
|
||||
|
||||
CMD_AI_Status_t ai_status; /* AI状态 */
|
||||
|
||||
const CMD_Params_t *param; /* 命令参数 */
|
||||
|
||||
CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
|
||||
|
||||
@ -112,3 +112,17 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
|
||||
/* 不为裁判系统设定值时,计算转速 */
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
@ -104,3 +104,53 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#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);
|
||||
|
||||
@ -12,34 +12,46 @@
|
||||
#include "component\crc16.h"
|
||||
#include "component\crc8.h"
|
||||
#include "component\user_math.h"
|
||||
#include "component\filter.h"
|
||||
|
||||
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
|
||||
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static volatile uint32_t drop_message = 0;
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
static uint8_t rxbuf[AI_LEN_RX_BUFF];
|
||||
|
||||
static bool inited = false;
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
|
||||
static void Ai_RxCpltCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
static void Ai_IdleLineCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t AI_Init(AI_t *ai) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
UNUSED(ai);
|
||||
ASSERT(ai);
|
||||
if (inited) return DEVICE_ERR_INITED;
|
||||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||||
VERIFY((thread_alert = osThreadGetId()) != NULL);
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
|
||||
Ai_RxCpltCallback);
|
||||
inited = true;
|
||||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
|
||||
Ai_IdleLineCallback);
|
||||
|
||||
ai->ai_online = false;
|
||||
inited = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -50,9 +62,9 @@ int8_t AI_Restart(void) {
|
||||
}
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai) {
|
||||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
ai->form_host.rx_buffer,
|
||||
sizeof(ai->form_host.rx_buffer)) == HAL_OK)
|
||||
UNUSED(ai);
|
||||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
|
||||
AI_LEN_RX_BUFF) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
@ -62,27 +74,12 @@ bool AI_WaitDmaCplt(void) {
|
||||
SIGNAL_AI_RAW_REDY);
|
||||
}
|
||||
|
||||
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
(void)cmd_host;
|
||||
ai->ai_online = true;
|
||||
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
|
||||
int8_t AI_ParseHost(AI_t *ai) {
|
||||
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
|
||||
goto error;
|
||||
memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data));
|
||||
// cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
||||
// cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
||||
// 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;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.data.gimbal.pit;
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
|
||||
cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
|
||||
|
||||
ai->ai_online = true;
|
||||
memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
|
||||
memset(rxbuf, 0, AI_LEN_RX_BUFF);
|
||||
return DEVICE_OK;
|
||||
|
||||
error:
|
||||
@ -90,20 +87,31 @@ error:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
||||
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
// cmd_host->fire = true;
|
||||
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;
|
||||
}
|
||||
|
||||
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
if (cmd_host == NULL) return DEVICE_ERR_NULL;
|
||||
ai->ai_online = false;
|
||||
memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
|
||||
memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
|
||||
memset(&(ai->form_host), 0, sizeof(ai->form_host));
|
||||
memset(cmd_host, 0, sizeof(*cmd_host));
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
|
||||
ai->to_host.mcu.id = AI_ID_MCU;
|
||||
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
|
||||
sizeof(*quat));
|
||||
memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
|
||||
sizeof(*gimbal_ai));
|
||||
ai->to_host.mcu.package.data.notice = 0;
|
||||
if (ai->status == AI_STATUS_AUTOAIM)
|
||||
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
|
||||
|
||||
@ -16,6 +16,7 @@ extern "C" {
|
||||
#include "component\ahrs.h"
|
||||
#include "component\cmd.h"
|
||||
#include "component\user_math.h"
|
||||
#include "component\filter.h"
|
||||
#include "device\device.h"
|
||||
#include "device\referee.h"
|
||||
#include "protocol.h"
|
||||
@ -36,10 +37,8 @@ typedef struct __packed {
|
||||
|
||||
typedef struct __packed {
|
||||
osThreadId_t thread_alert;
|
||||
struct {
|
||||
uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)];
|
||||
Protocol_DownPackage_t data;
|
||||
}form_host;
|
||||
|
||||
Protocol_DownPackage_t form_host;
|
||||
|
||||
struct {
|
||||
AI_UpPackageReferee_t ref;
|
||||
@ -47,7 +46,6 @@ typedef struct __packed {
|
||||
} to_host;
|
||||
|
||||
CMD_AI_Status_t status;
|
||||
|
||||
bool ai_online;
|
||||
} AI_t;
|
||||
|
||||
@ -57,11 +55,12 @@ int8_t AI_Restart(void);
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai);
|
||||
bool AI_WaitDmaCplt(void);
|
||||
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
int8_t AI_ParseHost(AI_t *ai);
|
||||
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, const AHRS_Eulr_t *gimbal_ai);
|
||||
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
|
||||
int8_t AI_StartSend(AI_t *ai, bool option);
|
||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -155,7 +155,7 @@ int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
|
||||
int16_t motor1, motor2, motor3, motor4;
|
||||
int16_t yaw_motor, pit_motor;
|
||||
int16_t fric1_motor, fric2_motor, fric3_motor, trig_motor;
|
||||
int16_t fric1_motor, fric2_motor, trig_motor;
|
||||
switch (group) {
|
||||
case CAN_MOTOR_GROUT_CHASSIS:
|
||||
motor1 =
|
||||
@ -219,8 +219,6 @@ int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
(int16_t)(output->shoot.named.fric1 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
fric2_motor =
|
||||
(int16_t)(output->shoot.named.fric2 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
fric3_motor =
|
||||
(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);
|
||||
|
||||
@ -233,10 +231,10 @@ int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
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[3] = (uint8_t)(fric2_motor & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((fric3_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(fric3_motor & 0xFF);
|
||||
raw_tx.tx_data[6] = (uint8_t)((trig_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[7] = (uint8_t)(trig_motor & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((trig_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(trig_motor & 0xFF);
|
||||
raw_tx.tx_data[6] = 0;
|
||||
raw_tx.tx_data[7] = 0;
|
||||
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
@ -266,7 +264,6 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
|
||||
|
||||
case CAN_M3508_FRIC1_ID:
|
||||
case CAN_M3508_FRIC2_ID:
|
||||
case CAN_M3508_FRIC3_ID:
|
||||
case CAN_M2006_TRIG_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_M3508_FRIC1_ID;
|
||||
can->recive_flag |= 1 << (index + 6);
|
||||
|
||||
@ -22,10 +22,9 @@ extern "C" {
|
||||
#define CAN_MOTOR_GIMBAL_PIT_RECV (1 << 5)
|
||||
#define CAN_MOTOR_SHOOT_FRIC1_RECV (1 << 6)
|
||||
#define CAN_MOTOR_SHOOT_FRIC2_RECV (1 << 7)
|
||||
#define CAN_MOTOR_SHOOT_FRIC3_RECV (1 << 8)
|
||||
#define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 9)
|
||||
#define CAN_MOTOR_CAP_RECV (1 << 10)
|
||||
#define CAN_TOF_RECV (1 << 11)
|
||||
#define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 8)
|
||||
#define CAN_MOTOR_CAP_RECV (1 << 9)
|
||||
#define CAN_TOF_RECV (1 << 10)
|
||||
|
||||
#define CAN_REC_CHASSIS_FINISHED \
|
||||
(CAN_MOTOR_CHASSIS_1_RECV | CAN_MOTOR_CHASSIS_2_RECV | \
|
||||
@ -34,7 +33,7 @@ extern "C" {
|
||||
(CAN_MOTOR_GIMBAL_YAW_RECV | CAN_MOTOR_GIMBAL_PIT_RECV)
|
||||
#define CAN_REC_SHOOT_FINISHED \
|
||||
(CAN_MOTOR_SHOOT_FRIC1_RECV | CAN_MOTOR_SHOOT_FRIC2_RECV | \
|
||||
CAN_MOTOR_SHOOT_FRIC3_RECV |CAN_MOTOR_SHOOT_TRIG_RECV)
|
||||
CAN_MOTOR_SHOOT_TRIG_RECV)
|
||||
#define CAN_REC_CAP_FINISHED CAN_MOTOR_CAP_RECV
|
||||
#define CAN_REC_TOF_FINISHED CAN_TOF_RECV
|
||||
|
||||
@ -73,8 +72,7 @@ typedef enum {
|
||||
|
||||
CAN_M3508_FRIC1_ID = 0x205, /* 5 */
|
||||
CAN_M3508_FRIC2_ID = 0x206, /* 6 */
|
||||
CAN_M3508_FRIC3_ID = 0x207, /* 7 */
|
||||
CAN_M2006_TRIG_ID = 0x208, /* 8 */
|
||||
CAN_M2006_TRIG_ID = 0x207, /* 8 */
|
||||
|
||||
CAN_GM6020_YAW_ID = 0x209, /* 5 */
|
||||
CAN_GM6020_PIT_ID = 0x20A, /* 6 */
|
||||
@ -112,7 +110,6 @@ enum CAN_MotorGimbal_e {
|
||||
enum CAN_MotorShoot_e {
|
||||
CAN_MOTOR_SHOOT_FRIC1 = 0,
|
||||
CAN_MOTOR_SHOOT_FRIC2,
|
||||
CAN_MOTOR_SHOOT_FRIC3,
|
||||
CAN_MOTOR_SHOOT_TRIG,
|
||||
CAN_MOTORSHOOT_NUM,
|
||||
};
|
||||
@ -150,7 +147,6 @@ typedef union {
|
||||
struct {
|
||||
float fric1;
|
||||
float fric2;
|
||||
float fric3;
|
||||
float trig;
|
||||
} named;
|
||||
} CAN_ShootOutput_t;
|
||||
@ -204,7 +200,6 @@ typedef union {
|
||||
struct {
|
||||
CAN_MotorFeedback_t fric1;
|
||||
CAN_MotorFeedback_t fric2;
|
||||
CAN_MotorFeedback_t fric3;
|
||||
CAN_MotorFeedback_t trig;
|
||||
} named;
|
||||
} CAN_ShootMotor_t;
|
||||
|
||||
@ -149,8 +149,6 @@ static const Config_RobotParam_t param_default = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -202,11 +200,7 @@ static const Config_RobotParam_t param_default = {
|
||||
}, /* can */
|
||||
}; /* param_default */
|
||||
|
||||
#ifdef DEBUG
|
||||
Config_RobotParam_t param_hero = {
|
||||
#else
|
||||
static const Config_RobotParam_t param_hero = {
|
||||
#endif
|
||||
.model = ROBOT_MODEL_HERO,
|
||||
|
||||
.chassis = { /* 底盘模块参数 */
|
||||
@ -240,7 +234,7 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
.yaw = true,
|
||||
},
|
||||
}, /* chassis */
|
||||
|
||||
@ -289,7 +283,7 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.9685447f,
|
||||
.pitch_travel_rad = 1.07685447f,
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -297,11 +291,9 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
.pit = false,
|
||||
.yaw = true,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -335,8 +327,8 @@ static const Config_RobotParam_t param_hero = {
|
||||
.trig = -1.0f,
|
||||
},
|
||||
},
|
||||
.num_trig_tooth = 5.0f,
|
||||
.trig_gear_ratio = 19.0f,
|
||||
.num_trig_tooth = 6.0f,
|
||||
.trig_gear_ratio = 3591.0f / 187.0f,
|
||||
.fric_radius = 0.03f,
|
||||
.cover_open_duty = 0.125f,
|
||||
.cover_close_duty = 0.075f,
|
||||
@ -353,12 +345,8 @@ static const Config_RobotParam_t param_hero = {
|
||||
}, /* can */
|
||||
}; /* param_hero */
|
||||
|
||||
#ifdef DEBUG
|
||||
Config_RobotParam_t param_sentry_chassis = {
|
||||
#else
|
||||
static const Config_RobotParam_t param_sentry_chassis = {
|
||||
#endif
|
||||
.model = ROBOT_MODEL_SENTRY_CHASSIS,
|
||||
static const Config_RobotParam_t param_sentry_left = {
|
||||
.model = ROBOT_MODEL_SENTRY_LEFT,
|
||||
|
||||
.chassis = { /* 底盘模块参数 */
|
||||
.type = CHASSIS_TYPE_MECANUM,
|
||||
@ -479,7 +467,7 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.85f,
|
||||
.pitch_travel_rad = 1.05f,
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -490,198 +478,6 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = true,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
|
||||
.fric_pid_param = {
|
||||
.k = 0.001f,
|
||||
.p = 1.0f,
|
||||
.i = 0.2f,
|
||||
.d = 0.01f,
|
||||
.i_limit = 0.5f,
|
||||
.out_limit = 0.5f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
},
|
||||
.trig_pid_param = {
|
||||
.k = 12.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0450000018f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.low_pass_cutoff_freq = {
|
||||
.in = {
|
||||
.fric = -1.0f,
|
||||
.trig = -1.0f,
|
||||
},
|
||||
.out = {
|
||||
.fric = -1.0f,
|
||||
.trig = -1.0f,
|
||||
},
|
||||
},
|
||||
.num_trig_tooth = 8.0f,
|
||||
.trig_gear_ratio = 36.0f,
|
||||
.fric_radius = 0.03f,
|
||||
.cover_open_duty = 0.10f,
|
||||
.cover_close_duty = 0.050f,
|
||||
.model = SHOOT_MODEL_17MM,
|
||||
.bullet_speed = 6.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_sentry_chassis */
|
||||
|
||||
#ifdef DEBUG
|
||||
Config_RobotParam_t param_sentry_gimbal = {
|
||||
#else
|
||||
static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
#endif
|
||||
.model = ROBOT_MODEL_SENTRY_GIMBAL,
|
||||
|
||||
.chassis = { /* 底盘模块参数 */
|
||||
.type = CHASSIS_TYPE_MECANUM,
|
||||
|
||||
.motor_pid_param = {
|
||||
.k = 0.001f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
|
||||
.follow_pid_param = {
|
||||
.k = 0.5f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.in = -1.0f,
|
||||
.out = -1.0f,
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
},
|
||||
|
||||
}, /* chassis */
|
||||
|
||||
.gimbal = { /* 云台模块参数 */
|
||||
.pid = {
|
||||
{
|
||||
// /* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
// .k = 0.25f,
|
||||
// .p = 1.0f,
|
||||
// .i = 1.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
// .k = 12.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.05f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.24f,
|
||||
.p = 1.0f,
|
||||
.i = 0.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
.k = 10.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.05f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
}, {
|
||||
// /* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
// .k = 0.35f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.f,
|
||||
// .d = 0.003f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
// .k = 15.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
.k = 0.25f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
.k = 2.0f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.85f,
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
.gyro = 1000.0f,
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = true,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -722,7 +518,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
.cover_close_duty = 0.050f,
|
||||
.model = SHOOT_MODEL_17MM,
|
||||
.bullet_speed = 25.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 20.0f),
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 1.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
@ -731,14 +527,14 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
.shoot = BSP_CAN_2,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_sentry_gimbal */
|
||||
}; /* param_sentry_left */
|
||||
|
||||
/* static const Config_RobotParam_t param_xxx; */
|
||||
|
||||
static const Config_PilotCfg_t cfg_qs = {
|
||||
.param = {
|
||||
.sens_mouse = 0.06f,
|
||||
.sens_rc = 6.0f,
|
||||
.sens_rc = 10.0f,
|
||||
.map = {
|
||||
.key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
|
||||
.key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
|
||||
@ -799,11 +595,10 @@ static const Config_RobotParamMap_t robot_param_map[] = {
|
||||
{"default", ¶m_default},
|
||||
{"infantry", ¶m_default},
|
||||
{"hero", ¶m_hero},
|
||||
{"sentry_chassis", ¶m_sentry_chassis},
|
||||
{"sentry_gimbal", ¶m_sentry_gimbal},
|
||||
// {"engineer", ¶m_engineer},
|
||||
// {"drone", ¶m_drone},
|
||||
// {"sentry", ¶m_sentry},
|
||||
{"sentry_left", ¶m_sentry_left},
|
||||
// {"sentry_right" ¶m_sentry_right},
|
||||
/* {"xxx", ¶m_xxx}, */
|
||||
{NULL, NULL},
|
||||
};
|
||||
|
||||
@ -48,7 +48,7 @@ typedef struct {
|
||||
|
||||
AHRS_Eulr_t mech_zero; /* 机械零点 */
|
||||
float gimbal_limit; /* 云台pitch轴软件限位最高点 */
|
||||
|
||||
|
||||
} Config_t;
|
||||
|
||||
/* 机器人参数和对应字符串的映射 */
|
||||
|
||||
@ -36,7 +36,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
|
||||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||||
if (g->mode == GIMBAL_MODE_RELAX) {
|
||||
if (mode == GIMBAL_MODE_ABSOLUTE) {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||
} else if (mode == GIMBAL_MODE_RELATIVE) {
|
||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||
}
|
||||
@ -130,12 +130,8 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
/* yaw坐标正方向与遥控器操作逻辑相反 */
|
||||
g_cmd->delta_eulr.pit = g_cmd->delta_eulr.pit;
|
||||
if (g->param->pit_ctrl_reverse){
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
}else{
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
}
|
||||
g_cmd->delta_eulr.pit = -g_cmd->delta_eulr.pit;
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||
|
||||
/* 处理yaw控制命令 */
|
||||
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
||||
@ -144,23 +140,13 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
const float delta_max =
|
||||
CircleError(g->limit.max,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.imu.pit),
|
||||
g->feedback.eulr.encoder.pit),
|
||||
M_2PI);
|
||||
const float delta_min =
|
||||
CircleError(g->limit.min,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.imu.pit),
|
||||
g->feedback.eulr.encoder.pit),
|
||||
M_2PI);
|
||||
// const float delta_max =
|
||||
// CircleError(g->limit.max,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.encoder.pit),
|
||||
// M_2PI);
|
||||
// const float delta_min =
|
||||
// CircleError(g->limit.min,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.encoder.pit),
|
||||
// M_2PI);
|
||||
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
|
||||
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
|
||||
|
||||
@ -177,25 +163,17 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
case GIMBAL_MODE_ABSOLUTE:
|
||||
yaw_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
||||
g->feedback.eulr.imu.yaw, 0.0f, g->dt);
|
||||
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
|
||||
g->out[GIMBAL_ACTR_YAW_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||
g->feedback.gyro.z, 0.f, g->dt);
|
||||
|
||||
pit_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||
// pit_omega_set_point =
|
||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
// g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||
|
||||
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
g->feedback.gyro.x, 0.f, g->dt);
|
||||
|
||||
// g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
// 0.0, 0.f, g->dt);
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_RELATIVE:
|
||||
@ -223,6 +201,20 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 打包云台数据给AI
|
||||
*
|
||||
* \param g 包含云台数据的结构体
|
||||
* \param gimbal_for_ai 云台AI数据
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai){
|
||||
memcpy((void *)ai, (const void *)&(g->feedback.eulr.encoder),
|
||||
sizeof(g->feedback.eulr.encoder));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 复制云台输出值
|
||||
*
|
||||
|
||||
@ -60,8 +60,6 @@ typedef struct {
|
||||
bool pit;
|
||||
} reverse;
|
||||
|
||||
bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
|
||||
|
||||
} Gimbal_Params_t;
|
||||
|
||||
/* 软件限位 */
|
||||
@ -147,6 +145,16 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
|
||||
*/
|
||||
int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
|
||||
|
||||
/**
|
||||
* \brief 打包云台数据给AI
|
||||
*
|
||||
* \param g 包含云台数据的结构体
|
||||
* \param gimbal_for_ai 云台AI数据
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai);
|
||||
|
||||
/**
|
||||
* \brief 复制云台输出值
|
||||
*
|
||||
|
||||
@ -35,7 +35,7 @@ static int8_t Shoot_SetMode(Shoot_t *s, CMD_ShootMode_t mode) {
|
||||
if (mode == s->mode) return SHOOT_OK;
|
||||
|
||||
/* 切换模式后重置PID和滤波器 */
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
PID_Reset(s->pid.fric + i);
|
||||
LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f);
|
||||
LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f);
|
||||
@ -117,7 +117,7 @@ int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq) {
|
||||
s->param = param; /* 初始化参数 */
|
||||
s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
/* PI控制器初始化PID */
|
||||
PID_Init(s->pid.fric + i, KPID_MODE_NO_D, target_freq,
|
||||
&(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 (can == NULL) return -1;
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
s->feedback.fric_rpm[i] = can->motor.shoot.as_array[i].rotor_speed;
|
||||
}
|
||||
|
||||
@ -245,16 +245,11 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
}
|
||||
|
||||
/* 计算摩擦轮转速的目标值 */
|
||||
// s->setpoint.fric_rpm[1] =
|
||||
// CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
|
||||
// (s->param->model == SHOOT_MODEL_17MM));
|
||||
// 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;
|
||||
s->setpoint.fric_rpm[1] =
|
||||
CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
|
||||
(s->param->model == SHOOT_MODEL_17MM));
|
||||
s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[1];
|
||||
s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
|
||||
|
||||
/* 计算拨弹电机位置的目标值 */
|
||||
if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&
|
||||
@ -291,7 +286,7 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
s->out[SHOOT_ACTR_TRIG_IDX] = LowPassFilter2p_Apply(
|
||||
&(s->filter.out.trig), s->out[SHOOT_ACTR_TRIG_IDX]);
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
/* 控制摩擦轮 */
|
||||
s->feedback.fric_rpm[i] = LowPassFilter2p_Apply(
|
||||
&(s->filter.in.fric[i]), s->feedback.fric_rpm[i]);
|
||||
|
||||
@ -29,7 +29,6 @@ extern "C" {
|
||||
enum Shoot_Acuator_e {
|
||||
SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC3_IDX, /* 3号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */
|
||||
SHOOT_ACTR_NUM, /* 总共的动作器数量 */
|
||||
};
|
||||
@ -107,20 +106,20 @@ typedef struct {
|
||||
|
||||
/* 反馈信息 */
|
||||
struct {
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
float trig_angle; /* 拨弹转盘角度，单位：弧度 */
|
||||
} feedback;
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
} setpoint;
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t fric[3]; /* 控制摩擦轮 */
|
||||
KPID_t fric[2]; /* 控制摩擦轮 */
|
||||
KPID_t trig; /* 控制拨弹电机 */
|
||||
} pid;
|
||||
|
||||
@ -128,13 +127,13 @@ typedef struct {
|
||||
struct {
|
||||
/* 反馈值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} in;
|
||||
|
||||
/* 输出值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} out;
|
||||
} filter;
|
||||
|
||||
@ -17,11 +17,13 @@ AI_t ai;
|
||||
CMD_Host_t cmd_host;
|
||||
AHRS_Quaternion_t quat;
|
||||
Referee_ForAI_t referee_ai;
|
||||
AHRS_Eulr_t gimbal_ai;
|
||||
#else
|
||||
static AI_t ai;
|
||||
static CMD_Host_t cmd_host;
|
||||
static AHRS_Quaternion_t quat;
|
||||
static Referee_ForAI_t referee_ai;
|
||||
static AHRS_Eulr_t gimbal_ai;
|
||||
#endif
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -42,9 +44,7 @@ void Task_Ai(void *argument) {
|
||||
AI_Init(&ai);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
uint32_t last_online_tick = tick;
|
||||
|
||||
while (1) {
|
||||
#ifdef DEBUG
|
||||
task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
|
||||
@ -54,19 +54,25 @@ void Task_Ai(void *argument) {
|
||||
|
||||
AI_StartReceiving(&ai);
|
||||
if (AI_WaitDmaCplt()) {
|
||||
AI_ParseHost(&ai, &cmd_host);
|
||||
AI_ParseHost(&ai);
|
||||
last_online_tick = tick;
|
||||
} else {
|
||||
if (tick - last_online_tick > 300) AI_HandleOffline(&ai, &cmd_host);
|
||||
if (tick - last_online_tick > 300) 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.cmd.ai, &(ai.status), NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), NULL, 0);
|
||||
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
|
||||
&(referee_ai), NULL, 0) == osOK);
|
||||
AI_PackMCU(&ai, &quat);
|
||||
|
||||
AI_PackMCU(&ai, &quat, &gimbal_ai);
|
||||
if (ref_update) AI_PackRef(&ai, &(referee_ai));
|
||||
|
||||
AI_StartSend(&(ai), ref_update);
|
||||
|
||||
@ -32,10 +32,10 @@ typedef struct {
|
||||
|
||||
static const char *const CLI_WELCOME_MESSAGE =
|
||||
"\r\n"
|
||||
" ______ __ _______ __ \r\n"
|
||||
" | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||
" | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||
" |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||
// " ______ __ _______ __ \r\n"
|
||||
// " | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||
// " | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||
// " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||
" -------------------------------------------------------------------\r\n"
|
||||
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
|
||||
"\r\n";
|
||||
|
||||
@ -68,7 +68,6 @@ void Task_Command(void *argument) {
|
||||
if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
|
||||
osOK)
|
||||
CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
|
||||
|
||||
osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
|
||||
|
||||
/* 将需要与其他任务分享的数据放到消息队列中 */
|
||||
|
||||
@ -22,11 +22,13 @@ CMD_GimbalCmd_t gimbal_cmd;
|
||||
Gimbal_t gimbal;
|
||||
CAN_GimbalOutput_t gimbal_out;
|
||||
Referee_GimbalUI_t gimbal_ui;
|
||||
AHRS_Eulr_t gimbal_for_ai;
|
||||
#else
|
||||
static CMD_GimbalCmd_t gimbal_cmd;
|
||||
static Gimbal_t gimbal;
|
||||
static CAN_GimbalOutput_t gimbal_out;
|
||||
static Referee_GimbalUI_t gimbal_ui;
|
||||
static AHRS_Eulr_t gimbal_for_ai;
|
||||
#endif
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {
|
||||
|
||||
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
|
||||
Gimbal_UpdateFeedback(&gimbal, &can);
|
||||
Gimbal_PackAI(&gimbal, &gimbal_for_ai);
|
||||
Gimbal_Control(&gimbal, &gimbal_cmd, tick);
|
||||
Gimbal_DumpOutput(&gimbal, &gimbal_out);
|
||||
|
||||
osKernelUnlock();
|
||||
osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
|
||||
osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_encoder);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_encoder, &gimbal_for_ai, 0, 0);
|
||||
|
||||
Gimbal_DumpUI(&gimbal, &gimbal_ui);
|
||||
osMessageQueueReset(task_runtime.msgq.ui.gimbal);
|
||||
|
||||
176
User/task/init.c
176
User/task/init.c
@ -30,101 +30,103 @@
|
||||
*
|
||||
* \param argument 未使用
|
||||
*/
|
||||
void Task_Init(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
void Task_Init(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
|
||||
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
|
||||
|
||||
osKernelLock();
|
||||
/* 创建任务 */
|
||||
task_runtime.thread.atti_esti =
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
|
||||
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
|
||||
task_runtime.thread.ctrl_chassis =
|
||||
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal =
|
||||
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.ctrl_shoot =
|
||||
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
|
||||
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
|
||||
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
|
||||
osKernelLock();
|
||||
/* 创建任务 */
|
||||
task_runtime.thread.atti_esti =
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
|
||||
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
|
||||
task_runtime.thread.ctrl_chassis =
|
||||
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal =
|
||||
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.ctrl_shoot =
|
||||
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
|
||||
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
|
||||
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
|
||||
|
||||
/* 创建消息队列 */
|
||||
/* motor */
|
||||
task_runtime.msgq.can.feedback.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.output.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
|
||||
/* 创建消息队列 */
|
||||
/* motor */
|
||||
task_runtime.msgq.can.feedback.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.output.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
|
||||
|
||||
/* command */
|
||||
task_runtime.msgq.cmd.chassis =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.gimbal =
|
||||
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.shoot =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.ai =
|
||||
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
|
||||
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
|
||||
/* command */
|
||||
task_runtime.msgq.cmd.chassis =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.gimbal =
|
||||
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.shoot =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.ai =
|
||||
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
|
||||
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
|
||||
|
||||
/* atti_esti */
|
||||
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.host =
|
||||
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
|
||||
/* atti_esti */
|
||||
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.host =
|
||||
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
|
||||
|
||||
task_runtime.msgq.gimbal.accl =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_imu =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.gimbal.gyro =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.gimbal.accl =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_imu =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.gimbal.gyro =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_encoder =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.cap_info =
|
||||
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
|
||||
|
||||
task_runtime.msgq.cap_info =
|
||||
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
|
||||
/* AI */
|
||||
task_runtime.msgq.ai.quat =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
|
||||
/* AI */
|
||||
task_runtime.msgq.ai.quat =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.ai =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
|
||||
task_runtime.msgq.referee.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
task_runtime.msgq.referee.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
|
||||
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.ai =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
|
||||
task_runtime.msgq.referee.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
task_runtime.msgq.referee.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
|
||||
/* UI */
|
||||
task_runtime.msgq.ui.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.ui.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.ui.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.ui.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
|
||||
/* UI */
|
||||
task_runtime.msgq.ui.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.ui.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.ui.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.ui.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
}
|
||||
|
||||
@ -58,6 +58,7 @@ typedef struct {
|
||||
osMessageQueueId_t accl; /* IMU读取 */
|
||||
osMessageQueueId_t gyro; /* IMU读取 */
|
||||
osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
|
||||
osMessageQueueId_t eulr_encoder; /* 编码器读取 */
|
||||
} gimbal;
|
||||
|
||||
/* 控制指令 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user