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

Compare commits

merge into: robofish:main
Branches Tags
robofish:main
robofish:原版
robofish:auto_sentry
robofish:哨兵
robofish:哨兵底盘
...
pull from: robofish:auto_sentry
Branches Tags
robofish:原版
robofish:auto_sentry
robofish:哨兵
robofish:main
robofish:哨兵底盘

4 Commits
main ... auto_sentr

Author SHA1 Message Date
RB
88a1f2bae4 左头自瞄差不多了 2025-03-05 15:02:44 +08:00
RB
180b6d728d 添加云台数据 2025-03-05 10:29:39 +08:00
RB
b6e1393f4d 上位机通讯正常 2025-03-04 17:26:45 +08:00
RB
772c44f3d9 提交一下 2025-03-04 17:20:57 +08:00
24 changed files with 10520 additions and 7233 deletions
Show Stats Expand all files Collapse all files

18
.vscode/c_cpp_properties.json vendored Normal file
View 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
View 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
View 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
}

34
MDK-ARM/DevC.uvoptx
View File

@ -10,7 +10,7 @@
<aExt>*.s*; *.src; *.a*</aExt> <aExt>*.s*; *.src; *.a*</aExt>
<oExt>*.obj; *.o</oExt> <oExt>*.obj; *.o</oExt>
<lExt>*.lib</lExt> <lExt>*.lib</lExt>
<tExt>*.txt; *.h; *.inc</tExt> <tExt>*.txt; *.h; *.inc; *.md</tExt>
<pExt>*.plm</pExt> <pExt>*.plm</pExt>
<CppX>*.cpp</CppX> <CppX>*.cpp</CppX>
<nMigrate>0</nMigrate> <nMigrate>0</nMigrate>
@ -387,6 +387,36 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>ref</ItemText> <ItemText>ref</ItemText>
</Ww> </Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>shoot</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>ai</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>for_chassis</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>for_shoot</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>cmd</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>host</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -1786,7 +1816,7 @@
<GroupNumber>12</GroupNumber> <GroupNumber>12</GroupNumber>
<FileNumber>105</FileNumber> <FileNumber>105</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\User\module\shoot.c</PathWithFileName> <PathWithFileName>..\User\module\shoot.c</PathWithFileName>

13
MDK-ARM/DevC.uvprojx
View File

@ -16,7 +16,7 @@
<TargetCommonOption> <TargetCommonOption>
<Device>STM32F407IGHx</Device> <Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor> <Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.2.14.0</PackID> <PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
<PackURL>http://www.keil.com/pack/</PackURL> <PackURL>http://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>
@ -1726,13 +1726,13 @@
<TargetName>Debug</TargetName> <TargetName>Debug</TargetName>
<ToolsetNumber>0x4</ToolsetNumber> <ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName> <ToolsetName>ARM-ADS</ToolsetName>
<pCCUsed>6140000::V6.14::ARMCLANG</pCCUsed> <pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
<uAC6>1</uAC6> <uAC6>1</uAC6>
<TargetOption> <TargetOption>
<TargetCommonOption> <TargetCommonOption>
<Device>STM32F407IGHx</Device> <Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor> <Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.2.14.0</PackID> <PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
<PackURL>http://www.keil.com/pack/</PackURL> <PackURL>http://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>
@ -2044,7 +2044,7 @@
<uC99>0</uC99> <uC99>0</uC99>
<uGnu>0</uGnu> <uGnu>0</uGnu>
<useXO>0</useXO> <useXO>0</useXO>
<v6Lang>6</v6Lang> <v6Lang>5</v6Lang>
<v6LangP>3</v6LangP> <v6LangP>3</v6LangP>
<vShortEn>1</vShortEn> <vShortEn>1</vShortEn>
<vShortWch>1</vShortWch> <vShortWch>1</vShortWch>
@ -3443,11 +3443,6 @@
<Layers> <Layers>
<Layer> <Layer>
<LayName>DevC</LayName> <LayName>DevC</LayName>
<LayDesc></LayDesc>
<LayUrl></LayUrl>
<LayKeys></LayKeys>
<LayCat></LayCat>
<LayLic></LayLic>
<LayTarg>0</LayTarg> <LayTarg>0</LayTarg>
<LayPrjMark>1</LayPrjMark> <LayPrjMark>1</LayPrjMark>
</Layer> </Layer>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

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

