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

添加了云台数据发送到ai

Browse Source
This commit is contained in:
Robofish 2025-02-11 19:46:41 +08:00
parent fa5dae4e9f
commit 4703c39639
21 changed files with 9541 additions and 9524 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -1,5 +1,6 @@
{
"files.associations": {
"user_math.h": "c"
"user_math.h": "c",
"shoot.h": "c"
}
}

19
MDK-ARM/DevC.uvoptx
View File

@ -422,6 +422,21 @@
<WinNumber>1</WinNumber>
<ItemText>eulr_to_send</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>up_data</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>AHRS_Eulr_t</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal_encoder</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -1685,7 +1700,7 @@
<Group>
<GroupName>User/device</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1765,7 +1780,7 @@
<Group>
<GroupName>User/module</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

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

@ -10,13 +10,15 @@
extern "C" {
#endif
#define AI_NOTICE_AUTOAIM (1 << 0)
#define AI_NOTICE_HITBUFF (1 << 1)
#define AI_NOTICE_AUTOMATIC (1 << 2)
#define AI_NOTICE_FIRE (1 << 3)
#define AI_NOTICE_TRACKING (1 << 4)
#define AI_NOTICE_FIRE (1 << 0)
#define AI_NOTICE_TRACKING (1 << 1)
#define AI_NOTICE_AUTOAIM (1 << 2)
#define AI_NOTICE_HITBUFF (1 << 3)
#define AI_NOTICE_AUTOMATIC (1 << 4)
#define AI_ID_MCU (0xC4)
#define AI_ID_MCU2 (0xD4)
#define AI_ID_REF (0xA8)
#define AI_TEAM_RED (0x01)
@ -34,16 +36,16 @@ typedef struct __attribute__((packed)) {
float q3;
} quat; /* 四元数 */
// float chassis_speed; /* 底盘速度(哨兵) */
struct __attribute__((packed)) {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
float bullet_speed /* 弹丸速度 */;
uint8_t notice; /* 控制命令 */
float ball_speed; /* 子弹初速度 */
struct __attribute__((packed)) {
float left;
float right;
} distance; /* 左右距离(哨兵) */
float chassis_speed; /* 底盘速度(哨兵) */
} Protocol_UpDataMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/

4
User/bsp/mm.h
View File

@ -13,6 +13,10 @@ extern "C" {
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
void *BSP_Malloc(size_t size);
/**
* @brief
* @param pv
*/
void BSP_Free(void *pv);
#ifdef __cplusplus

34
User/component/ballistics.c
View File

@ -4,10 +4,40 @@
#include "ballistics.h"
/**
* @brief
* @param b
*/
void Ballistics_Init(Ballistics_t *b) { (void)b; }
void Ballistics_Apply(Ballistics_t *b, float bullet_speed) {
/**
* @brief
* @param b
* @param bullet_speed
*/
void Ballistics_Apply(Ballistics_t *b, float bullet_speed)
{
(void)b;
(void)bullet_speed;
}
void Ballistics_Reset(Ballistics_t *b) { (void)b; }
/**
* @brief
* @param bullet_speed 弹丸速度
* @param distance 距离
* @return 高度补偿值
*/
float Simpal_Ballistics_Apply(float bullet_speed, float distance)
{
return distance /1000 / bullet_speed;
}
/**
* @brief
* @param b
*/
void Ballistics_Reset(Ballistics_t *b) { (void)b; }

8
User/component/ballistics.h
View File

@ -16,6 +16,14 @@ typedef struct {
void Ballistics_Init(Ballistics_t *b);
void Ballistics_Apply(Ballistics_t *b, float bullet_speed);
/**
* @brief
* @param bullet_speed 弹丸速度
* @param distance 距离
* @return 高度补偿值
*/
float Simpal_Ballistics_Apply(float bullet_speed, float distance);
void Ballistics_Reset(Ballistics_t *b);
#ifdef __cplusplus

42
User/component/cmd.c
View File

@ -158,8 +158,8 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
} else {
/* ai切换至自瞄模式，启用host控制 */
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
cmd->ai_status = AI_STATUS_AUTOAIM;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
}
} else {
@ -190,31 +190,19 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 左拨杆相应行为选择和解析 */
case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK;
cmd->host_overwrite = false;
cmd->shoot.ai_fire = false;
cmd->ai_status = AI_STATUS_STOP;
break;
case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
cmd->host_overwrite = true;
cmd->shoot.ai_fire = false;
cmd->ai_status = AI_STATUS_AUTOMATIC;
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;
cmd->ai_status = AI_STATUS_AUTOMATIC;
break;
case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->host_overwrite = false;
cmd->shoot.ai_fire = false;
cmd->ai_status = AI_STATUS_STOP;
break;
}
switch (rc->sw_r) {
@ -225,7 +213,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
@ -235,36 +223,19 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
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_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;
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
if (cmd->ai_status == AI_STATUS_OFFLINE || 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;
}
}
/**
@ -360,6 +331,11 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
cmd->shoot.mode = SHOOT_MODE_LOADED;
if (host->tracking){
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
}else{
cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
}
/* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire && cmd->shoot.ai_fire) {
cmd->shoot.fire = true;

2
User/component/cmd.h
View File

@ -179,6 +179,7 @@ typedef struct {
/* AI行为状态 */
typedef enum {
AI_STATUS_OFFLINE, /* 离线状态 */
AI_STATUS_STOP, /* 停止状态 */
AI_STATUS_AUTOAIM, /* 自瞄状态 */
AI_STATUS_HITSWITCH, /* 打符状态 */
@ -255,6 +256,7 @@ typedef struct {
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
bool tracking; /* 是否追踪目标 */
bool fire; /* 开火状态 */
} CMD_Host_t;

4
User/device/ai.c
View File

@ -124,10 +124,12 @@ int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *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;

2
User/device/ai.h
View File

@ -57,7 +57,7 @@ int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
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);

60
User/module/config.c
View File

@ -403,26 +403,6 @@ static const Config_RobotParam_t param_sentry_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,
@ -578,25 +558,6 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.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,
@ -617,25 +578,6 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.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,
@ -662,7 +604,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.yaw_travel_rad = -1.0f,
.pitch_travel_rad = 1.0f,
.yaw_imu = true,
.pit_imu = true,
.pit_imu = false,
},
.low_pass_cutoff_freq = {

7
User/module/gimbal.c
View File

@ -292,6 +292,13 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
return 0;
}
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai) {
memcpy((void *)eulr_gimbal_for_ai, (const void *)&(gimbal->feedback.eulr.encoder),
sizeof(gimbal->feedback.eulr.encoder));
return 0;
}
/**
* \brief 复制云台输出值
*

6
User/module/gimbal.h
View File

@ -141,6 +141,9 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit,
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
/**
* \brief 运行云台控制逻辑
*
@ -153,6 +156,9 @@ 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);
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
/**
* \brief 复制云台输出值
*

5
User/task/ai.c
View File

@ -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 --------------------------------------------------------- */
@ -66,10 +68,11 @@ void Task_Ai(void *argument) {
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);

4
User/task/atti_esti.c
View File

@ -126,10 +126,10 @@ void Task_AttiEsti(void *argument) {
osMessageQueuePut(task_runtime.msgq.gimbal.accl, &bmi088.accl, 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_imu);
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_imu, &eulr_to_send, 0, 0);
osMessageQueueReset(task_runtime.msgq.ai.quat);
osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
osMessageQueueReset(task_runtime.msgq.ai.quat);
osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
/* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT,

5
User/task/ctrl_gimbal.c
View File

@ -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 eulr_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 eulr_gimbal_for_ai;
#endif
/* Private function --------------------------------------------------------- */
@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
Gimbal_UpdateFeedback(&gimbal, &can);
Gimbal_PackAI(&gimbal, &eulr_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, &eulr_gimbal_for_ai, 0, 0);
Gimbal_DumpUI(&gimbal, &gimbal_ui);
osMessageQueueReset(task_runtime.msgq.ui.gimbal);

6
User/task/init.c
View File

@ -30,7 +30,8 @@
*
* \param argument 未使用
*/
void Task_Init(void *argument) {
void Task_Init(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
@ -96,7 +97,8 @@ void Task_Init(void *argument) {
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);

2
User/task/user_task.h
View File

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