好用的,哨兵,左头加底盘
This commit is contained in:
parent
76fde06500
commit
b7415fd2c9
@ -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>
|
||||
@ -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>
|
||||
@ -387,6 +387,11 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ref</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -1786,7 +1791,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.
16609
MDK-ARM/DevC/DevC.hex
16609
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
@ -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;
|
||||
|
||||
|
@ -155,7 +155,7 @@ int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
|
||||
int16_t motor1, motor2, motor3, motor4;
|
||||
int16_t yaw_motor, pit_motor;
|
||||
int16_t fric1_motor, fric2_motor, trig_motor;
|
||||
int16_t fric1_motor, fric2_motor, fric3_motor, trig_motor;
|
||||
switch (group) {
|
||||
case CAN_MOTOR_GROUT_CHASSIS:
|
||||
motor1 =
|
||||
@ -219,6 +219,8 @@ int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
(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);
|
||||
|
||||
@ -231,10 +233,10 @@ int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
|
||||
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[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,
|
||||
@ -264,6 +266,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
|
||||
|
||||
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);
|
||||
|
@ -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,7 +73,8 @@ 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 */
|
||||
@ -110,6 +112,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 +150,7 @@ typedef union {
|
||||
struct {
|
||||
float fric1;
|
||||
float fric2;
|
||||
float fric3;
|
||||
float trig;
|
||||
} named;
|
||||
} CAN_ShootOutput_t;
|
||||
@ -200,6 +204,7 @@ typedef union {
|
||||
struct {
|
||||
CAN_MotorFeedback_t fric1;
|
||||
CAN_MotorFeedback_t fric2;
|
||||
CAN_MotorFeedback_t fric3;
|
||||
CAN_MotorFeedback_t trig;
|
||||
} named;
|
||||
} CAN_ShootMotor_t;
|
||||
|
@ -149,6 +149,8 @@ static const Config_RobotParam_t param_default = {
|
||||
.yaw = false,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -294,6 +296,8 @@ static const Config_RobotParam_t param_hero = {
|
||||
.yaw = true,
|
||||
.pit = true,
|
||||
},
|
||||
|
||||
.pit_ctrl_reverse = false,
|
||||
}, /* gimbal */
|
||||
|
||||
.shoot = { /* 射击模块参数 */
|
||||
@ -345,6 +349,386 @@ static const Config_RobotParam_t param_hero = {
|
||||
}, /* 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 = 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_gimbal */
|
||||
|
||||
/* static const Config_RobotParam_t param_xxx; */
|
||||
|
||||
static const Config_PilotCfg_t cfg_qs = {
|
||||
@ -411,6 +795,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},
|
||||
|
@ -48,7 +48,7 @@ typedef struct {
|
||||
|
||||
AHRS_Eulr_t mech_zero; /* 机械零点 */
|
||||
float gimbal_limit; /* 云台pitch轴软件限位最高点 */
|
||||
|
||||
|
||||
} Config_t;
|
||||
|
||||
/* 机器人参数和对应字符串的映射 */
|
||||
|
@ -130,22 +130,36 @@ 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);
|
||||
|
||||
/* 处理pitch控制命令,软件限位 */
|
||||
// const float delta_max =
|
||||
// CircleError(g->limit.max,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.imu.pit),
|
||||
// M_2PI);
|
||||
// const float delta_min =
|
||||
// CircleError(g->limit.min,
|
||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
// g->feedback.eulr.imu.pit),
|
||||
// M_2PI);
|
||||
const float delta_max =
|
||||
CircleError(g->limit.max,
|
||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||
g->feedback.eulr.imu.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.imu.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;
|
||||
@ -168,12 +182,16 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||
g->feedback.gyro.z, 0.f, g->dt);
|
||||
|
||||
// pit_omega_set_point =
|
||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
// g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||
pit_omega_set_point =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||
g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||
|
||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||
g->feedback.gyro.x, 0.f, g->dt);
|
||||
0.0, 0.f, g->dt);
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_RELATIVE:
|
||||
|
@ -60,6 +60,8 @@ typedef struct {
|
||||
bool pit;
|
||||
} reverse;
|
||||
|
||||
bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
|
||||
|
||||
} Gimbal_Params_t;
|
||||
|
||||
/* 软件限位 */
|
||||
|
@ -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]);
|
||||
|
@ -29,6 +29,7 @@ extern "C" {
|
||||
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, /* 总共的动作器数量 */
|
||||
};
|
||||
@ -106,20 +107,20 @@ typedef struct {
|
||||
|
||||
/* 反馈信息 */
|
||||
struct {
|
||||
float fric_rpm[2]; /* 摩擦轮电机转速,单位:RPM */
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速,单位:RPM */
|
||||
float trig_motor_angle; /* 拨弹电机角度,单位:弧度 */
|
||||
float trig_angle; /* 拨弹转盘角度,单位:弧度 */
|
||||
} feedback;
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
float fric_rpm[2]; /* 摩擦轮电机转速,单位:RPM */
|
||||
float fric_rpm[3]; /* 摩擦轮电机转速,单位:RPM */
|
||||
float trig_angle; /* 拨弹电机角度,单位:弧度 */
|
||||
} setpoint;
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t fric[2]; /* 控制摩擦轮 */
|
||||
KPID_t fric[3]; /* 控制摩擦轮 */
|
||||
KPID_t trig; /* 控制拨弹电机 */
|
||||
} pid;
|
||||
|
||||
@ -127,13 +128,13 @@ typedef struct {
|
||||
struct {
|
||||
/* 反馈值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} in;
|
||||
|
||||
/* 输出值滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t fric[3]; /* 过滤摩擦轮 */
|
||||
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
|
||||
} out;
|
||||
} filter;
|
||||
|
Loading…
Reference in New Issue
Block a user