@ -7,7 +7,8 @@
#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)
@ -21,66 +22,70 @@ 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)) { {
float q0; struct __attribute__((packed))
float q1; {
float q2; float q0;
float q3; float q1;
} quat; /* 四元数 */ float q2;
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 right; float vx; /* x轴移动速度 */
} distance; /* 左右距离(哨兵) */ float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
uint8_t notice; /* 控制命令 */
} Protocol_DownData_t;
float chassis_speed; /* 底盘速度(哨兵) */ typedef struct __attribute__((packed))
} Protocol_UpDataMCU_t; {
Protocol_UpDataMCU_t data;
uint16_t crc16;
} Protocol_UpPackageMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/ typedef struct __attribute__((packed))
typedef struct __attribute__((packed)) { {
uint16_t team; /* 本身队伍 */ Protocol_UpDataReferee_t data;
uint16_t time; /* 比赛开始时间 */ uint16_t crc16;
} Protocol_UpDataReferee_t; } Protocol_UpPackageReferee_t;
/* 视觉 -> 电控 数据结构体*/ typedef struct __attribute__((packed))
typedef struct __attribute__((packed)) { {
struct __attribute__((packed)) { Protocol_DownData_t data;
float yaw; /* 偏航角（Yaw angle） */ uint16_t crc16;
float pit; /* 俯仰角（Pitch angle） */ } Protocol_DownPackage_t;
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
} }

287
User/component/cmd.c
View File

