Compare commits
3 Commits
5de502c448
...
fe1c02b130
| Author | SHA1 | Date | |
|---|---|---|---|
| fe1c02b130 | |||
| 6cbf728f10 | |||
| 1488098d60 |
@ -145,23 +145,24 @@ Mcu.Pin24=PA4
|
||||
Mcu.Pin25=PC4
|
||||
Mcu.Pin26=PC5
|
||||
Mcu.Pin27=PE9
|
||||
Mcu.Pin28=PA7
|
||||
Mcu.Pin29=PB0
|
||||
Mcu.Pin28=PE11
|
||||
Mcu.Pin29=PA7
|
||||
Mcu.Pin3=PB3
|
||||
Mcu.Pin30=VP_CRC_VS_CRC
|
||||
Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin32=VP_SYS_VS_Systick
|
||||
Mcu.Pin33=VP_TIM4_VS_ClockSourceINT
|
||||
Mcu.Pin34=VP_TIM7_VS_ClockSourceINT
|
||||
Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
|
||||
Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
|
||||
Mcu.Pin30=PB0
|
||||
Mcu.Pin31=VP_CRC_VS_CRC
|
||||
Mcu.Pin32=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin33=VP_SYS_VS_Systick
|
||||
Mcu.Pin34=VP_TIM4_VS_ClockSourceINT
|
||||
Mcu.Pin35=VP_TIM7_VS_ClockSourceINT
|
||||
Mcu.Pin36=VP_TIM10_VS_ClockSourceINT
|
||||
Mcu.Pin37=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
|
||||
Mcu.Pin4=PA14
|
||||
Mcu.Pin5=PA13
|
||||
Mcu.Pin6=PB7
|
||||
Mcu.Pin7=PB6
|
||||
Mcu.Pin8=PD0
|
||||
Mcu.Pin9=PC11
|
||||
Mcu.PinsNb=37
|
||||
Mcu.PinsNb=38
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407IGHx
|
||||
@ -272,9 +273,14 @@ PD14.GPIOParameters=GPIO_Label
|
||||
PD14.GPIO_Label=Buzzer
|
||||
PD14.Locked=true
|
||||
PD14.Signal=S_TIM4_CH3
|
||||
PE11.GPIOParameters=PinState,GPIO_PuPd
|
||||
PE11.GPIO_PuPd=GPIO_PULLDOWN
|
||||
PE11.Locked=true
|
||||
PE11.PinState=GPIO_PIN_SET
|
||||
PE11.Signal=GPIO_Output
|
||||
PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
|
||||
PE9.GPIO_Label=FlagForUpper
|
||||
PE9.GPIO_PuPd=GPIO_PULLUP
|
||||
PE9.GPIO_PuPd=GPIO_PULLDOWN
|
||||
PE9.Locked=true
|
||||
PE9.PinState=GPIO_PIN_SET
|
||||
PE9.Signal=GPIO_Output
|
||||
|
||||
@ -190,7 +190,7 @@
|
||||
#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */
|
||||
#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */
|
||||
#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */
|
||||
#define USE_HAL_UART_REGISTER_CALLBACKS 1U /* UART register callback disabled */
|
||||
#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */
|
||||
#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */
|
||||
#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */
|
||||
|
||||
|
||||
@ -61,7 +61,7 @@ void MX_GPIO_Init(void)
|
||||
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
|
||||
|
||||
/*Configure GPIO pin Output Level */
|
||||
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(GPIOE, FlagForUpper_Pin|GPIO_PIN_11, GPIO_PIN_SET);
|
||||
|
||||
/*Configure GPIO pin Output Level */
|
||||
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
|
||||
@ -92,12 +92,12 @@ void MX_GPIO_Init(void)
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
|
||||
|
||||
/*Configure GPIO pin : PtPin */
|
||||
GPIO_InitStruct.Pin = FlagForUpper_Pin;
|
||||
/*Configure GPIO pins : PEPin PE11 */
|
||||
GPIO_InitStruct.Pin = FlagForUpper_Pin|GPIO_PIN_11;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
HAL_GPIO_Init(FlagForUpper_GPIO_Port, &GPIO_InitStruct);
|
||||
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
|
||||
|
||||
/*Configure GPIO pin : PtPin */
|
||||
GPIO_InitStruct.Pin = GYRO_CS_Pin;
|
||||
|
||||
@ -79,7 +79,8 @@ int main(void)
|
||||
|
||||
/* MCU Configuration--------------------------------------------------------*/
|
||||
|
||||
/* Reset of all peripherals, Initializes the Flash interface and the Systick. * HAL_Init();
|
||||
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
|
||||
HAL_Init();
|
||||
|
||||
/* USER CODE BEGIN Init */
|
||||
|
||||
@ -94,7 +95,6 @@ int main(void)
|
||||
|
||||
/* Initialize all configured peripherals */
|
||||
MX_GPIO_Init();
|
||||
|
||||
MX_DMA_Init();
|
||||
MX_SPI1_Init();
|
||||
MX_TIM4_Init();
|
||||
|
||||
@ -140,7 +140,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"Horco CMSIS-DAP" -U8626380832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
<Name>-X"Horco CMSIS-DAP" -U4626385832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -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>
|
||||
@ -235,6 +235,11 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>flaggg,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>16</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ddd</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<WatchWindow2>
|
||||
<Ww>
|
||||
@ -469,7 +474,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -741,7 +746,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Drivers/CMSIS</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
Binary file not shown.
@ -309,3 +309,16 @@ uint8_t average(uint8_t arr[], uint8_t n) {
|
||||
}
|
||||
return (float) sum / n;
|
||||
}
|
||||
|
||||
|
||||
bool is_arrived(float target, float current, float mistake) {
|
||||
// 计算当前值与目标值的差值的绝对值
|
||||
float diff = fabs(target - current);
|
||||
|
||||
// 判断是否在误差范围内
|
||||
if (diff <= mistake) {
|
||||
return true; // 在误差范围内
|
||||
} else {
|
||||
return false; // 不在误差范围内
|
||||
}
|
||||
}
|
||||
@ -157,4 +157,7 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);
|
||||
/// @param n 元素数
|
||||
/// @return 平均值
|
||||
uint8_t average(uint8_t arr[], uint8_t n);
|
||||
/*判断是否在误差内*/
|
||||
bool is_arrived(float target, float current, float mistake) ;
|
||||
|
||||
#endif
|
||||
|
||||
@ -7,18 +7,6 @@
|
||||
|
||||
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
|
||||
|
||||
//#ifdef DEBUG
|
||||
|
||||
//ConfigParam_t param_up ={
|
||||
|
||||
//#else
|
||||
//static const ConfigParam_t param_up ={
|
||||
//#endif
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//};
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
@ -32,6 +20,8 @@ static const ConfigParam_t param_chassis ={
|
||||
#endif
|
||||
.up={
|
||||
|
||||
|
||||
/*上层pid参数*/
|
||||
.M2006_angle_param = {
|
||||
.p = 25.0f,
|
||||
.i = 0.0f,
|
||||
@ -46,6 +36,21 @@ static const ConfigParam_t param_chassis ={
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.M3508_angle_param = {
|
||||
.p = 10.0f,
|
||||
.i = 0.0f,
|
||||
.d = 1.5f,
|
||||
.i_limit = 1000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.M3508_speed_param={
|
||||
.p = 5.1f,
|
||||
.i = 0.02f,
|
||||
.d = 3.2f,
|
||||
.i_limit = 200.0f,
|
||||
.out_limit =6000.0f,
|
||||
},
|
||||
|
||||
.UP_GM6020_angle_param={
|
||||
.p = 30.0f,
|
||||
.i = 20.0f,
|
||||
@ -60,115 +65,35 @@ static const ConfigParam_t param_chassis ={
|
||||
.i_limit = 200.0f,
|
||||
.out_limit = 3000.0f
|
||||
},
|
||||
.M3508_speed_param={
|
||||
.p = 15.1f,
|
||||
.i = 0.02f,
|
||||
.d = 3.2f,
|
||||
.i_limit = 200.0f,
|
||||
.out_limit =6000.0f,
|
||||
},
|
||||
.go_param={
|
||||
|
||||
.go_param[0]={
|
||||
.rev = 0,
|
||||
.T=0.1,
|
||||
.W=0.1,
|
||||
.K_P=0.1,
|
||||
.K_W=0.1,
|
||||
}
|
||||
},
|
||||
.go_param[1]={
|
||||
.rev = 0,
|
||||
.T=0.1,
|
||||
.W=0.1,
|
||||
.K_P=0.10f,
|
||||
.K_W=0.1,
|
||||
},
|
||||
/*上层其他参数*/
|
||||
/*运球*/
|
||||
.DribbleConfig_Config = {
|
||||
.m3508_init_angle = 0,
|
||||
.m3508_high_angle = 900,
|
||||
.go2_init_angle = 35,
|
||||
.go2_flip_angle = -130,
|
||||
.flip_timing = 200,
|
||||
.release_threshold = -1.2f,
|
||||
},
|
||||
|
||||
|
||||
},
|
||||
|
||||
// .chassis = {/**/
|
||||
// .C6020pitAngle_param = {
|
||||
// .p = 15.0f,
|
||||
// .i = 0.3f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit = 3000.0f,
|
||||
// },
|
||||
// .C6020pitOmega_param = {
|
||||
// .p =30.0f,
|
||||
// .i =0.3f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit = 3000.0f
|
||||
// },
|
||||
//
|
||||
// .Gimbal_yawAngle_param = {
|
||||
// .p =8.0f,
|
||||
// .i =0.0f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit = 3000.0f
|
||||
// },
|
||||
//
|
||||
// .Gimbal_yawOmega_param = {
|
||||
// .p =18.0f,
|
||||
// .i =0.0f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit = 3000.0f
|
||||
// },
|
||||
//
|
||||
// .Gimbal_pitchAngle_param = {
|
||||
// .p =8.0f,
|
||||
// .i =0.0f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit = 3000.0f
|
||||
// },
|
||||
//
|
||||
// .Gimbal_pitchOmega_param = {
|
||||
// .p =18.0f,
|
||||
// .i =0.0f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit = 3000.0f
|
||||
// },
|
||||
// .AngleCor_param = {
|
||||
// .p =0.8f,
|
||||
// .i =0.0f,
|
||||
// .d =1.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit =5000.0f,
|
||||
// },
|
||||
//
|
||||
// .OmegaCor_param = {
|
||||
// .p =23.5f,
|
||||
// .i =0.0f,
|
||||
// .d =0.05f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit =5000.0f,
|
||||
// },
|
||||
//
|
||||
// .ImuCor_param = {
|
||||
// .p =95.0f,
|
||||
// .i =0.0f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit =200.0f,
|
||||
// },
|
||||
//
|
||||
// .DisCamera_param = {
|
||||
// .p =80.0f,
|
||||
// .i =0.1f,
|
||||
// .d =0.0f,
|
||||
// .i_limit = 0.0f,
|
||||
// .out_limit =5000.0f,
|
||||
// },
|
||||
|
||||
// .M3508_param = {
|
||||
// .p = 15.1f,
|
||||
// .i = 0.02f,
|
||||
// .d = 3.2f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit =6000.0f,
|
||||
// }
|
||||
//
|
||||
|
||||
// },
|
||||
|
||||
|
||||
.can = {
|
||||
.pitch6020 = BSP_CAN_1,
|
||||
.motor3508 = BSP_CAN_1,
|
||||
|
||||
175
User/Module/up.c
175
User/Module/up.c
@ -11,11 +11,26 @@
|
||||
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度
|
||||
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
|
||||
|
||||
/*投球*/
|
||||
#define M2006_INIT_ANGLE (-120) //初始和发射
|
||||
#define GO1_INIT_POSITION (0) //go初始
|
||||
|
||||
#define M2006_TRIGGER (0) //扳机
|
||||
#define GO_POSITION_TRIGGER (-300) //go发射控制值
|
||||
#define GO_POSITION_PITCH_FD (-4.8f) //反馈检测
|
||||
|
||||
|
||||
/*运球*/
|
||||
|
||||
|
||||
|
||||
#define M3508_INIT_ANGLE (0) //3508
|
||||
#define GO2_INIT_ANGLE (0) //go2的初始角度
|
||||
#define M3508_HIGH_ANGLE (900) //3508，升起角度
|
||||
#define GO2_Flip_timing (200) // go的翻转时机，以3508角度反馈值为准
|
||||
#define GO2_Flip_ANGLE (-160) //go2翻转角度
|
||||
#define BALL_REL_TIME (-1.2) //球放开时机，以go的反馈值为准
|
||||
// 定义继电器控制
|
||||
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
|
||||
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
|
||||
|
||||
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
{
|
||||
@ -37,14 +52,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
|
||||
|
||||
for(int i=0;i<2;i++){ //go初始位置设置为0
|
||||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W );
|
||||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );
|
||||
}
|
||||
|
||||
/**/
|
||||
|
||||
u->state .Dribble_flag =Not_started_dri;
|
||||
u->state. Pitch_flag=Not_started_Pit;
|
||||
u->state .last_state = Not_started_dri;
|
||||
// 初始化状态机
|
||||
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机
|
||||
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
|
||||
u->DribbleContext .DribbleState = STATE_GRAB_BALL; //状态更新,开始夹球
|
||||
u->DribbleContext .is_initialized = 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -186,7 +206,7 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
|
||||
int8_t GO_SendData(UP_t *u,int id,float pos)
|
||||
{
|
||||
|
||||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,AngleChange(RADIAN,pos),u->param->go_param .K_P ,u->param->go_param .K_W );
|
||||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id] .T ,u->param->go_param[id] .W ,AngleChange(RADIAN,pos),u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
|
||||
|
||||
}
|
||||
|
||||
@ -250,7 +270,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
{
|
||||
|
||||
if(u ==NULL) return 0;
|
||||
if(u ==NULL) return 0;
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
@ -261,29 +280,40 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
case Normal :
|
||||
|
||||
u->state .Pitch_flag =Not_started_Pit;
|
||||
u->state .last_state = Not_started_Pit;
|
||||
u->Oper_control_state .Pitch_flag =Not_started_Pit;
|
||||
u->Oper_control_state .last_state = Not_started_Pit;
|
||||
|
||||
u->motor_target .go_shoot =0;
|
||||
u->motor_target .M2006_angle =-140;
|
||||
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
|
||||
u->motor_target .go_spin =u->DribbleContext.DribbleConfig .go2_init_angle ;
|
||||
u->motor_target .M3508_angle =0;
|
||||
u->DribbleContext .DribbleState = STATE_GRAB_BALL; //状态更新,开始夹球
|
||||
|
||||
RELAY1_TOGGLE (1);//接球，1开0关
|
||||
RELAY2_TOGGLE (1);//夹球，0关1开
|
||||
|
||||
|
||||
break;
|
||||
|
||||
case Pitch_pull :
|
||||
if(u->state .last_state == Not_started_Pit)
|
||||
if(u->Oper_control_state .last_state == Not_started_Pit)
|
||||
{
|
||||
|
||||
u->motor_target .go_shoot =-300;
|
||||
|
||||
u->motor_target .go_shoot =-2050;
|
||||
u->motor_target .M2006_angle =-140;
|
||||
|
||||
if(u->motorfeedback .GO_motor_info[0]->Pos < (-4.8)) //到达位置后再扣扳机
|
||||
if(u->motorfeedback .GO_motor_info[0]->Pos < (-35.20)) //到达位置后再扣扳机
|
||||
u->motor_target .go_shoot =GO_POSITION_TRIGGER;
|
||||
|
||||
if(u->motorfeedback .GO_motor_info[0]->Pos < (GO_POSITION_PITCH_FD)) //到达位置后再扣扳机
|
||||
{
|
||||
u->motor_target .M2006_angle =0;
|
||||
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
|
||||
if(u->motorfeedback .M2006.total_angle>-5)
|
||||
{//避免没勾上就拉
|
||||
u->motor_target .go_shoot =0;
|
||||
u->state .Pitch_flag = Launch_Ready ;
|
||||
u->state .last_state = Launch_Ready;
|
||||
u->motor_target .go_shoot =GO1_INIT_POSITION;
|
||||
u->Oper_control_state .Pitch_flag = Launch_Ready ;
|
||||
u->Oper_control_state .last_state = Launch_Ready;
|
||||
|
||||
}
|
||||
|
||||
@ -292,35 +322,46 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
break ;
|
||||
case Pitch_shoot :
|
||||
if (u->state .last_state == Launch_Ready)
|
||||
if (u->Oper_control_state .last_state == Launch_Ready)
|
||||
{
|
||||
|
||||
u->motor_target .M2006_angle =-140;
|
||||
u->state .Pitch_flag = Done_Pit ;
|
||||
u->state .last_state = Done_Pit;
|
||||
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
|
||||
u->Oper_control_state .Pitch_flag = Done_Pit ;
|
||||
u->Oper_control_state .last_state = Done_Pit;
|
||||
|
||||
}
|
||||
{
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
case Dribble:
|
||||
{
|
||||
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
|
||||
Dribble_Process(u,out);
|
||||
|
||||
}break ;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case MID_NAVI :
|
||||
{
|
||||
}
|
||||
break ;
|
||||
|
||||
case PICK_t :
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
@ -334,5 +375,67 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
}
|
||||
|
||||
|
||||
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
|
||||
{
|
||||
|
||||
switch (u->DribbleContext.DribbleState) {
|
||||
case STATE_GRAB_BALL://开始
|
||||
|
||||
RELAY2_TOGGLE (0);//夹球
|
||||
|
||||
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
|
||||
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
|
||||
|
||||
|
||||
if((u->motorfeedback .M3508 .total_angle >400)) {
|
||||
RELAY1_TOGGLE (0);
|
||||
|
||||
|
||||
if((u->motorfeedback.GO_motor_info[1]->angle) <-53){
|
||||
|
||||
u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case STATE_RELEASE_BALL:
|
||||
RELAY2_TOGGLE (1);//松球
|
||||
|
||||
|
||||
|
||||
|
||||
if((u->motorfeedback.GO_motor_info[1]->angle) <-60){
|
||||
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_init_angle ; //恢复go2位置
|
||||
u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE_CATCH_PREP:
|
||||
if((u->motorfeedback.GO_motor_info[1]->angle) > -0){
|
||||
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; //当go2到初始位置，3508降
|
||||
|
||||
RELAY1_TOGGLE (1);//接球
|
||||
}
|
||||
if(u->motorfeedback .M3508 .total_angle <5){
|
||||
u->DribbleContext .DribbleState = STATE_CATCH_DONE;
|
||||
}
|
||||
break;
|
||||
|
||||
case STATE_CATCH_DONE:
|
||||
RELAY2_TOGGLE (1);//松球
|
||||
RELAY1_TOGGLE (0);
|
||||
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -14,6 +14,37 @@
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "bsp_usart.h"
|
||||
|
||||
/*
|
||||
状态机
|
||||
|
||||
状态结构体，触发标志位结构体
|
||||
|
||||
配置层，-->config.c
|
||||
|
|
||||
中间层，-->up.h,统一UP_t
|
||||
|
|
||||
运行函数，switch(状态) 运行相应函数
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
@ -25,25 +56,42 @@ typedef enum
|
||||
|
||||
}Pitch_flag_t;
|
||||
|
||||
typedef enum{
|
||||
/*运球*/
|
||||
typedef enum {
|
||||
STATE_GRAB_BALL, // 夹球升起阶段
|
||||
STATE_RELEASE_BALL, // 释放球体
|
||||
STATE_CATCH_PREP, // 接球准备
|
||||
STATE_CATCH_BALL, // 接球动作
|
||||
STATE_CATCH_DONE //完成
|
||||
} DribbleState_t;
|
||||
|
||||
Not_started_dri=0,//未开始
|
||||
No_ball =1, //抓上无球
|
||||
Have_ball_F=2, //刚开始有球
|
||||
Have_ball_S=3, //中途有球
|
||||
Done_dri =4 //已完成
|
||||
/* 参数配置结构体 */
|
||||
typedef struct {
|
||||
fp32 m3508_init_angle; // 3508初始角度
|
||||
fp32 m3508_high_angle; // 3508升起角度
|
||||
fp32 go2_init_angle; // GO2初始角度
|
||||
fp32 go2_flip_angle; // GO2翻转角度
|
||||
fp32 flip_timing; // 翻转触发时机
|
||||
fp32 release_threshold; // 释放球
|
||||
} DribbleConfig_t;
|
||||
|
||||
}Dribble_flag_t;
|
||||
/* 状态机上下文 */
|
||||
typedef struct {
|
||||
DribbleState_t DribbleState;
|
||||
DribbleConfig_t DribbleConfig;
|
||||
|
||||
uint8_t is_initialized;
|
||||
|
||||
} DribbleContext_t;
|
||||
|
||||
/*运行控制中的控制*/
|
||||
|
||||
typedef struct{
|
||||
|
||||
/*投球过程*/
|
||||
Pitch_flag_t Pitch_flag;
|
||||
/*运球过程*/
|
||||
Dribble_flag_t Dribble_flag;
|
||||
|
||||
int last_state;
|
||||
char last_state;
|
||||
|
||||
} Oper_control_state_t;
|
||||
|
||||
@ -77,7 +125,6 @@ typedef struct {
|
||||
typedef enum {
|
||||
M2006 = 1,
|
||||
M3508
|
||||
|
||||
} MotorType_t;
|
||||
|
||||
|
||||
@ -97,7 +144,9 @@ typedef struct
|
||||
pid_param_t M3508_speed_param;
|
||||
pid_param_t M3508_angle_param;
|
||||
|
||||
GO_param_t go_param;
|
||||
GO_param_t go_param[2];
|
||||
|
||||
DribbleConfig_t DribbleConfig_Config;
|
||||
|
||||
}UP_Param_t;
|
||||
|
||||
@ -119,11 +168,22 @@ typedef struct{
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
|
||||
int state;
|
||||
|
||||
|
||||
|
||||
/*运球过程*/
|
||||
DribbleContext_t DribbleContext;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
UP_Imu_t pos088;
|
||||
|
||||
/*控制及状态*/
|
||||
CMD_t *cmd;
|
||||
Oper_control_state_t state;//上层机构的运行状态
|
||||
Oper_control_state_t Oper_control_state;//上层机构的运行状态
|
||||
|
||||
struct{
|
||||
fp32 rotor_pit6020ecd;
|
||||
@ -211,6 +271,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
|
||||
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
|
||||
int8_t UP_M3508_speed(UP_t *u,fp32 speed);
|
||||
|
||||
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -165,6 +165,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
|
||||
motor->Temp = Temp;
|
||||
motor->MError = Temp_buffer[12] & 0x7;
|
||||
|
||||
motor ->angle = AngleChange(DEGREE,motor ->Pos);
|
||||
}
|
||||
|
||||
//力位混合控制
|
||||
|
||||
@ -64,11 +64,13 @@ typedef struct {
|
||||
float T;
|
||||
float W;
|
||||
float Pos;
|
||||
float angle;
|
||||
|
||||
int footForce;
|
||||
uint8_t buffer[17];
|
||||
uint8_t Rec_buffer[16];
|
||||
ControlData_t motor_send_data;
|
||||
|
||||
}GO_Motorfield;
|
||||
|
||||
|
||||
|
||||
@ -61,9 +61,16 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
|
||||
{
|
||||
|
||||
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
|
||||
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左中，右中，无模式
|
||||
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左中，右上，运球
|
||||
|
||||
if(rc->sw_r ==CMD_SW_MID)
|
||||
{
|
||||
cmd ->CMD_CtrlType =UP_RCcontrol;
|
||||
cmd ->CMD_UP_mode =Normal; //左中，右中，无模式
|
||||
}
|
||||
if(rc->sw_r ==CMD_SW_DOWN)
|
||||
{
|
||||
cmd ->CMD_UP_mode =Dribble; //左中，右上，运球
|
||||
cmd ->CMD_CtrlType =UP_RCcontrol;
|
||||
}
|
||||
}
|
||||
else if(rc->sw_l==CMD_SW_DOWN)
|
||||
{
|
||||
|
||||
@ -34,6 +34,7 @@ static CMD_t up_cmd;
|
||||
float aaa=0;
|
||||
float bbb=0;
|
||||
float CCC=0;
|
||||
float ddd=0;
|
||||
|
||||
|
||||
/**
|
||||
@ -68,7 +69,17 @@ void Task_up(void *argument)
|
||||
//
|
||||
// GO_SendData(&UP, 1,CCC);
|
||||
// GO_SendData(&UP, 0,aaa);
|
||||
|
||||
UP_control(&UP,&UP_CAN_out,&up_cmd);
|
||||
// UP.motor_target .go_shoot =aaa;
|
||||
// UP.motor_target .M2006_angle =bbb ;
|
||||
|
||||
// UP_control(&UP,&UP_CAN_out,&up_cmd);
|
||||
// UP.motor_target .M3508_angle =CCC;
|
||||
// UP.motor_target .go_shoot =aaa;
|
||||
// UP.motor_target .M2006_angle =bbb;
|
||||
// UP.motor_target .go_spin =ddd;
|
||||
|
||||
ALL_Motor_Control(&UP,&UP_CAN_out);
|
||||
|
||||
osDelay(1);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user