Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
87f56647b1 | ||
| 78712e5918 | |||
| 9fe34c0bf8 |
5
.vscode/settings.json
vendored
Normal file
5
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,5 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"functional": "cpp"
|
||||
}
|
||||
}
|
||||
@ -10,7 +10,7 @@
|
||||
<aExt>*.s*; *.src; *.a*</aExt>
|
||||
<oExt>*.obj; *.o</oExt>
|
||||
<lExt>*.lib</lExt>
|
||||
<tExt>*.txt; *.h; *.inc</tExt>
|
||||
<tExt>*.txt; *.h; *.inc; *.md</tExt>
|
||||
<pExt>*.plm</pExt>
|
||||
<CppX>*.cpp</CppX>
|
||||
<nMigrate>0</nMigrate>
|
||||
@ -360,7 +360,7 @@
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>chassis</ItemText>
|
||||
<ItemText>param_default</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
@ -370,24 +370,32 @@
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>gimbal_out</ItemText>
|
||||
<ItemText>shoot</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>for_chassis</ItemText>
|
||||
<ItemText>param_hero</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>task_runtime</ItemText>
|
||||
<ItemText>ref</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ref</ItemText>
|
||||
<ItemText>for_chassis</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>
|
||||
@ -1250,7 +1258,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/bsp</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1474,7 +1482,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/component</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1786,7 +1794,7 @@
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>105</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\module\shoot.c</PathWithFileName>
|
||||
|
||||
@ -16,7 +16,7 @@
|
||||
<TargetCommonOption>
|
||||
<Device>STM32F407IGHx</Device>
|
||||
<Vendor>STMicroelectronics</Vendor>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.14.0</PackID>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
|
||||
<PackURL>http://www.keil.com/pack/</PackURL>
|
||||
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu>
|
||||
<FlashUtilSpec></FlashUtilSpec>
|
||||
@ -1726,13 +1726,13 @@
|
||||
<TargetName>Debug</TargetName>
|
||||
<ToolsetNumber>0x4</ToolsetNumber>
|
||||
<ToolsetName>ARM-ADS</ToolsetName>
|
||||
<pCCUsed>6140000::V6.14::ARMCLANG</pCCUsed>
|
||||
<pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
|
||||
<uAC6>1</uAC6>
|
||||
<TargetOption>
|
||||
<TargetCommonOption>
|
||||
<Device>STM32F407IGHx</Device>
|
||||
<Vendor>STMicroelectronics</Vendor>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.14.0</PackID>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
|
||||
<PackURL>http://www.keil.com/pack/</PackURL>
|
||||
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu>
|
||||
<FlashUtilSpec></FlashUtilSpec>
|
||||
@ -3443,11 +3443,6 @@
|
||||
<Layers>
|
||||
<Layer>
|
||||
<LayName>DevC</LayName>
|
||||
<LayDesc></LayDesc>
|
||||
<LayUrl></LayUrl>
|
||||
<LayKeys></LayKeys>
|
||||
<LayCat></LayCat>
|
||||
<LayLic></LayLic>
|
||||
<LayTarg>0</LayTarg>
|
||||
<LayPrjMark>1</LayPrjMark>
|
||||
</Layer>
|
||||
|
||||
Binary file not shown.
16590
MDK-ARM/DevC/DevC.hex
16590
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
@ -86,7 +86,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;
|
||||
|
||||
|
||||
@ -68,7 +68,8 @@ 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]);
|
||||
|
||||
@ -79,36 +80,44 @@ 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);
|
||||
|
||||
@ -150,158 +159,175 @@ 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, 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);
|
||||
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);
|
||||
|
||||
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),
|
||||
&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.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;
|
||||
|
||||
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);
|
||||
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)((trig_motor >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(trig_motor & 0xFF);
|
||||
raw_tx.tx_data[6] = 0;
|
||||
raw_tx.tx_data[7] = 0;
|
||||
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));
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
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;
|
||||
}
|
||||
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_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);
|
||||
@ -320,7 +346,8 @@ 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;
|
||||
}
|
||||
|
||||
@ -22,9 +22,10 @@ extern "C" {
|
||||
#define CAN_MOTOR_GIMBAL_PIT_RECV (1 << 5)
|
||||
#define CAN_MOTOR_SHOOT_FRIC1_RECV (1 << 6)
|
||||
#define CAN_MOTOR_SHOOT_FRIC2_RECV (1 << 7)
|
||||
#define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 8)
|
||||
#define CAN_MOTOR_CAP_RECV (1 << 9)
|
||||
#define CAN_TOF_RECV (1 << 10)
|
||||
#define CAN_MOTOR_SHOOT_FRIC3_RECV (1 << 8)
|
||||
#define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 9)
|
||||
#define CAN_MOTOR_CAP_RECV (1 << 10)
|
||||
#define CAN_TOF_RECV (1 << 11)
|
||||
|
||||
#define CAN_REC_CHASSIS_FINISHED \
|
||||
(CAN_MOTOR_CHASSIS_1_RECV | CAN_MOTOR_CHASSIS_2_RECV | \
|
||||
@ -33,7 +34,7 @@ extern "C" {
|
||||
(CAN_MOTOR_GIMBAL_YAW_RECV | CAN_MOTOR_GIMBAL_PIT_RECV)
|
||||
#define CAN_REC_SHOOT_FINISHED \
|
||||
(CAN_MOTOR_SHOOT_FRIC1_RECV | CAN_MOTOR_SHOOT_FRIC2_RECV | \
|
||||
CAN_MOTOR_SHOOT_TRIG_RECV)
|
||||
CAN_MOTOR_SHOOT_FRIC3_RECV |CAN_MOTOR_SHOOT_TRIG_RECV)
|
||||
#define CAN_REC_CAP_FINISHED CAN_MOTOR_CAP_RECV
|
||||
#define CAN_REC_TOF_FINISHED CAN_TOF_RECV
|
||||
|
||||
@ -72,16 +73,23 @@ typedef enum {
|
||||
|
||||
CAN_M3508_FRIC1_ID = 0x205, /* 5 */
|
||||
CAN_M3508_FRIC2_ID = 0x206, /* 6 */
|
||||
CAN_M2006_TRIG_ID = 0x207, /* 7 */
|
||||
CAN_M3508_FRIC3_ID = 0x207, /* 7 */
|
||||
CAN_M2006_TRIG_ID = 0x208, /* 8 */
|
||||
|
||||
CAN_GM6020_YAW_ID = 0x209, /* 5 */
|
||||
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;
|
||||
BSP_CAN_t gimbal;
|
||||
Gimbal_CAN_t gimbal;
|
||||
BSP_CAN_t shoot;
|
||||
BSP_CAN_t trig;
|
||||
BSP_CAN_t cap;
|
||||
} CAN_Params_t;
|
||||
|
||||
@ -110,6 +118,7 @@ enum CAN_MotorGimbal_e {
|
||||
enum CAN_MotorShoot_e {
|
||||
CAN_MOTOR_SHOOT_FRIC1 = 0,
|
||||
CAN_MOTOR_SHOOT_FRIC2,
|
||||
CAN_MOTOR_SHOOT_FRIC3,
|
||||
CAN_MOTOR_SHOOT_TRIG,
|
||||
CAN_MOTORSHOOT_NUM,
|
||||
};
|
||||
@ -147,6 +156,7 @@ typedef union {
|
||||
struct {
|
||||
float fric1;
|
||||
float fric2;
|
||||
float fric3;
|
||||
float trig;
|
||||
} named;
|
||||
} CAN_ShootOutput_t;
|
||||
@ -200,6 +210,7 @@ typedef union {
|
||||
struct {
|
||||
CAN_MotorFeedback_t fric1;
|
||||
CAN_MotorFeedback_t fric2;
|
||||
CAN_MotorFeedback_t fric3;
|
||||
CAN_MotorFeedback_t trig;
|
||||
} named;
|
||||
} CAN_ShootMotor_t;
|
||||
|
||||
@ -194,13 +194,19 @@ static const Config_RobotParam_t param_default = {
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.gimbal.yaw = BSP_CAN_2,
|
||||
.gimbal.pitch = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.trig = BSP_CAN_2,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_default */
|
||||
|
||||
#ifdef DEBUG
|
||||
Config_RobotParam_t param_hero = {
|
||||
#else
|
||||
static const Config_RobotParam_t param_hero = {
|
||||
#endif
|
||||
.model = ROBOT_MODEL_HERO,
|
||||
|
||||
.chassis = { /* 底盘模块参数 */
|
||||
@ -234,7 +240,7 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = true,
|
||||
.yaw = false,
|
||||
},
|
||||
}, /* chassis */
|
||||
|
||||
@ -242,20 +248,20 @@ static const Config_RobotParam_t param_hero = {
|
||||
.pid = {
|
||||
{
|
||||
/* GIMBAL_PID_YAW_OMEGA_IDX */
|
||||
.k = 0.45f,
|
||||
.k = 0.24f,
|
||||
.p = 1.0f,
|
||||
.i = 6.0f,
|
||||
.d = 0.0008f,
|
||||
.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 = 20.0f,
|
||||
.k = 10.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.d = 0.05f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
@ -272,9 +278,9 @@ static const Config_RobotParam_t param_hero = {
|
||||
.range = -1.0f,
|
||||
}, {
|
||||
/* GIMBAL_PID_PIT_ANGLE_IDX */
|
||||
.k = 12.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.k = 2.0f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
@ -283,7 +289,7 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
}, /* pid */
|
||||
|
||||
.pitch_travel_rad = 1.07685447f,
|
||||
.pitch_travel_rad = 0.9685447f,
|
||||
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -291,8 +297,8 @@ static const Config_RobotParam_t param_hero = {
|
||||
},
|
||||
|
||||
.reverse = {
|
||||
.yaw = true,
|
||||
.pit = true,
|
||||
.yaw = false,
|
||||
.pit = false,
|
||||
},
|
||||
}, /* gimbal */
|
||||
|
||||
@ -327,8 +333,8 @@ static const Config_RobotParam_t param_hero = {
|
||||
.trig = -1.0f,
|
||||
},
|
||||
},
|
||||
.num_trig_tooth = 6.0f,
|
||||
.trig_gear_ratio = 3591.0f / 187.0f,
|
||||
.num_trig_tooth = 5.0f,
|
||||
.trig_gear_ratio = 19.0f,
|
||||
.fric_radius = 0.03f,
|
||||
.cover_open_duty = 0.125f,
|
||||
.cover_close_duty = 0.075f,
|
||||
@ -339,8 +345,10 @@ static const Config_RobotParam_t param_hero = {
|
||||
|
||||
.can = {
|
||||
.chassis = BSP_CAN_1,
|
||||
.gimbal = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.gimbal.yaw = BSP_CAN_1,
|
||||
.gimbal.pitch = BSP_CAN_2,
|
||||
.shoot = BSP_CAN_2,
|
||||
.trig = BSP_CAN_1,
|
||||
.cap = BSP_CAN_1,
|
||||
}, /* can */
|
||||
}; /* param_hero */
|
||||
@ -430,17 +438,21 @@ 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;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -448,7 +460,8 @@ 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));
|
||||
@ -461,10 +474,14 @@ 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;
|
||||
}
|
||||
}
|
||||
@ -477,20 +494,26 @@ 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;
|
||||
}
|
||||
|
||||
@ -35,7 +35,7 @@ static int8_t Shoot_SetMode(Shoot_t *s, CMD_ShootMode_t mode) {
|
||||
if (mode == s->mode) return SHOOT_OK;
|
||||
|
||||
/* 切换模式后重置PID和滤波器 */
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
PID_Reset(s->pid.fric + i);
|
||||
LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f);
|
||||
LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f);
|
||||
@ -117,7 +117,7 @@ int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq) {
|
||||
s->param = param; /* 初始化参数 */
|
||||
s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */
|
||||
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
/* PI控制器初始化PID */
|
||||
PID_Init(s->pid.fric + i, KPID_MODE_NO_D, target_freq,
|
||||
&(param->fric_pid_param));
|
||||
@ -154,7 +154,7 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can) {
|
||||
if (s == NULL) return -1;
|
||||
if (can == NULL) return -1;
|
||||
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
s->feedback.fric_rpm[i] = can->motor.shoot.as_array[i].rotor_speed;
|
||||
}
|
||||
|
||||
@ -245,20 +245,26 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
}
|
||||
|
||||
/* 计算摩擦轮转速的目标值 */
|
||||
s->setpoint.fric_rpm[1] =
|
||||
CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
|
||||
(s->param->model == SHOOT_MODEL_17MM));
|
||||
s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
|
||||
// s->setpoint.fric_rpm[1] =
|
||||
// CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
|
||||
// (s->param->model == SHOOT_MODEL_17MM));
|
||||
// s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
|
||||
|
||||
float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
|
||||
(s->param->model == SHOOT_MODEL_17MM));
|
||||
s->setpoint.fric_rpm[0] = -fric_rpm;
|
||||
s->setpoint.fric_rpm[1] = fric_rpm;
|
||||
s->setpoint.fric_rpm[2] = -fric_rpm;
|
||||
|
||||
/* 计算拨弹电机位置的目标值 */
|
||||
if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&
|
||||
(s_cmd->fire)) {
|
||||
/* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
|
||||
if (s_cmd->reverse_trig) { /* 反转拨弹 */
|
||||
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
|
||||
CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
|
||||
M_2PI);
|
||||
} else {
|
||||
CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
|
||||
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
|
||||
M_2PI);
|
||||
s->fire_ctrl.shooted++;
|
||||
s->fire_ctrl.last_shoot = now;
|
||||
@ -285,7 +291,7 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
s->out[SHOOT_ACTR_TRIG_IDX] = LowPassFilter2p_Apply(
|
||||
&(s->filter.out.trig), s->out[SHOOT_ACTR_TRIG_IDX]);
|
||||
|
||||
for (uint8_t i = 0; i < 2; i++) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
/* 控制摩擦轮 */
|
||||
s->feedback.fric_rpm[i] = LowPassFilter2p_Apply(
|
||||
&(s->filter.in.fric[i]), s->feedback.fric_rpm[i]);
|
||||
|
||||
@ -2,208 +2,210 @@
|
||||
* 射击模组
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* 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 */
|
||||
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/* 用enum组合所有PID，方便访问，配合数组使用 */
|
||||
enum Shoot_Acuator_e {
|
||||
SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
|
||||
SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */
|
||||
SHOOT_ACTR_NUM, /* 总共的动作器数量 */
|
||||
};
|
||||
|
||||
/* 发射机构型号 */
|
||||
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;
|
||||
|
||||
/* 输出 */
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */
|
||||
|
||||
/* 模块通用 */
|
||||
CMD_ShootMode_t mode; /* 射击模式 */
|
||||
|
||||
/* 反馈信息 */
|
||||
struct {
|
||||
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
float trig_angle; /* 拨弹转盘角度，单位：弧度 */
|
||||
} feedback;
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */
|
||||
float trig_angle; /* 拨弹电机角度，单位：弧度 */
|
||||
} setpoint;
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t fric[2]; /* 控制摩擦轮 */
|
||||
KPID_t trig; /* 控制拨弹电机 */
|
||||
} pid;
|
||||
|
||||
/* 过滤器 */
|
||||
struct {
|
||||
/* 反馈值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} in;
|
||||
|
||||
/* 输出值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} out;
|
||||
} filter;
|
||||
|
||||
Shoot_HeatCtrl_t heat_ctrl;
|
||||
Shoot_FireCtrl_t fire_ctrl;
|
||||
|
||||
float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */
|
||||
|
||||
} Shoot_t;
|
||||
|
||||
/* 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 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 out CAN设备射击输出结构体
|
||||
*/
|
||||
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out);
|
||||
|
||||
/**
|
||||
* \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
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* 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 */
|
||||
|
||||
/* 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, /* 总共的动作器数量 */
|
||||
};
|
||||
|
||||
/* 发射机构型号 */
|
||||
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;
|
||||
|
||||
/* 输出 */
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */
|
||||
|
||||
/* 模块通用 */
|
||||
CMD_ShootMode_t mode; /* 射击模式 */
|
||||
|
||||
/* 反馈信息 */
|
||||
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 {
|
||||
KPID_t fric[3]; /* 控制摩擦轮 */
|
||||
KPID_t trig; /* 控制拨弹电机 */
|
||||
} pid;
|
||||
|
||||
/* 过滤器 */
|
||||
struct {
|
||||
/* 反馈值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} in;
|
||||
|
||||
/* 输出值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} out;
|
||||
} filter;
|
||||
|
||||
Shoot_HeatCtrl_t heat_ctrl;
|
||||
Shoot_FireCtrl_t fire_ctrl;
|
||||
|
||||
float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */
|
||||
|
||||
} Shoot_t;
|
||||
|
||||
/* 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 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 out CAN设备射击输出结构体
|
||||
*/
|
||||
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out);
|
||||
|
||||
/**
|
||||
* \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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user