@ -14,12 +14,14 @@
* @return uint16_t 行为对应的按键 * @return uint16_t 行为对应的按键
*/ */
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd, 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; return cmd->param->map.key_map[behavior].key;
} }
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd, 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; return cmd->param->map.key_map[behavior].active;
} }
@ -32,43 +34,53 @@ static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
* @return true 按下 * @return true 按下
* @return false 未按下 * @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; return rc->mouse.l_click;
} }
if (key == CMD_R_CLICK) { if (key == CMD_R_CLICK)
{
return rc->mouse.r_click; return rc->mouse.r_click;
} }
return rc->key & (1u << key); return rc->key & (1u << key);
} }
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd, 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_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior); CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
bool now_key_pressed, last_key_pressed; bool now_key_pressed, last_key_pressed;
/* 按下按键为鼠标左、右键 */ /* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) { if (key == CMD_L_CLICK)
{
now_key_pressed = rc->mouse.l_click; now_key_pressed = rc->mouse.l_click;
last_key_pressed = cmd->mouse_last.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; now_key_pressed = rc->mouse.r_click;
last_key_pressed = cmd->mouse_last.r_click; last_key_pressed = cmd->mouse_last.r_click;
} else { }
else
{
now_key_pressed = rc->key & (1u << key); now_key_pressed = rc->key & (1u << key);
last_key_pressed = cmd->key_last & (1u << key); last_key_pressed = cmd->key_last & (1u << key);
} }
switch (active) { switch (active)
case CMD_ACTIVE_PRESSING: {
return now_key_pressed && !last_key_pressed; case CMD_ACTIVE_PRESSING:
case CMD_ACTIVE_RASING: return now_key_pressed && !last_key_pressed;
return !now_key_pressed && last_key_pressed; case CMD_ACTIVE_RASING:
case CMD_ACTIVE_PRESSED: return !now_key_pressed && last_key_pressed;
return now_key_pressed; case CMD_ACTIVE_PRESSED:
return now_key_pressed;
} }
} }
@ -79,7 +91,8 @@ static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
* @param cmd 主结构体 * @param cmd 主结构体
* @param dt_sec 两次解析的间隔 * @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; cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
/* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */ /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
@ -91,85 +104,111 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->shoot.reverse_trig = false; 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; 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; 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; 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; 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.vx *= cmd->param->move.move_fast_sense;
cmd->chassis.ctrl_vec.vy *= 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.vx *= cmd->param->move.move_slow_sense;
cmd->chassis.ctrl_vec.vy *= 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.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true; cmd->shoot.fire = true;
} else { }
else
{
/* 切换至准备模式，停止射击 */ /* 切换至准备模式，停止射击 */
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = false; 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++;
cmd->shoot.fire_mode %= FIRE_MODE_NUM; 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 = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_RAND; 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; cmd->shoot.cover_open = !cmd->shoot.cover_open;
} }
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) { if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF))
if (cmd->ai_status == AI_STATUS_HITSWITCH) { {
if (cmd->ai_status == AI_STATUS_HITSWITCH)
{
/* 停止ai的打符模式，停用host控制 */ /* 停止ai的打符模式，停用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP); CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
cmd->host_overwrite = false; cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP; 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控制 */ /* ai切换至打符模式，启用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START); CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
cmd->ai_status = AI_STATUS_HITSWITCH; cmd->ai_status = AI_STATUS_HITSWITCH;
cmd->host_overwrite = true; cmd->host_overwrite = true;
} }
} }
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) { if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM))
if (cmd->ai_status == AI_STATUS_AUTOAIM) { {
if (cmd->ai_status == AI_STATUS_AUTOAIM)
{
/* 停止ai的自瞄模式，停用host控制 */ /* 停止ai的自瞄模式，停用host控制 */
cmd->host_overwrite = false; cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP; cmd->ai_status = AI_STATUS_STOP;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP); CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
} else { }
else
{
/* ai切换至自瞄模式，启用host控制 */ /* ai切换至自瞄模式，启用host控制 */
cmd->ai_status = AI_STATUS_AUTOAIM; cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true; cmd->host_overwrite = true;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START); CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
} }
} else { }
else
{
cmd->host_overwrite = false; cmd->host_overwrite = false;
// TODO: 修复逻辑 // TODO: 修复逻辑
} }
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) { if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG))
{
/* 按下拨弹反转 */ /* 按下拨弹反转 */
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; cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
} }
/* 保存当前按下的键位状态 */ /* 保存当前按下的键位状态 */
@ -184,73 +223,69 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
* @param cmd 主结构体 * @param cmd 主结构体
* @param dt_sec 两次解析的间隔 * @param dt_sec 两次解析的间隔
*/ */
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
switch (rc->sw_l) { {
/* 左拨杆相应行为选择和解析 */ switch (rc->sw_l)
case CMD_SW_UP: {
cmd->chassis.mode = CHASSIS_MODE_BREAK; /* 左拨杆相应行为选择和解析 */
break; 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: case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break; cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
break;
case CMD_SW_DOWN: case CMD_SW_DOWN:
cmd->chassis.mode = CHASSIS_MODE_ROTOR; cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_CW; cmd->chassis.mode_rotor = ROTOR_MODE_CW;
break; cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
break;
case CMD_SW_ERR: case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX; cmd->chassis.mode = CHASSIS_MODE_RELAX;
break; cmd->ai_status = AI_STATUS_STOP;
cmd->host_overwrite = false;
break;
} }
switch (rc->sw_r) { switch (rc->sw_r)
/* 右拨杆相应行为选择和解析*/ {
case CMD_SW_UP: /* 右拨杆相应行为选择和解析*/
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; case CMD_SW_UP:
cmd->shoot.mode = SHOOT_MODE_SAFE; cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
break; cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID: case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.fire = false; cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->shoot.mode = SHOOT_MODE_LOADED;
break; break;
case CMD_SW_DOWN: case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE; cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true; cmd->shoot.fire = true;
break; break;
/*
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID: case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX; cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.fire = false; cmd->shoot.mode = SHOOT_MODE_RELAX;
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->chassis.ctrl_vec.vx = rc->ch_l_x; if (cmd->ai_status == AI_STATUS_STOP || cmd->host_overwrite == false)
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->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc; 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;
}
} }
/** /**
@ -258,7 +293,8 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
* *
* @param cmd 主结构体 * @param cmd 主结构体
*/ */
static void CMD_RcLostLogic(CMD_t *cmd) { static void CMD_RcLostLogic(CMD_t *cmd)
{
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */ /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
cmd->chassis.mode = CHASSIS_MODE_RELAX; cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->gimbal.mode = GIMBAL_MODE_RELAX; cmd->gimbal.mode = GIMBAL_MODE_RELAX;
@ -272,10 +308,13 @@ static void CMD_RcLostLogic(CMD_t *cmd) {
* @param param 参数 * @param param 参数
* @return int8_t 0对应没有错误 * @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 (cmd == NULL)
if (param == NULL) return -1; return -1;
if (param == NULL)
return -1;
/* 设置机器人的命令参数，初始化控制方式为rc控制 */ /* 设置机器人的命令参数，初始化控制方式为rc控制 */
cmd->pc_ctrl = false; cmd->pc_ctrl = false;
@ -301,10 +340,13 @@ inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
* @param dt_sec 两次解析的间隔 * @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误 * @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 (rc == NULL)
if (cmd == NULL) return -1; return -1;
if (cmd == NULL)
return -1;
/* 在pc控制和rc控制间切换 */ /* 在pc控制和rc控制间切换 */
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
@ -315,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_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
cmd->pc_ctrl = false; cmd->pc_ctrl = false;
/*c当rc丢控时，恢复机器人至默认状态 */ /*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); CMD_RcLostLogic(cmd);
} else { }
if (cmd->pc_ctrl) { else
{
if (cmd->pc_ctrl)
{
CMD_PcLogic(rc, cmd, dt_sec); CMD_PcLogic(rc, cmd, dt_sec);
} else { }
else
{
CMD_RcLogic(rc, cmd, dt_sec); CMD_RcLogic(rc, cmd, dt_sec);
} }
} }
@ -335,21 +383,27 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
* @param dt_sec 两次解析的间隔 * @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误 * @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，消除警告 */ (void)dt_sec; /* 未使用dt_sec，消除警告 */
/* 指针检测 */ /* 指针检测 */
if (host == NULL) return -1; if (host == NULL)
if (cmd == NULL) return -1; return -1;
if (cmd == NULL)
return -1;
/* 云台欧拉角设置为host相应的变化的欧拉角 */ /* 云台欧拉角设置为host相应的变化的欧拉角 */
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw; cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit; cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
/* host射击命令，设置不同的射击频率和弹丸初速度 */ /* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire) { if (host->fire)
{
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true; cmd->shoot.fire = true;
} else { }
else
{
cmd->shoot.mode = SHOOT_MODE_SAFE; cmd->shoot.mode = SHOOT_MODE_SAFE;
} }
return 0; return 0;
@ -362,11 +416,14 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
* @param cmd 要添加的命令 * @param cmd 要添加的命令
* @return int8_t 0对应没有错误 * @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; ref->cmd[ref->counter] = cmd;

3
User/component/cmd.h
View File

@ -21,7 +21,8 @@ typedef enum {
ROBOT_MODEL_HERO, /* 英雄机器人 */ ROBOT_MODEL_HERO, /* 英雄机器人 */
ROBOT_MODEL_ENGINEER, /* 工程机器人 */ ROBOT_MODEL_ENGINEER, /* 工程机器人 */
ROBOT_MODEL_DRONE, /* 空中机器人 */ ROBOT_MODEL_DRONE, /* 空中机器人 */
ROBOT_MODEL_SENTRY, /* 哨兵机器人 */ ROBOT_MODEL_SENTRY_LEFT, /* 哨兵机器人左头 */
ROBOT_MODEL_SENTRY_RIGHT, /* 哨兵机器人右头 */
ROBOT_MODEL_NUM, /* 型号数量 */ ROBOT_MODEL_NUM, /* 型号数量 */
} CMD_RobotModel_t; } CMD_RobotModel_t;

14
User/component/user_math.c
View File

@ -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); 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();
}
}

50
User/component/user_math.h
View File

@ -104,3 +104,53 @@ 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

@ -12,31 +12,45 @@
#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 osThreadId_t thread_alert; static uint8_t rxbuf[AI_LEN_RX_BUFF];
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) {
if (ai == NULL) return DEVICE_ERR_NULL; UNUSED(ai);
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED; 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, 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;
} }
@ -48,9 +62,9 @@ int8_t AI_Restart(void) {
} }
int8_t AI_StartReceiving(AI_t *ai) { int8_t AI_StartReceiving(AI_t *ai) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), UNUSED(ai);
(uint8_t *)&(ai->form_host), if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
sizeof(ai->form_host)) == HAL_OK) AI_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
@ -60,17 +74,12 @@ bool AI_WaitDmaCplt(void) {
SIGNAL_AI_RAW_REDY); SIGNAL_AI_RAW_REDY);
} }
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) { int8_t AI_ParseHost(AI_t *ai) {
(void)cmd_host; if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
goto error; goto error;
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit; ai->ai_online = true;
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw; memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol; memset(rxbuf, 0, AI_LEN_RX_BUFF);
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:
@ -78,19 +87,31 @@ error:
return DEVICE_ERR; 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) { 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->form_host), 0, sizeof(ai->form_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;
} }
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; ai->to_host.mcu.id = AI_ID_MCU;
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat, memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
sizeof(*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; ai->to_host.mcu.package.data.notice = 0;
if (ai->status == AI_STATUS_AUTOAIM) if (ai->status == AI_STATUS_AUTOAIM)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM; ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;

7
User/device/ai.h
View File

@ -16,6 +16,7 @@ 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"
@ -45,6 +46,7 @@ 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 -------------------------------------------- */
@ -53,11 +55,12 @@ 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, 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_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_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

2
User/device/can.h
View File

@ -72,7 +72,7 @@ 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_M2006_TRIG_ID = 0x207, /* 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 */

189
User/module/config.c
View File

@ -345,12 +345,196 @@ static const Config_RobotParam_t param_hero = {
}, /* can */ }, /* can */
}; /* param_hero */ }; /* param_hero */
static const Config_RobotParam_t param_sentry_left = {
.model = ROBOT_MODEL_SENTRY_LEFT,
.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 = 1.05f,
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
},
.reverse = {
.yaw = false,
.pit = 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 = 25.f,
.min_shoot_delay = (uint32_t)(1000.0f / 1.0f),
}, /* shoot */
.can = {
.chassis = BSP_CAN_1,
.gimbal = BSP_CAN_2,
.shoot = BSP_CAN_2,
.cap = BSP_CAN_1,
}, /* can */
}; /* param_sentry_left */
/* static const Config_RobotParam_t param_xxx; */ /* static const Config_RobotParam_t param_xxx; */
static const Config_PilotCfg_t cfg_qs = { static const Config_PilotCfg_t cfg_qs = {
.param = { .param = {
.sens_mouse = 0.06f, .sens_mouse = 0.06f,
.sens_rc = 6.0f, .sens_rc = 10.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},
@ -413,7 +597,8 @@ static const Config_RobotParamMap_t robot_param_map[] = {
{"hero", &param_hero}, {"hero", &param_hero},
// {"engineer", &param_engineer}, // {"engineer", &param_engineer},
// {"drone", &param_drone}, // {"drone", &param_drone},
// {"sentry", &param_sentry}, {"sentry_left", &param_sentry_left},
// {"sentry_right" &param_sentry_right},
/* {"xxx", &param_xxx}, */ /* {"xxx", &param_xxx}, */
{NULL, NULL}, {NULL, NULL},
}; };

24
User/module/gimbal.c
View File

@ -36,7 +36,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
if (g->mode == GIMBAL_MODE_RELAX) { if (g->mode == GIMBAL_MODE_RELAX) {
if (mode == GIMBAL_MODE_ABSOLUTE) { 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) { } else if (mode == GIMBAL_MODE_RELATIVE) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw; g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
} }
@ -140,12 +140,12 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
const float delta_max = const float delta_max =
CircleError(g->limit.max, CircleError(g->limit.max,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit - (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit), g->feedback.eulr.encoder.pit),
M_2PI); M_2PI);
const float delta_min = const float delta_min =
CircleError(g->limit.min, CircleError(g->limit.min,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit - (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit), g->feedback.eulr.encoder.pit),
M_2PI); M_2PI);
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max); Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit; g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
@ -163,14 +163,14 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
case GIMBAL_MODE_ABSOLUTE: case GIMBAL_MODE_ABSOLUTE:
yaw_omega_set_point = yaw_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw, 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] = g->out[GIMBAL_ACTR_YAW_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point, PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
g->feedback.gyro.z, 0.f, g->dt); g->feedback.gyro.z, 0.f, g->dt);
pit_omega_set_point = pit_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit, PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
g->feedback.eulr.imu.pit, 0.0f, g->dt); g->feedback.eulr.encoder.pit, 0.0f, g->dt);
g->out[GIMBAL_ACTR_PIT_IDX] = g->out[GIMBAL_ACTR_PIT_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point, PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
g->feedback.gyro.x, 0.f, g->dt); g->feedback.gyro.x, 0.f, g->dt);
@ -201,6 +201,20 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
return 0; 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 复制云台输出值 * \brief 复制云台输出值
* *

