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

Compare commits

base: robofish:main
Branches Tags
robofish:main
robofish:原版
robofish:auto_sentry
robofish:哨兵
robofish:哨兵底盘
..
compare: robofish:哨兵底盘
Branches Tags
robofish:原版
robofish:auto_sentry
robofish:哨兵
robofish:main
robofish:哨兵底盘

2 Commits
main ... 哨兵底盘

Author SHA1 Message Date
Robofish
0eba61c81a 写到能够底盘接收数据了 2025-01-16 03:55:16 +08:00
Robofish
8e7ac722e6 这一套要留好，很好用 2025-01-16 02:17:32 +08:00
16 changed files with 9424 additions and 9346 deletions
Show Stats Expand all files Collapse all files

15
MDK-ARM/DevC.uvoptx
View File

@ -407,11 +407,6 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>rc</ItemText> <ItemText>rc</ItemText>
</Ww> </Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_host</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -663,7 +658,7 @@
<Group> <Group>
<GroupName>Application/User/USB_DEVICE/App</GroupName> <GroupName>Application/User/USB_DEVICE/App</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1219,7 +1214,7 @@
<Group> <Group>
<GroupName>Middlewares/USB_Device_Library</GroupName> <GroupName>Middlewares/USB_Device_Library</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1499,7 +1494,7 @@
<Group> <Group>
<GroupName>User/component</GroupName> <GroupName>User/component</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1675,7 +1670,7 @@
<Group> <Group>
<GroupName>User/device</GroupName> <GroupName>User/device</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1755,7 +1750,7 @@
<Group> <Group>
<GroupName>User/module</GroupName> <GroupName>User/module</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

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

@ -53,12 +53,14 @@ typedef struct __attribute__((packed)) {
/* 视觉 -> 电控 数据结构体*/ /* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
struct __attribute__((packed)) { struct __attribute__((packed)) {
float yaw; /* 偏航角（Yaw angle） */ float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */ float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */ float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */ } gimbal; /* 欧拉角 */
float distance; /*目标距离*/
uint8_t notice; /* 控制命令 */ uint8_t notice; /* 控制命令 */
struct __attribute__((packed)) { struct __attribute__((packed)) {

39
User/component/cmd.c
View File

@ -6,7 +6,6 @@
#include <string.h> #include <string.h>
#include <device\ai.h>
/** /**
* @brief 行为转换为对应按键 * @brief 行为转换为对应按键
* *
@ -190,47 +189,45 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 左拨杆相应行为选择和解析 */ /* 左拨杆相应行为选择和解析 */
case CMD_SW_UP: case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK; cmd->chassis.mode = CHASSIS_MODE_BREAK;
// cmd->host_overwrite = false;
// cmd->shoot.ai_fire = false;
break; break;
case CMD_SW_MID: case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
// cmd->host_overwrite = true;
// cmd->shoot.ai_fire = false;
break; 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;
// cmd->host_overwrite = true;
// cmd->shoot.ai_fire = true;
break; break;
case CMD_SW_ERR: case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX; cmd->chassis.mode = CHASSIS_MODE_RELAX;
// cmd->host_overwrite = false;
// cmd->shoot.ai_fire = false;
break; break;
} }
switch (rc->sw_r) { switch (rc->sw_r) {
/* 右拨杆相应行为选择和解析*/ /* 右拨杆相应行为选择和解析*/
case CMD_SW_UP: case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_SAFE; // cmd->shoot.mode = SHOOT_MODE_SAFE;
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
break; 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->host_overwrite = true;
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->ai_status = AI_STATUS_AUTOMATIC;
// cmd->shoot.fire = false;
// 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->host_overwrite = true;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE; cmd->ai_status = AI_STATUS_AUTOMATIC;
cmd->shoot.fire = true; // cmd->shoot.mode = SHOOT_MODE_LOADED;
// cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
// cmd->shoot.fire = true;
break; break;
/* /*
case CMD_SW_UP: case CMD_SW_UP:
@ -253,8 +250,9 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
*/ */
case CMD_SW_ERR: case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX; cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX; cmd->host_overwrite = false;
// cmd->host_overwrite = false; cmd->ai_status = AI_STATUS_STOP;
// cmd->shoot.mode = SHOOT_MODE_RELAX;
} }
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
cmd->chassis.ctrl_vec.vx = rc->ch_l_x; cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
@ -355,6 +353,11 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
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;
if (cmd->ai_status == AI_STATUS_AUTOMATIC){
cmd->chassis.ctrl_vec.vx = host->chassis_move_vec.vx;
cmd->chassis.ctrl_vec.vy = host->chassis_move_vec.vy;
cmd->chassis.ctrl_vec.wz = host->chassis_move_vec.wz;
}
/* host射击命令，设置不同的射击频率和弹丸初速度 */ /* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire) { if (host->fire) {
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->shoot.mode = SHOOT_MODE_LOADED;

2
User/component/cmd.h
View File

@ -84,7 +84,6 @@ typedef struct {
CMD_ShootMode_t mode; /* 射击运行模式 */ CMD_ShootMode_t mode; /* 射击运行模式 */
CMD_FireMode_t fire_mode; /* 开火模式 */ CMD_FireMode_t fire_mode; /* 开火模式 */
bool fire; /*开火*/ bool fire; /*开火*/
bool ai_fire; /* AI开火状态 */
bool cover_open; /* 弹舱盖开关 */ bool cover_open; /* 弹舱盖开关 */
bool reverse_trig; /* 拨弹电机状态 */ bool reverse_trig; /* 拨弹电机状态 */
} CMD_ShootCmd_t; } CMD_ShootCmd_t;
@ -213,6 +212,7 @@ typedef struct {
} mouse_last; /* 鼠标值 */ } mouse_last; /* 鼠标值 */
CMD_AI_Status_t ai_status; /* AI状态 */ CMD_AI_Status_t ai_status; /* AI状态 */
const CMD_Params_t *param; /* 命令参数 */ const CMD_Params_t *param; /* 命令参数 */
CMD_ChassisCmd_t chassis; /* 底盘控制命令 */ CMD_ChassisCmd_t chassis; /* 底盘控制命令 */

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

