Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| b84b648d54 | |||
| 9f67e5e851 | |||
| ee7ed0b056 | |||
| 9923566a10 | |||
| b7415fd2c9 |
5
.vscode/settings.json
vendored
5
.vscode/settings.json
vendored
@ -1,5 +0,0 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"functional": "cpp"
|
||||
}
|
||||
}
|
||||
@ -347,7 +347,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -360,7 +360,7 @@
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>param_default</ItemText>
|
||||
<ItemText>chassis</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
@ -370,32 +370,49 @@
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot</ItemText>
|
||||
<ItemText>gimbal_out</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>param_hero</ItemText>
|
||||
<ItemText>for_chassis</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ref</ItemText>
|
||||
<ItemText>task_runtime</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>for_chassis</ItemText>
|
||||
<ItemText>ref</ItemText>
|
||||
</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>cmd</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_host</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<MemoryWindow4>
|
||||
<Mm>
|
||||
<WinNumber>4</WinNumber>
|
||||
<SubType>0</SubType>
|
||||
<ItemText>for_chassis</ItemText>
|
||||
<AccSizeX>0</AccSizeX>
|
||||
</Mm>
|
||||
</MemoryWindow4>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
</Tracepoint>
|
||||
@ -646,7 +663,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Application/User/USB_DEVICE/App</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1202,7 +1219,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Middlewares/USB_Device_Library</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1658,7 +1675,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/device</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1738,7 +1755,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/module</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
Binary file not shown.
18445
MDK-ARM/DevC/DevC.hex
18445
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
1
Middlewares/Third_Party/Protocol/protocol.h
vendored
1
Middlewares/Third_Party/Protocol/protocol.h
vendored
@ -21,6 +21,7 @@ extern "C" {
|
||||
#define AI_TEAM_RED (0x01)
|
||||
#define AI_TEAM_BLUE (0x02)
|
||||
|
||||
|
||||
typedef uint8_t Protocol_ID_t;
|
||||
|
||||
/* 电控 -> 视觉 MCU数据结构体*/
|
||||
|
||||
@ -12,9 +12,9 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
|
||||
if (huart->Instance == USART3)
|
||||
return BSP_UART_DR16;
|
||||
else if (huart->Instance == USART1)
|
||||
return BSP_UART_REF;
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_AI;
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_REF;
|
||||
/*
|
||||
else if (huart->Instance == USARTX)
|
||||
return BSP_UART_XXX;
|
||||
@ -110,9 +110,9 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
|
||||
case BSP_UART_DR16:
|
||||
return &huart3;
|
||||
case BSP_UART_REF:
|
||||
return &huart1;
|
||||
case BSP_UART_AI:
|
||||
return &huart6;
|
||||
case BSP_UART_AI:
|
||||
return &huart1;
|
||||
/*
|
||||
case BSP_UART_XXX:
|
||||
return &huartX;
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include <device\ai.h>
|
||||
/**
|
||||
* @brief 行为转换为对应按键
|
||||
*
|
||||
@ -86,7 +87,7 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
cmd->gimbal.delta_eulr.yaw =
|
||||
(float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
|
||||
cmd->gimbal.delta_eulr.pit =
|
||||
(float)(rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
|
||||
(float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
|
||||
cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
|
||||
cmd->shoot.reverse_trig = false;
|
||||
|
||||
@ -189,19 +190,27 @@ 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;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
// cmd->host_overwrite = true;
|
||||
// cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
|
||||
// cmd->host_overwrite = true;
|
||||
// cmd->shoot.ai_fire = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_ERR:
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
// cmd->host_overwrite = false;
|
||||
// cmd->shoot.ai_fire = false;
|
||||
break;
|
||||
}
|
||||
switch (rc->sw_r) {
|
||||
@ -245,6 +254,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
// cmd->host_overwrite = false;
|
||||
}
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
|
||||
@ -22,6 +22,8 @@ typedef enum {
|
||||
ROBOT_MODEL_ENGINEER, /* 工程机器人 */
|
||||
ROBOT_MODEL_DRONE, /* 空中机器人 */
|
||||
ROBOT_MODEL_SENTRY, /* 哨兵机器人 */
|
||||
ROBOT_MODEL_SENTRY_GIMBAL, /* 哨兵云台 */
|
||||
ROBOT_MODEL_SENTRY_CHASSIS, /* 哨兵底盘 */
|
||||
ROBOT_MODEL_NUM, /* 型号数量 */
|
||||
} CMD_RobotModel_t;
|
||||
|
||||
@ -82,6 +84,7 @@ typedef struct {
|
||||
CMD_ShootMode_t mode; /* 射击运行模式 */
|
||||
CMD_FireMode_t fire_mode; /* 开火模式 */
|
||||
bool fire; /*开火*/
|
||||
bool ai_fire; /* AI开火状态 */
|
||||
bool cover_open; /* 弹舱盖开关 */
|
||||
bool reverse_trig; /* 拨弹电机状态 */
|
||||
} CMD_ShootCmd_t;
|
||||
@ -210,7 +213,6 @@ typedef struct {
|
||||
} mouse_last; /* 鼠标值 */
|
||||
|
||||
CMD_AI_Status_t ai_status; /* AI状态 */
|
||||
|
||||
const CMD_Params_t *param; /* 命令参数 */
|
||||
|
||||
CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
|
||||
|
||||
@ -38,6 +38,8 @@ int8_t AI_Init(AI_t *ai) {
|
||||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
|
||||
Ai_RxCpltCallback);
|
||||
inited = true;
|
||||
|
||||
ai->ai_online = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -49,8 +51,8 @@ int8_t AI_Restart(void) {
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai) {
|
||||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
(uint8_t *)&(ai->form_host),
|
||||
sizeof(ai->form_host)) == HAL_OK)
|
||||
ai->form_host.rx_buffer,
|
||||
sizeof(ai->form_host.rx_buffer)) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
@ -62,15 +64,25 @@ bool AI_WaitDmaCplt(void) {
|
||||
|
||||
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
(void)cmd_host;
|
||||
if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
|
||||
ai->ai_online = true;
|
||||
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
|
||||
goto error;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
|
||||
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
|
||||
memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data));
|
||||
// cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
||||
// cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
||||
// cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
|
||||
// cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
// cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
|
||||
// cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
|
||||
// cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.data.gimbal.pit;
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
|
||||
cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
|
||||
|
||||
return DEVICE_OK;
|
||||
|
||||
error:
|
||||
@ -81,8 +93,9 @@ error:
|
||||
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
if (cmd_host == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
memset(&(ai->form_host), 0, sizeof(ai->form_host));
|
||||
ai->ai_online = false;
|
||||
memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
|
||||
memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
|
||||
memset(cmd_host, 0, sizeof(*cmd_host));
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -36,8 +36,10 @@ typedef struct __packed {
|
||||
|
||||
typedef struct __packed {
|
||||
osThreadId_t thread_alert;
|
||||
|
||||
Protocol_DownPackage_t form_host;
|
||||
struct {
|
||||
uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)];
|
||||
Protocol_DownPackage_t data;
|
||||
}form_host;
|
||||
|
||||
struct {
|
||||
AI_UpPackageReferee_t ref;
|
||||
@ -45,6 +47,8 @@ typedef struct __packed {
|
||||
} to_host;
|
||||
|
||||
CMD_AI_Status_t status;
|
||||
|
||||
bool ai_online;
|
||||
} AI_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
@ -68,8 +68,7 @@ static bool inited = false;
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
static void CAN_Motor_Decode(CAN_MotorFeedback_t *feedback,
|
||||
const uint8_t *raw)
|
||||
{
|
||||
const uint8_t *raw) {
|
||||
uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
|
||||
int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);
|
||||
|
||||
@ -80,44 +79,36 @@ static void CAN_Motor_Decode(CAN_MotorFeedback_t *feedback,
|
||||
feedback->temp = raw[6];
|
||||
}
|
||||
|
||||
void CAN_Cap_Decode(CAN_CapFeedback_t *feedback, const uint8_t *raw)
|
||||
{
|
||||
void CAN_Cap_Decode(CAN_CapFeedback_t *feedback, const uint8_t *raw) {
|
||||
feedback->input_volt = (float)((raw[1] << 8) | raw[0]) / (float)CAN_CAP_RES;
|
||||
feedback->cap_volt = (float)((raw[3] << 8) | raw[2]) / (float)CAN_CAP_RES;
|
||||
feedback->input_curr = (float)((raw[5] << 8) | raw[4]) / (float)CAN_CAP_RES;
|
||||
feedback->target_power = (float)((raw[7] << 8) | raw[6]) / (float)CAN_CAP_RES;
|
||||
}
|
||||
|
||||
void CAN_Tof_Decode(CAN_Tof_t *tof, const uint8_t *raw)
|
||||
{
|
||||
void CAN_Tof_Decode(CAN_Tof_t *tof, const uint8_t *raw) {
|
||||
tof->dist = (float)((raw[2] << 16) | (raw[1] << 8) | raw[0]) / 1000.0f;
|
||||
tof->status = raw[3];
|
||||
tof->signal_strength = (raw[5] << 8) | raw[4];
|
||||
}
|
||||
|
||||
static void CAN_CAN1RxFifoMsgPendingCallback(void)
|
||||
{
|
||||
static void CAN_CAN1RxFifoMsgPendingCallback(void) {
|
||||
HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_MOTOR_RX_FIFO,
|
||||
&raw_rx1.rx_header, raw_rx1.rx_data);
|
||||
osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0);
|
||||
}
|
||||
|
||||
static void CAN_CAN2RxFifoMsgPendingCallback(void)
|
||||
{
|
||||
static void CAN_CAN2RxFifoMsgPendingCallback(void) {
|
||||
HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_CAP_RX_FIFO,
|
||||
&raw_rx2.rx_header, raw_rx2.rx_data);
|
||||
osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param)
|
||||
{
|
||||
if (can == NULL)
|
||||
return DEVICE_ERR_NULL;
|
||||
if (inited)
|
||||
return DEVICE_ERR_INITED;
|
||||
if ((thread_alert = osThreadGetId()) == NULL)
|
||||
return DEVICE_ERR_NULL;
|
||||
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
|
||||
if (can == NULL) return DEVICE_ERR_NULL;
|
||||
if (inited) return DEVICE_ERR_INITED;
|
||||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL);
|
||||
|
||||
@ -159,175 +150,161 @@ int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param)
|
||||
}
|
||||
|
||||
int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
CAN_t *can)
|
||||
{
|
||||
if (output == NULL)
|
||||
return DEVICE_ERR_NULL;
|
||||
CAN_t *can) {
|
||||
if (output == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
int16_t motor1, motor2, motor3, motor4;
|
||||
int16_t yaw_motor, pit_motor;
|
||||
int16_t fric1_motor, fric2_motor, fric3_motor, trig_motor;
|
||||
switch (group)
|
||||
{
|
||||
case CAN_MOTOR_GROUT_CHASSIS:
|
||||
motor1 =
|
||||
(int16_t)(output->chassis.named.m1 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
motor2 =
|
||||
(int16_t)(output->chassis.named.m2 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
motor3 =
|
||||
(int16_t)(output->chassis.named.m3 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
motor4 =
|
||||
(int16_t)(output->chassis.named.m4 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
switch (group) {
|
||||
case CAN_MOTOR_GROUT_CHASSIS:
|
||||
motor1 =
|
||||
(int16_t)(output->chassis.named.m1 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
motor2 =
|
||||
(int16_t)(output->chassis.named.m2 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
motor3 =
|
||||
(int16_t)(output->chassis.named.m3 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
motor4 =
|
||||
(int16_t)(output->chassis.named.m4 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
|
||||
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_BASE;
|
||||
raw_tx.tx_header.IDE = CAN_ID_STD;
|
||||
raw_tx.tx_header.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_BASE;
|
||||
raw_tx.tx_header.IDE = CAN_ID_STD;
|
||||
raw_tx.tx_header.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
|
||||
raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
|
||||
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
|
||||
raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
|
||||
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
|
||||
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.chassis));
|
||||
break;
|
||||
case CAN_MOTOR_GROUT_GIMBAL1:
|
||||
case CAN_MOTOR_GROUT_GIMBAL2:
|
||||
yaw_motor =
|
||||
(int16_t)(output->gimbal.named.yaw * (float)CAN_GM6020_MAX_ABS_LSB);
|
||||
pit_motor =
|
||||
(int16_t)(output->gimbal.named.pit * (float)CAN_GM6020_MAX_ABS_LSB);
|
||||
raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_EXTAND;
|
||||
raw_tx.tx_header.IDE = CAN_ID_STD;
|
||||
raw_tx.tx_header.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.chassis));
|
||||
break;
|
||||
case CAN_MOTOR_GROUT_GIMBAL1:
|
||||
case CAN_MOTOR_GROUT_GIMBAL2:
|
||||
yaw_motor =
|
||||
(int16_t)(output->gimbal.named.yaw * (float)CAN_GM6020_MAX_ABS_LSB);
|
||||
pit_motor =
|
||||
(int16_t)(output->gimbal.named.pit * (float)CAN_GM6020_MAX_ABS_LSB);
|
||||
raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_EXTAND;
|
||||
raw_tx.tx_header.IDE = CAN_ID_STD;
|
||||
raw_tx.tx_header.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
|
||||
raw_tx.tx_data[0] = (uint8_t)((yaw_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(yaw_motor & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((pit_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(pit_motor & 0xFF);
|
||||
raw_tx.tx_data[4] = 0;
|
||||
raw_tx.tx_data[5] = 0;
|
||||
raw_tx.tx_data[6] = 0;
|
||||
raw_tx.tx_data[7] = 0;
|
||||
raw_tx.tx_data[0] = (uint8_t)((yaw_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(yaw_motor & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((pit_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(pit_motor & 0xFF);
|
||||
raw_tx.tx_data[4] = 0;
|
||||
raw_tx.tx_data[5] = 0;
|
||||
raw_tx.tx_data[6] = 0;
|
||||
raw_tx.tx_data[7] = 0;
|
||||
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal.yaw),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.gimbal));
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal.pitch),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.gimbal));
|
||||
// HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
|
||||
// &raw_tx.tx_header, raw_tx.tx_data,
|
||||
// &(can->mailbox.gimbal));
|
||||
break;
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.gimbal));
|
||||
// HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
|
||||
// &raw_tx.tx_header, raw_tx.tx_data,
|
||||
// &(can->mailbox.gimbal));
|
||||
break;
|
||||
|
||||
case CAN_MOTOR_GROUT_SHOOT1:
|
||||
case CAN_MOTOR_GROUT_SHOOT2:
|
||||
fric1_motor =
|
||||
(int16_t)(output->shoot.named.fric1 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
fric2_motor =
|
||||
(int16_t)(output->shoot.named.fric2 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
fric3_motor =
|
||||
(int16_t)(output->shoot.named.fric3 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
trig_motor =
|
||||
(int16_t)(output->shoot.named.trig * (float)CAN_M2006_MAX_ABS_LSB);
|
||||
case CAN_MOTOR_GROUT_SHOOT1:
|
||||
case CAN_MOTOR_GROUT_SHOOT2:
|
||||
fric1_motor =
|
||||
(int16_t)(output->shoot.named.fric1 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
fric2_motor =
|
||||
(int16_t)(output->shoot.named.fric2 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
fric3_motor =
|
||||
(int16_t)(output->shoot.named.fric3 * (float)CAN_M3508_MAX_ABS_LSB);
|
||||
trig_motor =
|
||||
(int16_t)(output->shoot.named.trig * (float)CAN_M2006_MAX_ABS_LSB);
|
||||
|
||||
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_EXTAND;
|
||||
raw_tx.tx_header.IDE = CAN_ID_STD;
|
||||
raw_tx.tx_header.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_EXTAND;
|
||||
raw_tx.tx_header.IDE = CAN_ID_STD;
|
||||
raw_tx.tx_header.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
|
||||
raw_tx.tx_data[0] = (uint8_t)((fric1_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(fric1_motor & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((fric2_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(fric2_motor & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((fric3_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(fric3_motor & 0xFF);
|
||||
raw_tx.tx_data[6] = (uint8_t)((trig_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[7] = (uint8_t)(trig_motor & 0xFF);
|
||||
raw_tx.tx_data[0] = (uint8_t)((fric1_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(fric1_motor & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((fric2_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(fric2_motor & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((fric3_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(fric3_motor & 0xFF);
|
||||
raw_tx.tx_data[6] = (uint8_t)((trig_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[7] = (uint8_t)(trig_motor & 0xFF);
|
||||
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.shoot));
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->trig),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.shoot));
|
||||
break;
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.shoot));
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx)
|
||||
{
|
||||
if (can == NULL)
|
||||
return DEVICE_ERR_NULL;
|
||||
if (can_rx == NULL)
|
||||
return DEVICE_ERR_NULL;
|
||||
int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
|
||||
if (can == NULL) return DEVICE_ERR_NULL;
|
||||
if (can_rx == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
uint32_t index;
|
||||
switch (can_rx->rx_header.StdId)
|
||||
{
|
||||
case CAN_M3508_M1_ID:
|
||||
case CAN_M3508_M2_ID:
|
||||
case CAN_M3508_M3_ID:
|
||||
case CAN_M3508_M4_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
|
||||
CAN_Motor_Decode(&(can->motor.chassis.as_array[index]), can_rx->rx_data);
|
||||
can->recive_flag |= 1 << index;
|
||||
break;
|
||||
switch (can_rx->rx_header.StdId) {
|
||||
case CAN_M3508_M1_ID:
|
||||
case CAN_M3508_M2_ID:
|
||||
case CAN_M3508_M3_ID:
|
||||
case CAN_M3508_M4_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
|
||||
CAN_Motor_Decode(&(can->motor.chassis.as_array[index]), can_rx->rx_data);
|
||||
can->recive_flag |= 1 << index;
|
||||
break;
|
||||
|
||||
case CAN_M3508_FRIC1_ID:
|
||||
case CAN_M3508_FRIC2_ID:
|
||||
case CAN_M3508_FRIC3_ID:
|
||||
case CAN_M2006_TRIG_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_M3508_FRIC1_ID;
|
||||
can->recive_flag |= 1 << (index + 6);
|
||||
CAN_Motor_Decode(&(can->motor.shoot.as_array[index]), can_rx->rx_data);
|
||||
break;
|
||||
case CAN_GM6020_YAW_ID:
|
||||
case CAN_GM6020_PIT_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_GM6020_YAW_ID;
|
||||
can->recive_flag |= 1 << (index + 4);
|
||||
CAN_Motor_Decode(&(can->motor.gimbal.as_array[index]), can_rx->rx_data);
|
||||
break;
|
||||
case CAN_CAP_FB_ID_BASE:
|
||||
can->recive_flag |= 1 << 9;
|
||||
CAN_Cap_Decode(&(can->cap.cap_feedback), can_rx->rx_data);
|
||||
break;
|
||||
case CAN_TOF_ID_BASE:
|
||||
can->recive_flag |= 1 << 10;
|
||||
CAN_Tof_Decode(&(can->tof), can_rx->rx_data);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case CAN_M3508_FRIC1_ID:
|
||||
case CAN_M3508_FRIC2_ID:
|
||||
case CAN_M3508_FRIC3_ID:
|
||||
case CAN_M2006_TRIG_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_M3508_FRIC1_ID;
|
||||
can->recive_flag |= 1 << (index + 6);
|
||||
CAN_Motor_Decode(&(can->motor.shoot.as_array[index]), can_rx->rx_data);
|
||||
break;
|
||||
|
||||
case CAN_GM6020_YAW_ID:
|
||||
case CAN_GM6020_PIT_ID:
|
||||
index = can_rx->rx_header.StdId - CAN_GM6020_YAW_ID;
|
||||
can->recive_flag |= 1 << (index + 4);
|
||||
CAN_Motor_Decode(&(can->motor.gimbal.as_array[index]), can_rx->rx_data);
|
||||
break;
|
||||
case CAN_CAP_FB_ID_BASE:
|
||||
can->recive_flag |= 1 << 9;
|
||||
CAN_Cap_Decode(&(can->cap.cap_feedback), can_rx->rx_data);
|
||||
break;
|
||||
case CAN_TOF_ID_BASE:
|
||||
can->recive_flag |= 1 << 10;
|
||||
CAN_Tof_Decode(&(can->tof), can_rx->rx_data);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
bool CAN_CheckFlag(CAN_t *can, uint32_t flag)
|
||||
{
|
||||
if (can == NULL)
|
||||
return false;
|
||||
bool CAN_CheckFlag(CAN_t *can, uint32_t flag) {
|
||||
if (can == NULL) return false;
|
||||
return (can->recive_flag & flag) == flag;
|
||||
}
|
||||
|
||||
int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag)
|
||||
{
|
||||
if (can == NULL)
|
||||
return DEVICE_ERR_NULL;
|
||||
int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag) {
|
||||
if (can == NULL) return DEVICE_ERR_NULL;
|
||||
can->recive_flag &= ~flag;
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can)
|
||||
{
|
||||
int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can) {
|
||||
float power_limit = output->power_limit;
|
||||
|
||||
uint16_t cap = (uint16_t)(power_limit * CAN_CAP_RES);
|
||||
@ -346,8 +323,7 @@ int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can)
|
||||
}
|
||||
|
||||
void CAN_CAP_HandleOffline(CAN_Capacitor_t *cap, CAN_CapOutput_t *cap_out,
|
||||
float power_chassis)
|
||||
{
|
||||
float power_chassis) {
|
||||
cap->cap_status = CAN_CAP_STATUS_OFFLINE;
|
||||
cap_out->power_limit = power_chassis;
|
||||
}
|
||||
|
||||
@ -80,16 +80,10 @@ typedef enum {
|
||||
CAN_GM6020_PIT_ID = 0x20A, /* 6 */
|
||||
} CAN_MotorId_t;
|
||||
|
||||
typedef struct {
|
||||
BSP_CAN_t yaw;
|
||||
BSP_CAN_t pitch;
|
||||
} Gimbal_CAN_t;
|
||||
|
||||
typedef struct {
|
||||
BSP_CAN_t chassis;
|
||||
Gimbal_CAN_t gimbal;
|
||||
BSP_CAN_t gimbal;
|
||||
BSP_CAN_t shoot;
|
||||
BSP_CAN_t trig;
|
||||
BSP_CAN_t cap;
|
||||
} CAN_Params_t;
|
||||
|
||||
|
||||
@ -149,6 +149,8 @@ static const Config_RobotParam_t param_default = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -194,10 +196,8 @@ static const Config_RobotParam_t param_default = {
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal.yaw = BSP_CAN_2,
|
||||
.gimbal.pitch = BSP_CAN_2,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.trig = BSP_CAN_2,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_default */
|
||||
@ -248,20 +248,20 @@ static const Config_RobotParam_t param_hero = {
|
||||
.pid = {
|
||||
{
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.24f,
|
||||
.k = 0.45f,
|
||||
.p = 1.0f,
|
||||
.i = 0.5f,
|
||||
.d = 0.0f,
|
||||
.i = 6.0f,
|
||||
.d = 0.0008f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
.k = 10.0f,
|
||||
.k = 20.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.05f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
@ -278,9 +278,9 @@ static const Config_RobotParam_t param_hero = {
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
.k = 2.0f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.k = 12.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
@ -300,6 +300,8 @@ static const Config_RobotParam_t param_hero = {
|
||||
.yaw = false,
|
||||
.pit = false,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -345,14 +347,392 @@ static const Config_RobotParam_t param_hero = {
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal.yaw = BSP_CAN_1,
|
||||
.gimbal.pitch = BSP_CAN_2,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.trig = BSP_CAN_1,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_hero */
|
||||
|
||||
#ifdef DEBUG
|
||||
Config_RobotParam_t param_sentry_chassis = {
|
||||
#else
|
||||
static const Config_RobotParam_t param_sentry_chassis = {
|
||||
#endif
|
||||
.model = ROBOT_MODEL_SENTRY_CHASSIS,
|
||||
|
||||
.chassis = { /* 底盘模块参数 */
|
||||
.type = CHASSIS_TYPE_MECANUM,
|
||||
|
||||
.motor_pid_param = {
|
||||
.k = 0.001f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
|
||||
.follow_pid_param = {
|
||||
.k = 0.5f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.in = -1.0f,
|
||||
.out = -1.0f,
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
},
|
||||
|
||||
}, /* chassis */
|
||||
|
||||
.gimbal = { /* 云台模块参数 */
|
||||
.pid = {
|
||||
{
|
||||
// /* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
// .k = 0.25f,
|
||||
// .p = 1.0f,
|
||||
// .i = 1.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
// .k = 12.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.05f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.24f,
|
||||
.p = 1.0f,
|
||||
.i = 0.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
.k = 10.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.05f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
}, {
|
||||
// /* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
// .k = 0.35f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.f,
|
||||
// .d = 0.003f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
// .k = 15.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
.k = 0.25f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
.k = 2.0f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.85f,
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
.gyro = 1000.0f,
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = true,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
|
||||
.fric_pid_param = {
|
||||
.k = 0.001f,
|
||||
.p = 1.0f,
|
||||
.i = 0.2f,
|
||||
.d = 0.01f,
|
||||
.i_limit = 0.5f,
|
||||
.out_limit = 0.5f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
},
|
||||
.trig_pid_param = {
|
||||
.k = 12.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0450000018f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.low_pass_cutoff_freq = {
|
||||
.in = {
|
||||
.fric = -1.0f,
|
||||
.trig = -1.0f,
|
||||
},
|
||||
.out = {
|
||||
.fric = -1.0f,
|
||||
.trig = -1.0f,
|
||||
},
|
||||
},
|
||||
.num_trig_tooth = 8.0f,
|
||||
.trig_gear_ratio = 36.0f,
|
||||
.fric_radius = 0.03f,
|
||||
.cover_open_duty = 0.10f,
|
||||
.cover_close_duty = 0.050f,
|
||||
.model = SHOOT_MODEL_17MM,
|
||||
.bullet_speed = 6.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_sentry_chassis */
|
||||
|
||||
#ifdef DEBUG
|
||||
Config_RobotParam_t param_sentry_gimbal = {
|
||||
#else
|
||||
static const Config_RobotParam_t param_sentry_gimbal = {
|
||||
#endif
|
||||
.model = ROBOT_MODEL_SENTRY_GIMBAL,
|
||||
|
||||
.chassis = { /* 底盘模块参数 */
|
||||
.type = CHASSIS_TYPE_MECANUM,
|
||||
|
||||
.motor_pid_param = {
|
||||
.k = 0.001f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
|
||||
.follow_pid_param = {
|
||||
.k = 0.5f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.in = -1.0f,
|
||||
.out = -1.0f,
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
},
|
||||
|
||||
}, /* chassis */
|
||||
|
||||
.gimbal = { /* 云台模块参数 */
|
||||
.pid = {
|
||||
{
|
||||
// /* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
// .k = 0.25f,
|
||||
// .p = 1.0f,
|
||||
// .i = 1.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
// .k = 12.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.05f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.24f,
|
||||
.p = 1.0f,
|
||||
.i = 0.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_YAW_ANGLE_IDX */
|
||||
.k = 10.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.05f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
}, {
|
||||
// /* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
// .k = 0.35f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.f,
|
||||
// .d = 0.003f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = -1.0f,
|
||||
// }, {
|
||||
// /* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
// .k = 15.0f,
|
||||
// .p = 1.0f,
|
||||
// .i = 0.0f,
|
||||
// .d = 0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit = 10.0f,
|
||||
// .d_cutoff_freq = -1.0f,
|
||||
// .range = M_2PI,
|
||||
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||
.k = 0.25f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
.k = 2.0f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 0.85f,
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
.gyro = 1000.0f,
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = true,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
|
||||
.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 / 20.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_sentry_gimbal */
|
||||
|
||||
/* static const Config_RobotParam_t param_xxx; */
|
||||
|
||||
static const Config_PilotCfg_t cfg_qs = {
|
||||
@ -419,6 +799,8 @@ static const Config_RobotParamMap_t robot_param_map[] = {
|
||||
{"default", ¶m_default},
|
||||
{"infantry", ¶m_default},
|
||||
{"hero", ¶m_hero},
|
||||
{"sentry_chassis", ¶m_sentry_chassis},
|
||||
{"sentry_gimbal", ¶m_sentry_gimbal},
|
||||
// {"engineer", ¶m_engineer},
|
||||
// {"drone", ¶m_drone},
|
||||
// {"sentry", ¶m_sentry},
|
||||
@ -438,21 +820,17 @@ static const Config_PilotCfgMap_t pilot_cfg_map[] = {
|
||||
*
|
||||
* \param cfg 配置信息
|
||||
*/
|
||||
void Config_Get(Config_t *cfg)
|
||||
{
|
||||
void Config_Get(Config_t *cfg) {
|
||||
BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
|
||||
cfg->pilot_cfg = Config_GetPilotCfg(cfg->pilot_cfg_name);
|
||||
cfg->robot_param = Config_GetRobotParam(cfg->robot_param_name);
|
||||
/* 防止第一次烧写后访问NULL指针 */
|
||||
if (cfg->robot_param == NULL)
|
||||
cfg->robot_param = ¶m_default;
|
||||
if (cfg->pilot_cfg == NULL)
|
||||
cfg->pilot_cfg = &cfg_qs;
|
||||
if (cfg->robot_param == NULL) cfg->robot_param = ¶m_default;
|
||||
if (cfg->pilot_cfg == NULL) cfg->pilot_cfg = &cfg_qs;
|
||||
/* 防止擦除后全为1 */
|
||||
if ((uint32_t)(cfg->robot_param) == UINT32_MAX)
|
||||
cfg->robot_param = ¶m_default;
|
||||
if ((uint32_t)(cfg->pilot_cfg) == UINT32_MAX)
|
||||
cfg->pilot_cfg = &cfg_qs;
|
||||
if ((uint32_t)(cfg->pilot_cfg) == UINT32_MAX) cfg->pilot_cfg = &cfg_qs;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -460,8 +838,7 @@ void Config_Get(Config_t *cfg)
|
||||
*
|
||||
* \param cfg 配置信息
|
||||
*/
|
||||
void Config_Set(Config_t *cfg)
|
||||
{
|
||||
void Config_Set(Config_t *cfg) {
|
||||
osKernelLock();
|
||||
BSP_Flash_EraseSector(11);
|
||||
BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
|
||||
@ -474,14 +851,10 @@ void Config_Set(Config_t *cfg)
|
||||
* @param robot_param_name 机器人参数名称
|
||||
* @return const Config_RobotParam_t* 机器人参数的指针
|
||||
*/
|
||||
const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name)
|
||||
{
|
||||
if (robot_param_name == NULL)
|
||||
return NULL;
|
||||
for (size_t j = 0; robot_param_map[j].name != NULL; j++)
|
||||
{
|
||||
if (strcmp(robot_param_map[j].name, robot_param_name) == 0)
|
||||
{
|
||||
const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name) {
|
||||
if (robot_param_name == NULL) return NULL;
|
||||
for (size_t j = 0; robot_param_map[j].name != NULL; j++) {
|
||||
if (strcmp(robot_param_map[j].name, robot_param_name) == 0) {
|
||||
return robot_param_map[j].param;
|
||||
}
|
||||
}
|
||||
@ -494,26 +867,20 @@ const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name)
|
||||
* @param pilot_cfg_name 操作手配置名称
|
||||
* @return const Config_PilotCfg_t* 操作手配置的指针
|
||||
*/
|
||||
const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name)
|
||||
{
|
||||
if (pilot_cfg_name == NULL)
|
||||
return NULL;
|
||||
for (size_t j = 0; pilot_cfg_map[j].name != NULL; j++)
|
||||
{
|
||||
if (strcmp(pilot_cfg_map[j].name, pilot_cfg_name) == 0)
|
||||
{
|
||||
const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name) {
|
||||
if (pilot_cfg_name == NULL) return NULL;
|
||||
for (size_t j = 0; pilot_cfg_map[j].name != NULL; j++) {
|
||||
if (strcmp(pilot_cfg_map[j].name, pilot_cfg_name) == 0) {
|
||||
return pilot_cfg_map[j].param;
|
||||
}
|
||||
}
|
||||
return NULL; /* No match. */
|
||||
}
|
||||
|
||||
const Config_PilotCfgMap_t *Config_GetPilotNameMap(void)
|
||||
{
|
||||
const Config_PilotCfgMap_t *Config_GetPilotNameMap(void) {
|
||||
return pilot_cfg_map;
|
||||
}
|
||||
|
||||
const Config_RobotParamMap_t *Config_GetRobotNameMap(void)
|
||||
{
|
||||
const Config_RobotParamMap_t *Config_GetRobotNameMap(void) {
|
||||
return robot_param_map;
|
||||
}
|
||||
|
||||
@ -130,8 +130,12 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
/* yaw坐标正方向与遥控器操作逻辑相反 */
|
||||
g_cmd->delta_eulr.pit = -g_cmd->delta_eulr.pit;
|
||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.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;
|
||||
}
|
||||
|
||||
/* 处理yaw控制命令 */
|
||||
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
||||
@ -147,6 +151,16 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.imu.pit),
|
||||
M_2PI);
|
||||
// const float delta_max =
|
||||
// CircleError(g->limit.max,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.encoder.pit),
|
||||
// M_2PI);
|
||||
// const float delta_min =
|
||||
// CircleError(g->limit.min,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.encoder.pit),
|
||||
// M_2PI);
|
||||
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
|
||||
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
|
||||
|
||||
@ -171,9 +185,17 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
pit_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||
// pit_omega_set_point =
|
||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
// g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||
|
||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
g->feedback.gyro.x, 0.f, g->dt);
|
||||
|
||||
// g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
// 0.0, 0.f, g->dt);
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_RELATIVE:
|
||||
|
||||
@ -60,6 +60,8 @@ typedef struct {
|
||||
bool pit;
|
||||
} reverse;
|
||||
|
||||
bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
|
||||
|
||||
} Gimbal_Params_t;
|
||||
|
||||
/* 软件限位 */
|
||||
|
||||
@ -2,210 +2,209 @@
|
||||
* 射击模组
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <cmsis_os2.h>
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <cmsis_os2.h>
|
||||
|
||||
#include "component\cmd.h"
|
||||
#include "component\filter.h"
|
||||
#include "component\pid.h"
|
||||
#include "device\can.h"
|
||||
#include "device\referee.h"
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
#define SHOOT_OK (0) /* 运行正常 */
|
||||
#define SHOOT_ERR (-1) /* 运行时发现了其他错误 */
|
||||
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
||||
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ShootMode_t */
|
||||
#include "component\cmd.h"
|
||||
#include "component\filter.h"
|
||||
#include "component\pid.h"
|
||||
#include "device\can.h"
|
||||
#include "device\referee.h"
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
#define SHOOT_OK (0) /* 运行正常 */
|
||||
#define SHOOT_ERR (-1) /* 运行时发现了其他错误 */
|
||||
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
||||
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ShootMode_t */
|
||||
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/* 用enum组合所有PID，方便访问，配合数组使用 */
|
||||
enum Shoot_Acuator_e {
|
||||
SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC3_IDX, /* 3号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */
|
||||
SHOOT_ACTR_NUM, /* 总共的动作器数量 */
|
||||
};
|
||||
/* 用enum组合所有PID，方便访问，配合数组使用 */
|
||||
enum Shoot_Acuator_e {
|
||||
SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC3_IDX, /* 3号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */
|
||||
SHOOT_ACTR_NUM, /* 总共的动作器数量 */
|
||||
};
|
||||
|
||||
/* 发射机构型号 */
|
||||
typedef enum {
|
||||
SHOOT_MODEL_17MM = 0, /* 17mm发射机构 */
|
||||
SHOOT_MODEL_42MM, /* 42mm发射机构 */
|
||||
} Shoot_Model_t;
|
||||
/* 发射机构型号 */
|
||||
typedef enum {
|
||||
SHOOT_MODEL_17MM = 0, /* 17mm发射机构 */
|
||||
SHOOT_MODEL_42MM, /* 42mm发射机构 */
|
||||
} Shoot_Model_t;
|
||||
|
||||
/* 射击参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
|
||||
typedef struct {
|
||||
KPID_Params_t fric_pid_param; /* 摩擦轮电机控制PID的参数 */
|
||||
KPID_Params_t trig_pid_param; /* 扳机电机控制PID的参数 */
|
||||
/* 低通滤波器截止频率 */
|
||||
struct {
|
||||
/* 输入 */
|
||||
struct {
|
||||
float fric; /* 摩擦轮电机 */
|
||||
float trig; /* 扳机电机 */
|
||||
} in;
|
||||
/* 射击参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
|
||||
typedef struct {
|
||||
KPID_Params_t fric_pid_param; /* 摩擦轮电机控制PID的参数 */
|
||||
KPID_Params_t trig_pid_param; /* 扳机电机控制PID的参数 */
|
||||
/* 低通滤波器截止频率 */
|
||||
struct {
|
||||
/* 输入 */
|
||||
struct {
|
||||
float fric; /* 摩擦轮电机 */
|
||||
float trig; /* 扳机电机 */
|
||||
} in;
|
||||
|
||||
/* 输出 */
|
||||
struct {
|
||||
float fric; /* 摩擦轮电机 */
|
||||
float trig; /* 扳机电机 */
|
||||
} out;
|
||||
} low_pass_cutoff_freq;
|
||||
/* 输出 */
|
||||
struct {
|
||||
float fric; /* 摩擦轮电机 */
|
||||
float trig; /* 扳机电机 */
|
||||
} out;
|
||||
} low_pass_cutoff_freq;
|
||||
|
||||
float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */
|
||||
float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */
|
||||
float fric_radius; /* 摩擦轮半径，单位：米 */
|
||||
float cover_open_duty; /* 弹舱盖打开时舵机PWM占空比 */
|
||||
float cover_close_duty; /* 弹舱盖关闭时舵机PWM占空比 */
|
||||
Shoot_Model_t model; /* 发射机构型号 */
|
||||
float bullet_speed; /* 弹丸初速度 */
|
||||
uint32_t min_shoot_delay; /* 通过设置最小射击间隔来设置最大射频 */
|
||||
} Shoot_Params_t;
|
||||
float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */
|
||||
float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */
|
||||
float fric_radius; /* 摩擦轮半径，单位：米 */
|
||||
float cover_open_duty; /* 弹舱盖打开时舵机PWM占空比 */
|
||||
float cover_close_duty; /* 弹舱盖关闭时舵机PWM占空比 */
|
||||
Shoot_Model_t model; /* 发射机构型号 */
|
||||
float bullet_speed; /* 弹丸初速度 */
|
||||
uint32_t min_shoot_delay; /* 通过设置最小射击间隔来设置最大射频 */
|
||||
} Shoot_Params_t;
|
||||
|
||||
typedef struct {
|
||||
float heat; /* 现在热量水平 */
|
||||
float last_heat; /* 之前的热量水平 */
|
||||
float heat_limit; /* 热量上限 */
|
||||
float speed_limit; /* 弹丸初速是上限 */
|
||||
float cooling_rate; /* 冷却速率 */
|
||||
float heat_increase; /* 每发热量增加值 */
|
||||
typedef struct {
|
||||
float heat; /* 现在热量水平 */
|
||||
float last_heat; /* 之前的热量水平 */
|
||||
float heat_limit; /* 热量上限 */
|
||||
float speed_limit; /* 弹丸初速是上限 */
|
||||
float cooling_rate; /* 冷却速率 */
|
||||
float heat_increase; /* 每发热量增加值 */
|
||||
|
||||
float last_bullet_speed; /* 之前的弹丸速度 */
|
||||
uint32_t available_shot; /* 热量范围内还可以发射的数量 */
|
||||
} Shoot_HeatCtrl_t;
|
||||
float last_bullet_speed; /* 之前的弹丸速度 */
|
||||
uint32_t available_shot; /* 热量范围内还可以发射的数量 */
|
||||
} Shoot_HeatCtrl_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t last_shoot; /* 上次射击时间 单位：ms */
|
||||
bool last_fire; /* 上次开火状态 */
|
||||
bool first_fire; /* 第一次收到开火指令 */
|
||||
uint32_t shooted; /* 已经发射的弹丸 */
|
||||
uint32_t to_shoot; /* 计划发射的弹丸 */
|
||||
float bullet_speed; /* 弹丸初速度 */
|
||||
uint32_t period_ms; /* 弹丸击发延迟 */
|
||||
CMD_FireMode_t fire_mode;
|
||||
} Shoot_FireCtrl_t;
|
||||
typedef struct {
|
||||
uint32_t last_shoot; /* 上次射击时间 单位：ms */
|
||||
bool last_fire; /* 上次开火状态 */
|
||||
bool first_fire; /* 第一次收到开火指令 */
|
||||
uint32_t shooted; /* 已经发射的弹丸 */
|
||||
uint32_t to_shoot; /* 计划发射的弹丸 */
|
||||
float bullet_speed; /* 弹丸初速度 */
|
||||
uint32_t period_ms; /* 弹丸击发延迟 */
|
||||
CMD_FireMode_t fire_mode;
|
||||
} Shoot_FireCtrl_t;
|
||||
|
||||
/*
|
||||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
|
||||
* 包含了初始化参数，中间变量，输出变量。
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t lask_wakeup;
|
||||
float dt;
|
||||
/*
|
||||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
|
||||
* 包含了初始化参数，中间变量，输出变量。
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t lask_wakeup;
|
||||
float dt;
|
||||
|
||||
const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */
|
||||
const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */
|
||||
|
||||
/* 模块通用 */
|
||||
CMD_ShootMode_t mode; /* 射击模式 */
|
||||
/* 模块通用 */
|
||||
CMD_ShootMode_t mode; /* 射击模式 */
|
||||
|
||||
/* 反馈信息 */
|
||||
struct {
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
float trig_angle; /* 拨弹转盘角度，单位：弧度 */
|
||||
} feedback;
|
||||
/* 反馈信息 */
|
||||
struct {
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
float trig_angle; /* 拨弹转盘角度，单位：弧度 */
|
||||
} feedback;
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
} setpoint;
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
} setpoint;
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t fric[3]; /* 控制摩擦轮 */
|
||||
KPID_t trig; /* 控制拨弹电机 */
|
||||
} pid;
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t fric[3]; /* 控制摩擦轮 */
|
||||
KPID_t trig; /* 控制拨弹电机 */
|
||||
} pid;
|
||||
|
||||
/* 过滤器 */
|
||||
struct {
|
||||
/* 反馈值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} in;
|
||||
/* 过滤器 */
|
||||
struct {
|
||||
/* 反馈值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} in;
|
||||
|
||||
/* 输出值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} out;
|
||||
} filter;
|
||||
/* 输出值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} out;
|
||||
} filter;
|
||||
|
||||
Shoot_HeatCtrl_t heat_ctrl;
|
||||
Shoot_FireCtrl_t fire_ctrl;
|
||||
Shoot_HeatCtrl_t heat_ctrl;
|
||||
Shoot_FireCtrl_t fire_ctrl;
|
||||
|
||||
float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */
|
||||
float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */
|
||||
|
||||
} Shoot_t;
|
||||
} Shoot_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
/**
|
||||
* \brief 初始化射击
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param param 包含射击参数的结构体指针
|
||||
* \param target_freq 任务预期的运行频率
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq);
|
||||
/**
|
||||
* \brief 初始化射击
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param param 包含射击参数的结构体指针
|
||||
* \param target_freq 任务预期的运行频率
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq);
|
||||
|
||||
/**
|
||||
* \brief 更新射击的反馈信息
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param can CAN设备结构体
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can);
|
||||
/**
|
||||
* \brief 更新射击的反馈信息
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param can CAN设备结构体
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can);
|
||||
|
||||
/**
|
||||
* \brief 运行射击控制逻辑
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param s_cmd 射击控制指令
|
||||
* \param s_ref 裁判系统数据
|
||||
* \param dt_sec 两次调用的时间间隔
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
Referee_ForShoot_t *s_ref, uint32_t now);
|
||||
/**
|
||||
* \brief 运行射击控制逻辑
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param s_cmd 射击控制指令
|
||||
* \param s_ref 裁判系统数据
|
||||
* \param dt_sec 两次调用的时间间隔
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
Referee_ForShoot_t *s_ref, uint32_t now);
|
||||
|
||||
/**
|
||||
* \brief 复制射击输出值
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param out CAN设备射击输出结构体
|
||||
*/
|
||||
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out);
|
||||
/**
|
||||
* \brief 复制射击输出值
|
||||
*
|
||||
* \param s 包含射击数据的结构体
|
||||
* \param out CAN设备射击输出结构体
|
||||
*/
|
||||
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out);
|
||||
|
||||
/**
|
||||
* \brief 清空输出值
|
||||
*
|
||||
* \param output 要清空的结构体
|
||||
*/
|
||||
void Shoot_ResetOutput(CAN_ShootOutput_t *output);
|
||||
/**
|
||||
* \brief 清空输出值
|
||||
*
|
||||
* \param output 要清空的结构体
|
||||
*/
|
||||
void Shoot_ResetOutput(CAN_ShootOutput_t *output);
|
||||
|
||||
/**
|
||||
* @brief 导出射击UI数据
|
||||
*
|
||||
* @param s 射击结构体
|
||||
* @param ui UI结构体
|
||||
*/
|
||||
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
/**
|
||||
* @brief 导出射击UI数据
|
||||
*
|
||||
* @param s 射击结构体
|
||||
* @param ui UI结构体
|
||||
*/
|
||||
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -42,6 +42,9 @@ void Task_Ai(void *argument) {
|
||||
AI_Init(&ai);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
uint32_t last_online_tick = tick;
|
||||
|
||||
while (1) {
|
||||
#ifdef DEBUG
|
||||
task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
|
||||
@ -52,8 +55,9 @@ void Task_Ai(void *argument) {
|
||||
AI_StartReceiving(&ai);
|
||||
if (AI_WaitDmaCplt()) {
|
||||
AI_ParseHost(&ai, &cmd_host);
|
||||
last_online_tick = tick;
|
||||
} else {
|
||||
AI_HandleOffline(&ai, &cmd_host);
|
||||
if (tick - last_online_tick > 300) AI_HandleOffline(&ai, &cmd_host);
|
||||
}
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
|
||||
|
||||
@ -32,10 +32,10 @@ typedef struct {
|
||||
|
||||
static const char *const CLI_WELCOME_MESSAGE =
|
||||
"\r\n"
|
||||
// " ______ __ _______ __ \r\n"
|
||||
// " | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||
// " | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||
// " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||
" ______ __ _______ __ \r\n"
|
||||
" | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||
" | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||
" |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||
" -------------------------------------------------------------------\r\n"
|
||||
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
|
||||
"\r\n";
|
||||
|
||||
Loading…
Reference in New Issue
Block a user