10
User/module/gimbal.h
View File

@ -145,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); 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 复制云台输出值 * \brief 复制云台输出值
* *

5
User/module/shoot.c
View File

@ -248,6 +248,7 @@ 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[1] = -s->setpoint.fric_rpm[1];
s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1]; s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
/* 计算拨弹电机位置的目标值 */ /* 计算拨弹电机位置的目标值 */
@ -255,10 +256,10 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
(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;

20
User/task/ai.c
View File

@ -17,11 +17,13 @@ AI_t ai;
CMD_Host_t cmd_host; CMD_Host_t cmd_host;
AHRS_Quaternion_t quat; AHRS_Quaternion_t quat;
Referee_ForAI_t referee_ai; Referee_ForAI_t referee_ai;
AHRS_Eulr_t gimbal_ai;
#else #else
static AI_t ai; static AI_t ai;
static CMD_Host_t cmd_host; static CMD_Host_t cmd_host;
static AHRS_Quaternion_t quat; static AHRS_Quaternion_t quat;
static Referee_ForAI_t referee_ai; static Referee_ForAI_t referee_ai;
static AHRS_Eulr_t gimbal_ai;
#endif #endif
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -42,6 +44,7 @@ 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());
@ -51,18 +54,25 @@ void Task_Ai(void *argument) {
AI_StartReceiving(&ai); AI_StartReceiving(&ai);
if (AI_WaitDmaCplt()) { if (AI_WaitDmaCplt()) {
AI_ParseHost(&ai, &cmd_host); AI_ParseHost(&ai);
last_online_tick = tick;
} else { } else {
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.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);
osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), 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, &gimbal_ai);
if (ref_update) AI_PackRef(&ai, &(referee_ai)); if (ref_update) AI_PackRef(&ai, &(referee_ai));
AI_StartSend(&(ai), ref_update); AI_StartSend(&(ai), ref_update);