68
User/device/ai.c
View File

@ -14,11 +14,17 @@
#include "component\user_math.h" #include "component\user_math.h"
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define AI_HOST_MAX_CONTROL_VALUE (1.0f)
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0; static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[AI_LEN_RX_BUFF];
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
static bool inited = false; static bool inited = false;
@ -29,17 +35,22 @@ 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;
ai->ai_online = false;
return 0; return 0;
} }
@ -50,9 +61,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);
ai->form_host.rx_buffer, if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
sizeof(ai->form_host.rx_buffer)) == HAL_OK) AI_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
@ -62,27 +73,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)))
ai->ai_online = true;
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
goto error; goto error;
memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data)); ai->ai_online = true;
// cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit; memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
// cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw; memset(rxbuf, 0, AI_LEN_RX_BUFF);
// cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
// cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
// cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
// cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
// cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
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;
return DEVICE_OK; return DEVICE_OK;
error: error:
@ -90,12 +86,26 @@ 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;
Clip(&(cmd_host->gimbal_delta.yaw), -AI_HOST_MAX_CONTROL_VALUE,
AI_HOST_MAX_CONTROL_VALUE);
Clip(&(cmd_host->gimbal_delta.pit), -AI_HOST_MAX_CONTROL_VALUE,
AI_HOST_MAX_CONTROL_VALUE);
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;
}
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; ai->ai_online = false;
memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer)); memset(&(ai->form_host), 0, sizeof(ai->form_host));
memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
memset(cmd_host, 0, sizeof(*cmd_host)); memset(cmd_host, 0, sizeof(*cmd_host));
return 0; return 0;
} }

10
User/device/ai.h
View File

@ -36,10 +36,8 @@ typedef struct __packed {
typedef struct __packed { typedef struct __packed {
osThreadId_t thread_alert; osThreadId_t thread_alert;
struct {
uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)]; Protocol_DownPackage_t form_host;
Protocol_DownPackage_t data;
}form_host;
struct { struct {
AI_UpPackageReferee_t ref; AI_UpPackageReferee_t ref;
@ -47,7 +45,6 @@ typedef struct __packed {
} to_host; } to_host;
CMD_AI_Status_t status; CMD_AI_Status_t status;
bool ai_online; bool ai_online;
} AI_t; } AI_t;
@ -57,11 +54,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);
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/module/chassis.c
View File

@ -264,7 +264,7 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
case CHASSIS_MODE_INDENPENDENT: /* 独立模式控制向量与运动向量相等 */ case CHASSIS_MODE_INDENPENDENT: /* 独立模式控制向量与运动向量相等 */
c->move_vec.vx = c_cmd->ctrl_vec.vx; c->move_vec.vx = c_cmd->ctrl_vec.vx;
c->move_vec.vy = c_cmd->ctrl_vec.vx; c->move_vec.vy = c_cmd->ctrl_vec.vy;
break; break;
case CHASSIS_MODE_OPEN: case CHASSIS_MODE_OPEN:

16
User/module/config.c
View File

@ -202,11 +202,7 @@ static const Config_RobotParam_t param_default = {
}, /* can */ }, /* can */
}; /* param_default */ }; /* param_default */
#ifdef DEBUG
Config_RobotParam_t param_hero = {
#else
static const Config_RobotParam_t param_hero = { static const Config_RobotParam_t param_hero = {
#endif
.model = ROBOT_MODEL_HERO, .model = ROBOT_MODEL_HERO,
.chassis = { /* 底盘模块参数 */ .chassis = { /* 底盘模块参数 */
@ -240,7 +236,7 @@ static const Config_RobotParam_t param_hero = {
}, },
.reverse = { .reverse = {
.yaw = false, .yaw = true,
}, },
}, /* chassis */ }, /* chassis */
@ -289,7 +285,7 @@ static const Config_RobotParam_t param_hero = {
}, },
}, /* pid */ }, /* pid */
.pitch_travel_rad = 0.9685447f, .pitch_travel_rad = 1.07685447f,
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -297,8 +293,8 @@ static const Config_RobotParam_t param_hero = {
}, },
.reverse = { .reverse = {
.yaw = false, .yaw = true,
.pit = false, .pit = true,
}, },
.pit_ctrl_reverse = false, .pit_ctrl_reverse = false,
@ -335,8 +331,8 @@ static const Config_RobotParam_t param_hero = {
.trig = -1.0f, .trig = -1.0f,
}, },
}, },
.num_trig_tooth = 5.0f, .num_trig_tooth = 6.0f,
.trig_gear_ratio = 19.0f, .trig_gear_ratio = 3591.0f / 187.0f,
.fric_radius = 0.03f, .fric_radius = 0.03f,
.cover_open_duty = 0.125f, .cover_open_duty = 0.125f,
.cover_close_duty = 0.075f, .cover_close_duty = 0.075f,

50
User/module/gimbal.c
View File

@ -130,37 +130,39 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
Gimbal_SetMode(g, g_cmd->mode); Gimbal_SetMode(g, g_cmd->mode);
/* yaw坐标正方向与遥控器操作逻辑相反 */ /* 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.pit = g_cmd->delta_eulr.pit;
if (g->param->pit_ctrl_reverse){ g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
}else{
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
}
/* 处理yaw控制命令 */ /* 处理yaw控制命令 */
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI); CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
/* 处理pitch控制命令，软件限位 */ /* 处理pitch控制命令，软件限位 */
const float delta_max =
CircleError(g->limit.max,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.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),
M_2PI);
// 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.encoder.pit), // g->feedback.eulr.imu.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.encoder.pit), // g->feedback.eulr.imu.pit),
// M_2PI); // 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); 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;
@ -182,20 +184,16 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
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 =
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 = // 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.encoder.pit, 0.0f, g->dt); // 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->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); 0.0, 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; break;
case GIMBAL_MODE_RELATIVE: case GIMBAL_MODE_RELATIVE:

6
User/module/shoot.c
View File

@ -252,9 +252,9 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius, float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
(s->param->model == SHOOT_MODEL_17MM)); (s->param->model == SHOOT_MODEL_17MM));
s->setpoint.fric_rpm[0] = -fric_rpm; s->setpoint.fric_rpm[0] = fric_rpm;
s->setpoint.fric_rpm[1] = fric_rpm; s->setpoint.fric_rpm[1] = -fric_rpm;
s->setpoint.fric_rpm[2] = -fric_rpm; s->setpoint.fric_rpm[2] = fric_rpm;
/* 计算拨弹电机位置的目标值 */ /* 计算拨弹电机位置的目标值 */
if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) && if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&

15
User/task/ai.c
View File

@ -42,9 +42,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; 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());
@ -54,18 +52,23 @@ 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; last_online_tick = tick;
} else { } 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.ai.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0); osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai, bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
&(referee_ai), NULL, 0) == osOK); &(referee_ai), NULL, 0) == osOK);
AI_PackMCU(&ai, &quat); AI_PackMCU(&ai, &quat);
if (ref_update) AI_PackRef(&ai, &(referee_ai)); if (ref_update) AI_PackRef(&ai, &(referee_ai));

8
User/task/cli.c
View File

@ -32,10 +32,10 @@ typedef struct {
static const char *const CLI_WELCOME_MESSAGE = 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"
" -------------------------------------------------------------------\r\n" " -------------------------------------------------------------------\r\n"
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n" " FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
"\r\n"; "\r\n";