1
User/task/command.c
View File

@ -68,7 +68,6 @@ void Task_Command(void *argument) {
if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) == if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
osOK) osOK)
CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND); CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */ osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
/* 将需要与其他任务分享的数据放到消息队列中 */ /* 将需要与其他任务分享的数据放到消息队列中 */

5
User/task/ctrl_gimbal.c
View File

@ -22,11 +22,13 @@ CMD_GimbalCmd_t gimbal_cmd;
Gimbal_t gimbal; Gimbal_t gimbal;
CAN_GimbalOutput_t gimbal_out; CAN_GimbalOutput_t gimbal_out;
Referee_GimbalUI_t gimbal_ui; Referee_GimbalUI_t gimbal_ui;
AHRS_Eulr_t gimbal_for_ai;
#else #else
static CMD_GimbalCmd_t gimbal_cmd; static CMD_GimbalCmd_t gimbal_cmd;
static Gimbal_t gimbal; static Gimbal_t gimbal;
static CAN_GimbalOutput_t gimbal_out; static CAN_GimbalOutput_t gimbal_out;
static Referee_GimbalUI_t gimbal_ui; static Referee_GimbalUI_t gimbal_ui;
static AHRS_Eulr_t gimbal_for_ai;
#endif #endif
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */ osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
Gimbal_UpdateFeedback(&gimbal, &can); Gimbal_UpdateFeedback(&gimbal, &can);
Gimbal_PackAI(&gimbal, &gimbal_for_ai);
Gimbal_Control(&gimbal, &gimbal_cmd, tick); Gimbal_Control(&gimbal, &gimbal_cmd, tick);
Gimbal_DumpOutput(&gimbal, &gimbal_out); Gimbal_DumpOutput(&gimbal, &gimbal_out);
osKernelUnlock(); osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.can.output.gimbal); osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0); 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); Gimbal_DumpUI(&gimbal, &gimbal_ui);
osMessageQueueReset(task_runtime.msgq.ui.gimbal); osMessageQueueReset(task_runtime.msgq.ui.gimbal);

176
User/task/init.c
View File

@ -30,101 +30,103 @@
* *
* \param argument 未使用 * \param argument 未使用
*/ */
void Task_Init(void *argument) { void Task_Init(void *argument)
(void)argument; /* 未使用argument，消除警告 */ {
(void)argument; /* 未使用argument，消除警告 */
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */ Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
osKernelLock(); osKernelLock();
/* 创建任务 */ /* 创建任务 */
task_runtime.thread.atti_esti = task_runtime.thread.atti_esti =
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti); osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli); task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command); task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
task_runtime.thread.ctrl_chassis = task_runtime.thread.ctrl_chassis =
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis); osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = task_runtime.thread.ctrl_gimbal =
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal); osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
task_runtime.thread.ctrl_shoot = task_runtime.thread.ctrl_shoot =
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot); osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info); task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor); task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can); task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee); task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai); task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc); task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap); task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
/* 创建消息队列 */ /* 创建消息队列 */
/* motor */ /* motor */
task_runtime.msgq.can.feedback.chassis = task_runtime.msgq.can.feedback.chassis =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.feedback.gimbal = task_runtime.msgq.can.feedback.gimbal =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.feedback.shoot = task_runtime.msgq.can.feedback.shoot =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.feedback.cap = task_runtime.msgq.can.feedback.cap =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.output.chassis = task_runtime.msgq.can.output.chassis =
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
task_runtime.msgq.can.output.gimbal = task_runtime.msgq.can.output.gimbal =
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
task_runtime.msgq.can.output.shoot = task_runtime.msgq.can.output.shoot =
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
task_runtime.msgq.can.output.cap = task_runtime.msgq.can.output.cap =
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
/* command */ /* command */
task_runtime.msgq.cmd.chassis = task_runtime.msgq.cmd.chassis =
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL); osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
task_runtime.msgq.cmd.gimbal = task_runtime.msgq.cmd.gimbal =
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL); osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
task_runtime.msgq.cmd.shoot = task_runtime.msgq.cmd.shoot =
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL); osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
task_runtime.msgq.cmd.ai = task_runtime.msgq.cmd.ai =
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL); osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL); task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
/* atti_esti */ /* atti_esti */
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL); task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
task_runtime.msgq.cmd.raw.host = task_runtime.msgq.cmd.raw.host =
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL); osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
task_runtime.msgq.gimbal.accl = task_runtime.msgq.gimbal.accl =
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL); osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
task_runtime.msgq.gimbal.eulr_imu = task_runtime.msgq.gimbal.eulr_imu =
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL); osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
task_runtime.msgq.gimbal.gyro = task_runtime.msgq.gimbal.gyro =
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL); 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 = /* AI */
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL); task_runtime.msgq.ai.quat =
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
/* AI */ /* 裁判系统 */
task_runtime.msgq.ai.quat = task_runtime.msgq.referee.ai =
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL); 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.referee.ai = task_runtime.msgq.ui.chassis =
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL); osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
task_runtime.msgq.referee.chassis = task_runtime.msgq.ui.cap =
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL); osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
task_runtime.msgq.referee.cap = task_runtime.msgq.ui.gimbal =
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL); osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
task_runtime.msgq.referee.shoot = task_runtime.msgq.ui.shoot =
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL); osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
/* UI */ osKernelUnlock();
task_runtime.msgq.ui.chassis = osThreadTerminate(osThreadGetId()); /* 结束自身 */
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()); /* 结束自身 */
} }

1
User/task/user_task.h
View File

@ -58,6 +58,7 @@ typedef struct {
osMessageQueueId_t accl; /* IMU读取 */ osMessageQueueId_t accl; /* IMU读取 */
osMessageQueueId_t gyro; /* IMU读取 */ osMessageQueueId_t gyro; /* IMU读取 */
osMessageQueueId_t eulr_imu; /* 姿态解算得到 */ osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
osMessageQueueId_t eulr_encoder; /* 编码器读取 */
} gimbal; } gimbal;
/* 控制指令 */ /* 控制指令 */