diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h index 2c650e5..de1767c 100644 --- a/Core/Inc/stm32f4xx_it.h +++ b/Core/Inc/stm32f4xx_it.h @@ -69,6 +69,7 @@ void CAN2_RX0_IRQHandler(void); void CAN2_RX1_IRQHandler(void); void OTG_FS_IRQHandler(void); void DMA2_Stream5_IRQHandler(void); +void DMA2_Stream6_IRQHandler(void); void DMA2_Stream7_IRQHandler(void); void USART6_IRQHandler(void); /* USER CODE BEGIN EFP */ diff --git a/Core/Src/dma.c b/Core/Src/dma.c index 4c8e202..16a09df 100644 --- a/Core/Src/dma.c +++ b/Core/Src/dma.c @@ -59,6 +59,9 @@ void MX_DMA_Init(void) /* DMA2_Stream5_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0); HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn); + /* DMA2_Stream6_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn); /* DMA2_Stream7_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0); HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn); diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 05c6ecb..c681352 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -69,6 +69,7 @@ extern DMA_HandleTypeDef hdma_usart1_tx; extern DMA_HandleTypeDef hdma_usart1_rx; extern DMA_HandleTypeDef hdma_usart3_rx; extern DMA_HandleTypeDef hdma_usart6_rx; +extern DMA_HandleTypeDef hdma_usart6_tx; extern UART_HandleTypeDef huart1; extern UART_HandleTypeDef huart6; /* USER CODE BEGIN EV */ @@ -418,6 +419,20 @@ void DMA2_Stream5_IRQHandler(void) /* USER CODE END DMA2_Stream5_IRQn 1 */ } +/** + * @brief This function handles DMA2 stream6 global interrupt. + */ +void DMA2_Stream6_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream6_IRQn 0 */ + + /* USER CODE END DMA2_Stream6_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart6_tx); + /* USER CODE BEGIN DMA2_Stream6_IRQn 1 */ + + /* USER CODE END DMA2_Stream6_IRQn 1 */ +} + /** * @brief This function handles DMA2 stream7 global interrupt. */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c index 82d84f0..f133fad 100644 --- a/Core/Src/usart.c +++ b/Core/Src/usart.c @@ -31,6 +31,7 @@ DMA_HandleTypeDef hdma_usart1_tx; DMA_HandleTypeDef hdma_usart1_rx; DMA_HandleTypeDef hdma_usart3_rx; DMA_HandleTypeDef hdma_usart6_rx; +DMA_HandleTypeDef hdma_usart6_tx; /* USART1 init function */ @@ -103,7 +104,7 @@ void MX_USART6_UART_Init(void) /* USER CODE END USART6_Init 1 */ huart6.Instance = USART6; - huart6.Init.BaudRate = 115200; + huart6.Init.BaudRate = 4000000; huart6.Init.WordLength = UART_WORDLENGTH_8B; huart6.Init.StopBits = UART_STOPBITS_1; huart6.Init.Parity = UART_PARITY_NONE; @@ -278,6 +279,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart6_rx); + /* USART6_TX Init */ + hdma_usart6_tx.Instance = DMA2_Stream6; + hdma_usart6_tx.Init.Channel = DMA_CHANNEL_5; + hdma_usart6_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; + hdma_usart6_tx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_usart6_tx.Init.MemInc = DMA_MINC_ENABLE; + hdma_usart6_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_usart6_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_usart6_tx.Init.Mode = DMA_NORMAL; + hdma_usart6_tx.Init.Priority = DMA_PRIORITY_LOW; + hdma_usart6_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_usart6_tx) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart6_tx); + /* USART6 interrupt Init */ HAL_NVIC_SetPriority(USART6_IRQn, 5, 0); HAL_NVIC_EnableIRQ(USART6_IRQn); @@ -352,6 +371,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) /* USART6 DMA DeInit */ HAL_DMA_DeInit(uartHandle->hdmarx); + HAL_DMA_DeInit(uartHandle->hdmatx); /* USART6 interrupt Deinit */ HAL_NVIC_DisableIRQ(USART6_IRQn); diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index b9480f8..7c29790 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -165,6 +165,101 @@ 1 dr16,0x0A + + 3 + 1 + UP_t,0x0A + + + 4 + 1 + UP,0x0A + + + 5 + 1 + can,0x0A + + + 6 + 1 + up_cmd,0x0A + + + 7 + 1 + rc_ctrl + + + 8 + 1 + up_cmd + + + 9 + 1 + cmd,0x0A + + + 10 + 1 + ctrl,0x0A + + + 11 + 1 + ctrl_up,0x0A + + + 12 + 1 + task_runtime,0x0A + + + 13 + 1 + can,0x0A + + + 14 + 1 + can_rx,0x0A + + + 15 + 1 + can_out,0x0A + + + 16 + 1 + gcan + + + 17 + 1 + raw_tx,0x0A + + + 18 + 1 + raw_rx2,0x0A + + + 19 + 1 + can_rx,0x0A + + + 20 + 1 + can_out + + + 21 + 1 + UP_CAN_out,0x0A + 0 @@ -252,7 +347,7 @@ Application/User/Core - 0 + 1 0 0 0 diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf new file mode 100644 index 0000000..3d40118 Binary files /dev/null and b/MDK-ARM/R2/R2.axf differ diff --git a/R2.ioc b/R2.ioc index 85412d8..1e268a9 100644 --- a/R2.ioc +++ b/R2.ioc @@ -28,7 +28,8 @@ Dma.Request2=USART3_RX Dma.Request3=USART6_RX Dma.Request4=USART1_TX Dma.Request5=USART1_RX -Dma.RequestsNb=6 +Dma.Request6=USART6_TX +Dma.RequestsNb=7 Dma.SPI1_RX.0.Direction=DMA_PERIPH_TO_MEMORY Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE Dma.SPI1_RX.0.Instance=DMA2_Stream2 @@ -89,6 +90,16 @@ Dma.USART6_RX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE Dma.USART6_RX.3.PeriphInc=DMA_PINC_DISABLE Dma.USART6_RX.3.Priority=DMA_PRIORITY_VERY_HIGH Dma.USART6_RX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode +Dma.USART6_TX.6.Direction=DMA_MEMORY_TO_PERIPH +Dma.USART6_TX.6.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART6_TX.6.Instance=DMA2_Stream6 +Dma.USART6_TX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART6_TX.6.MemInc=DMA_MINC_ENABLE +Dma.USART6_TX.6.Mode=DMA_NORMAL +Dma.USART6_TX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART6_TX.6.PeriphInc=DMA_PINC_DISABLE +Dma.USART6_TX.6.Priority=DMA_PRIORITY_LOW +Dma.USART6_TX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode FREERTOS.INCLUDE_pcTaskGetTaskName=1 FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=1 FREERTOS.INCLUDE_vTaskCleanUpResources=1 @@ -177,6 +188,7 @@ NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA2_Stream2_IRQn=true\:5\:0\:true\:false\:true\:false\:false\:true\:true NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true +NVIC.DMA2_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA2_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index 1ed3fe0..8d720c2 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -21,10 +21,10 @@ static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){ if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */ - if (ctrl->CMD_CtrlType== c->ctrl && ctrl->CMD_mode == c->mode) return CHASSIS_OK; /*模式未改变直接返回*/ + if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/ //此处源代码处做了pid的reset 待添加 - c->ctrl =ctrl->CMD_CtrlType ; - c->mode =ctrl->CMD_mode ; + c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ; + c->chassis_ctrl.mode =ctrl->CMD_mode ; return CHASSIS_OK; } //设置控制模式 @@ -37,8 +37,8 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) { if (can == NULL) return CHASSIS_ERR_NULL; for (uint8_t i = 0; i < 4; i++) { - c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed; - c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current; + c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed; + c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current; } @@ -124,26 +124,28 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) //此处对imu加滤波做修正 c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z); - switch (c->ctrl) + switch (c->chassis_ctrl .ctrl) { - case RCcontrol: //手动控制 + case RCcontrol: //手动控制 /* 在cmd里对数据进行处理 包括方向和油门 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/ + if(c->chassis_ctrl .mode != Pitch){ c->move_vec.Vw = ctrl->Vw*6000; c->move_vec.Vx = ctrl->Vy*6000; c->move_vec.Vy = ctrl->Vx*6000; + } break; case AUTO : //在自动模式下 - switch(c->mode ){ + switch(c->chassis_ctrl.mode ){ - case AUTO_NAVI: //自动雷达 + case AUTO_NAVI: //自动雷达 // //这套是全向轮的方向,一定要注意这里的xy方向 c->move_vec.Vw =ctrl->cmd_MID360 .posw *1000 ; c->move_vec.Vy =-ctrl->cmd_MID360.posy *1000 ; @@ -210,6 +212,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) } + //电机速度限幅 abs_limit_fp(&c->move_vec.Vx,6000.0f); diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index 138cde6..31a6dd8 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -118,9 +118,12 @@ typedef struct{ const Chassis_Param_t *param; //一些固定的参数 ChassisImu_t pos088; //088的实时姿态 + struct{ + CMD_CtrlType_t ctrl; - CMD_mode_t mode; - + CMD_mode_t mode; + + }chassis_ctrl; ChassisMove_Vec move_vec; //由控制任务决定 struct{ diff --git a/User/Module/config.c b/User/Module/config.c index b4acbaf..8be6541 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -124,6 +124,8 @@ static const ConfigParam_t param ={ .chassis5065 = BSP_CAN_1, .sick = BSP_CAN_2, + .encoder=BSP_CAN_2, + }, }; diff --git a/User/Module/up.c b/User/Module/up.c index 43b1812..bcb2dca 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -61,8 +61,8 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { for(int i=0;i<4;i++){ - u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ; - u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .motor3508 .as_array [i].rotor_ecd ; + u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_motor3508 .as_array [i].rotor_speed ; + u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_motor3508.as_array [i].rotor_ecd ; DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR2006_ECD_TO_ANGLE); } @@ -122,7 +122,6 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed) - /*go电机控制*/ int8_t GO_SendData(UP_t *u, float pos, float limit) @@ -130,17 +129,17 @@ int8_t GO_SendData(UP_t *u, float pos, float limit) static int is_calibration=0; static fp32 error=0; //误差量 - GO_MotorData_t *GO_calibration;//校准前，原始数据 - GO_calibration = get_GO_measure_point() ; - if(is_calibration==0) - { - is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 ); - u->go_cmd.Pos = -50; //上电之后跑 - error= GO_calibration->Pos ; - } - u->motorfeedback .go_data = GO_calibration; - u->motorfeedback .go_data ->Pos= error ; - u->go_cmd.Pos = pos; +// GO_MotorData_t *GO_calibration;//校准前，原始数据 + u->motorfeedback .go_data = get_GO_measure_point() ; +// if(is_calibration==0) +// { +// is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 ); +// u->go_cmd.Pos = -50; //上电之后跑 +// error= GO_calibration->Pos ; +// } +// u->motorfeedback .go_data = GO_calibration; +// u->motorfeedback .go_data ->Pos= error ; +// u->go_cmd.Pos = pos; // 读取参数 float tff = u->go_cmd.T; // 前馈力矩 @@ -181,7 +180,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) &u->pid .Pitch_M2006_angle , &u->pid .Pitch_M2006_speed , u->motor_target .Pitch_M2006_angle ); - GO_SendData(u,u->motor_target .go_shoot,1 ); + GO_SendData(u,u->motor_target .go_shoot,5 ); for(int i=0;i<4;i++){ @@ -218,21 +217,54 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; is_pitch=0; } + u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; + u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 +// /*运球*/ +// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ; +// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; +// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态 + + +// RELAY1_TOGGLE (1);//夹球，0夹1开 +// RELAY2_TOGGLE (0);//球，0接1收 // break; case Pitch : - if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动 + if (u->PitchContext .PitchState ==PITCH_PREPARE) { u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 } - + Pitch_Process(u,out,c); break ; + case Dribble: + { + /*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */ + + if(u->DribbleContext.DribbleState== Dribble_PREPARE){ + + u->DribbleContext .DribbleState =STATE_GRAB_BALL; + } +// Dribble_Process(u,out); + + + }break ; + case Dribbl_transfer: + break ; +// Dribbl_transfer_a(u,out); + } + break; + + + default: + break; + } + return 0; @@ -240,9 +272,65 @@ return 0; } -} + + + + + + + + + + + + + +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) +{ + static fp32 go1_position,M2006_Screw_position ; + static int is_initialize=1; + if(is_initialize) + { + go1_position=u->PitchContext .PitchConfig .go1_init_position ; + M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init; + is_initialize=0; } + switch(u->PitchContext .PitchState){ + + case PITCH_START: + u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold; + + if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 + u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0 + + u->PitchContext .PitchState=PITCH_PULL_TRIGGER; +}//更改标志位 + + break ; + case PITCH_PULL_TRIGGER: + + + if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 + { + u->motor_target .go_shoot = go1_position;//go下拉 + u->motor_target .Pitch_M2006_angle = M2006_Screw_position;//丝杠上的2006 + go1_position = go1_position + c->Vx ; + M2006_Screw_position=M2006_Screw_position+ c->Vy; + + + } + + break ; + + + + + } + +return 0; + +} @@ -251,3 +339,130 @@ return 0; + + + + + + + + + + + + + + + + + + + + + +//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) +//{ +// +// switch (u->DribbleContext.DribbleState) { +// case STATE_GRAB_BALL://开始 +// +// RELAY1_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)) { +// RELAY2_TOGGLE (1); +// + +//// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){ +//// +//// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变 +//// } +// +// } +// break; + + +// case STATE_RELEASE_BALL: +// RELAY1_TOGGLE (1);//松球 + +// +// +// +//// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){ +//// u->motor_target.go_spin =-40; //恢复go2位置 +//// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变 + +//// } + +// break; + +// case STATE_CATCH_PREP: +//// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){ +//// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降 +//// +//// RELAY2_TOGGLE (0);//接球 +//// } +// if(u->motorfeedback .M3508 .total_angle <51.0f){ +// RELAY1_TOGGLE (0);//夹球，0夹1开 + +// u->DribbleContext .DribbleState = STATE_TRANSFER; +// } +// +// break; +// + +// +// +// break ; + + + +// default: +// break; +// } + +// return 0; +//} + +//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out) +//{ +// switch (u->DribbleContext.DribbleState) { +// +// +// +// case STATE_TRANSFER: +// if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球 +// { +// +// u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ; +// } +// +//// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9) +//// { +//// RELAY1_TOGGLE (1);//夹球，0夹1开 +//// +//// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8) +//// { +//// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变 +//// } +//// } +// +// +// break ; + +// case STATE_CATCH_DONE: +// +// RELAY1_TOGGLE (1);//夹球，0夹1开 +// RELAY2_TOGGLE (0);//夹球，0接1收 + +// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ; + +// break; + +// break; + +// } +//} diff --git a/User/Module/up.h b/User/Module/up.h index cc81c4f..54ce955 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -138,7 +138,7 @@ typedef struct fp32 orig_angle; fp32 last_angle; int32_t round_cnt; - uint16_t init_cnt; + int init_cnt; fp32 total_angle; }DJmotor_feedback_t; @@ -228,6 +228,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c); int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out); int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle); int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle); +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); #endif diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c index 626c84b..3138f39 100644 --- a/User/device/GO_M8010_6_Driver.c +++ b/User/device/GO_M8010_6_Driver.c @@ -102,7 +102,7 @@ void USART6_RxCompleteCallback(void ) memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data extract_data(&data); } -const GO_MotorData_t *get_GO_measure_point(void) +GO_MotorData_t *get_GO_measure_point(void) { return &data; } diff --git a/User/device/GO_M8010_6_Driver.h b/User/device/GO_M8010_6_Driver.h index d6b418e..15ffbdc 100644 --- a/User/device/GO_M8010_6_Driver.h +++ b/User/device/GO_M8010_6_Driver.h @@ -107,10 +107,12 @@ typedef struct void modify_data(GO_MotorCmd_t *motor_s); void extract_data(GO_MotorData_t *motor_r); -const GO_MotorData_t *get_GO_measure_point(void); +GO_MotorData_t *get_GO_measure_point(void); void GO_M8010_send_data( GO_MotorCmd_t *cmd); void USART6_RxCompleteCallback(void ); + + #endif /*__GO_M8010_6_H */ diff --git a/User/device/can_use.c b/User/device/can_use.c index 5d48677..1d22015 100644 --- a/User/device/can_use.c +++ b/User/device/can_use.c @@ -30,9 +30,11 @@ #define CAN_M3508_MAX_ABS_CUR (20) #define CAN_M2006_MAX_ABS_CUR (10) -static CAN_RawRx_t raw_rx1;//原始的can数据 -static CAN_RawRx_t raw_rx2; -CAN_RawTx_t raw_tx; +#define CAN_ENCODER_RESOLUTION (32768) //欧艾迪编码器分辨率 + + CAN_RawRx_t raw_rx1;//原始的can数据 + CAN_RawRx_t raw_rx2; + CAN_RawTx_t raw_tx; /*用于can原始数据，传入消息队列*/ @@ -48,7 +50,7 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback, feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]); feedback->temp = raw[6]; - feedback->rotor_ecd = raw_ecd / (float)CAN_MOTOR_ENC_RES * 360.0f; + feedback->rotor_ecd = raw_ecd ; feedback->torque_current = raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; } @@ -61,6 +63,22 @@ static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, feedback->raw_dis[3]= (uint16_t)((raw[6] << 8) | raw[7]); } +void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, + const uint8_t *raw) { + if (feedback == NULL || raw == NULL) return; + switch(raw[1])//判断编码器id + { + case 0x01: + + switch(raw[2])//判断指令id + { + case 0x01: + feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;//接收编码器值 + feedback->angle=feedback->ecd*360/CAN_ENCODER_RESOLUTION; + break; + } + } +} static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback, const uint8_t *raw) { @@ -89,6 +107,7 @@ static void CAN_CAN1RxFifoMsgPendingCallback(void) { HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_RX_FIFO0, &raw_rx1.rx_header, raw_rx1.rx_data); raw_rx1.hcan =BSP_CAN_GetHandle(BSP_CAN_1); + osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0); } @@ -104,7 +123,7 @@ static void CAN_CAN2RxFifoMsgPendingCallback(void) { int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) { if (can == NULL) return DEVICE_ERR_NULL; - can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL); + can->msgq_raw = osMessageQueueNew(12, sizeof(CAN_RawRx_t), NULL); can->param = param; @@ -141,7 +160,21 @@ int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) { gcan = can; return DEVICE_OK; } +//用来问答接收来自欧艾迪编码器的数据 +void CAN_Encoder_Control(CAN_t *can) +{ + raw_tx.tx_header.StdId = 0x01; + raw_tx.tx_header.IDE = CAN_ID_STD; + raw_tx.tx_header.DLC = 4; + raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡ + raw_tx.tx_data[0] = 0x04; + raw_tx.tx_data[1] = 0x01; + raw_tx.tx_data[2] = 0x01; + raw_tx.tx_data[3] = 0x00; + + HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->encoder),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.up )); +} int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output, CAN_t *can) { if (output == NULL) return DEVICE_ERR_NULL; @@ -363,7 +396,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { // 存储消息到对应的电机结构体中 index = can_rx->rx_header.StdId - CAN_CHASSIS_M3508_M1_ID; can->recive_flag |= 1 << (index); - CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data); + CAN_DJIMotor_Decode(&(can->motor.chassis5065.as_array[index]), can_rx->rx_data); detect_hook(CHASSIS3508M1_TOE + index); break; @@ -389,7 +422,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { // 存储消息到对应的电机结构体中 index = can_rx->rx_header.StdId - CAN_UP_M3508_M1_ID; can->recive_flag |= 1 << (index); - CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data); + CAN_DJIMotor_Decode(&(can->motor.up_motor3508.as_array[index]), can_rx->rx_data); detect_hook(CHASSIS3508M1_TOE + index); break; @@ -397,6 +430,9 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { // 存储消息到sickfed结构体中 CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data); break; + case CAN_Encoder_ID: + CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data); + break ; default: break; diff --git a/User/device/can_use.h b/User/device/can_use.h index 182db2b..e6f1993 100644 --- a/User/device/can_use.h +++ b/User/device/can_use.h @@ -23,6 +23,8 @@ typedef enum { CAN_SICK_ID=0x301, + CAN_Encoder_ID=0x01, + CAN_G6020_Pitch =0x209, // CAN_VESC5065_M1 =0x211, //vesc的数据指令使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 @@ -111,6 +113,8 @@ typedef struct { BSP_CAN_t motor_UP3508; BSP_CAN_t pitch6020; BSP_CAN_t sick; + BSP_CAN_t encoder; + } CAN_Params_t; @@ -127,6 +131,13 @@ typedef struct { uint16_t raw_dis[4]; int changed_dis[4]; }CAN_SickFeedback_t; +/* encoder编码器反馈信息 */ +typedef struct { + + uint32_t ecd; + float angle; + +}CAN_EncoderFeedback_t; typedef union { CAN_MotorFeedback_t as_array[4]; @@ -152,7 +163,8 @@ typedef union { typedef struct { CAN_ChassisMotor_t chassis6020; CAN_ChassisMotor_t chassis5065; - CAN_ChassisMotor_t motor3508; + CAN_ChassisMotor_t up_motor3508; + CAN_ChassisMotor_t chassis_motor3508; CAN_ChassisMotor_t pit6020; } CAN_Motor_t; @@ -161,6 +173,7 @@ typedef struct { uint32_t recive_flag; CAN_Motor_t motor; + CAN_EncoderFeedback_t Oid; CAN_SickFeedback_t sickfed; const CAN_Params_t *param; struct { @@ -186,5 +199,9 @@ int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_ void CAN_Sick_Control(CAN_t *can); +void CAN_Encoder_Control(CAN_t *can); + +void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, + const uint8_t *raw) ; #endif diff --git a/User/device/cmd.c b/User/device/cmd.c index 0a1f114..d73c2f7 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -162,25 +162,23 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { else if(rc->sw_l==CMD_SW_UP) { cmd ->CMD_CtrlType =RCcontrol; - if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的 - if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 - if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Pitch_HAND; //左上，右上，手动调整 + if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的 + + if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 + + if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，手动调整 } else if(rc->sw_l==CMD_SW_MID) { - - if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达 - if(rc->sw_r ==CMD_SW_MID) - { - cmd ->CMD_CtrlType =RCcontrol; - cmd ->CMD_mode =Normal; //左中，右中，无模式 - } - if(rc->sw_r ==CMD_SW_DOWN) - { - cmd ->CMD_mode =Normal; //左中，右下，无模式 - cmd ->CMD_CtrlType =RCcontrol; - } + + cmd ->CMD_CtrlType =AUTO; + if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达 + + if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式 + + if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉 + } else if(rc->sw_l==CMD_SW_DOWN) { diff --git a/User/device/cmd.h b/User/device/cmd.h index 7e80722..986e80d 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -9,8 +9,6 @@ typedef enum{ RCcontrol,//遥控器控制，左按键上，控制上层 - MID_NAVI,//雷达导航 - PICK_t, AUTO, RELAXED,//异常模式 @@ -24,11 +22,10 @@ typedef enum{ AUTO_PICK, Dribble , //运球 - Pitch, //投篮 - /*视觉辅助下的投篮*/ - Pitch_HAND, + Pitch, //投篮,底盘锁定 - Dribbl_transfer + + Dribbl_transfer }CMD_mode_t; typedef struct { uint8_t status_fromnuc; diff --git a/User/task/can_task.c b/User/task/can_task.c index d557f43..c21e9cf 100644 --- a/User/task/can_task.c +++ b/User/task/can_task.c @@ -43,14 +43,18 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN; while (osMessageQueueGet(can.msgq_raw, &can_rx, 0, 0)==osOK) { CAN_StoreMsg(&can, &can_rx); - } - //一问一答sick数据指令 - CAN_Sick_Control(&can); + } + + //一问一答sick数据指令 +// CAN_Sick_Control(&can); + CAN_Encoder_Control(&can); + /*can设备数据存入队列*/ - osMessageQueueReset(task_runtime.msgq.can.feedback.CAN_feedback ); - osMessageQueuePut(task_runtime.msgq.can.feedback.CAN_feedback , &can, 0, 0); - + osMessageQueueReset(task_runtime.msgq.can.feedback.CHASSIS_CAN_feedback ); + osMessageQueuePut(task_runtime.msgq.can.feedback.CHASSIS_CAN_feedback , &can, 0, 0); + osMessageQueueReset(task_runtime.msgq.can.feedback.UP_CAN_feedback ); + osMessageQueuePut(task_runtime.msgq.can.feedback.UP_CAN_feedback , &can, 0, 0); /*电机控制*/ if (osMessageQueueGet(task_runtime.msgq.can.output.pitch6020, &(can_out.pitch6020),0,0) == osOK) { @@ -60,7 +64,7 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN; &(can_out.motor_CHASSIS3508), 0, 0) == osOK) { CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS3508,&can_out,&can); } - if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508, + if (osMessageQueueGet(task_runtime.msgq.can.output.up3508, &(can_out.motor_UP3508), 0, 0) == osOK) { CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can); } diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c index d9f8c87..be1d133 100644 --- a/User/task/chassis_task.c +++ b/User/task/chassis_task.c @@ -67,10 +67,11 @@ void Task_Chassis(void *argument) osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0); /*can上设备数据获取*/ - osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0); + osMessageQueueGet(task_runtime.msgq.can.feedback.CHASSIS_CAN_feedback, &can, NULL, 0); /*底盘控制信息获取,目前dji遥控器*/ - osMessageQueueGet(task_runtime.msgq.cmd.cmd_ctrl_t,&ctrl, NULL, 0); + + osMessageQueueGet(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t,&ctrl, NULL, 0); /*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/ osKernelLock(); diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c index 23df6f0..651baa9 100644 --- a/User/task/cmd_task.c +++ b/User/task/cmd_task.c @@ -12,7 +12,6 @@ CMD_NUC_t Nuc; static CMD_t cmd; static CMD_RC_t rc_ctrl; static CMD_NUC_t Nuc; -static CMD_ACTION_t cmd_ops_out; #endif @@ -53,8 +52,10 @@ void Task_cmd(void *argument){ osKernelUnlock(); /* 同上 解锁RTOS内核 */ /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */ - osMessageQueueReset(task_runtime.msgq.cmd.cmd_ctrl_t); - osMessageQueuePut(task_runtime.msgq.cmd.cmd_ctrl_t,&cmd,0,0); + osMessageQueueReset(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t); + osMessageQueuePut(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t,&cmd,0,0); + osMessageQueueReset(task_runtime.msgq.cmd.UP_cmd_ctrl_t); + osMessageQueuePut(task_runtime.msgq.cmd.UP_cmd_ctrl_t,&cmd,0,0); tick += delay_tick; /*计算下一个唤醒时刻*/ osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */ diff --git a/User/task/init.c b/User/task/init.c index e06a809..7300d1a 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -54,13 +54,15 @@ void Task_Init(void *argument) { /* 消息队列 */ /* can */ - task_runtime.msgq.can.feedback.CAN_feedback = - osMessageQueueNew(2u, sizeof(CAN_t), NULL); - - + + task_runtime.msgq.can.feedback.UP_CAN_feedback = + osMessageQueueNew(2u, sizeof(CAN_t), NULL); + task_runtime.msgq.can.feedback.CHASSIS_CAN_feedback = + osMessageQueueNew(2u, sizeof(CAN_t), NULL); task_runtime.msgq.can.output.chassis3508 = osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); - + task_runtime.msgq.can.output.up3508 = + osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); task_runtime.msgq.can.output.pitch6020 = osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); @@ -77,8 +79,12 @@ void Task_Init(void *argument) { /*cmd */ task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL); - task_runtime.msgq.cmd.cmd_ctrl_t = + + task_runtime.msgq.cmd.UP_cmd_ctrl_t = osMessageQueueNew(3u, sizeof(CMD_t), NULL); + task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t = + osMessageQueueNew(3u, sizeof(CMD_t), NULL); + task_runtime.msgq.cmd.raw.nuc = osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL); diff --git a/User/task/up_task.c b/User/task/up_task.c index 7769574..506576d 100644 --- a/User/task/up_task.c +++ b/User/task/up_task.c @@ -57,16 +57,6 @@ void Task_up(void *argument) task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId()); #endif UP_UpdateFeedback(&UP, &can,&up_cmd) ; -// GM6020_control(&UP, 100) ; -// UP_M3508_speed(&UP, 500); - -// UP_angle_control(&UP,0,M2006); -// -// -// VESC_M5065_Control(&UP, 20000); - - -// UP_control(&UP,&UP_CAN_out,&up_cmd); // UP.motor_target .go_shoot =aaa; @@ -89,10 +79,10 @@ void Task_up(void *argument) osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0); /*can上设备数据获取*/ - osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0); + osMessageQueueGet(task_runtime.msgq.can.feedback.UP_CAN_feedback, &can, NULL, 0); - osMessageQueueGet(task_runtime.msgq.cmd .cmd_ctrl_t , &up_cmd, NULL, 0); - + osMessageQueueGet(task_runtime.msgq.cmd.UP_cmd_ctrl_t,&up_cmd, NULL, 0); + /*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/ osKernelLock(); diff --git a/User/task/user_task.c b/User/task/user_task.c index bbf54ea..569ce05 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -7,6 +7,9 @@ 对于内存分配的一些理论规则： 此处堆栈存放的都是一些局部变量和全局变量，需要根据线程自身的需求大小进行设定。 + + 涉及上下层的消息队列，不使用多个任务读取同一消息队列的方式 + 建立各自使用的消息队列，避免影响，涉及cmd控制，can消息 */ /* Includes ----------------------------------------------------------------- */ @@ -30,11 +33,6 @@ const osThreadAttr_t attr_chassis = { .priority = osPriorityAboveNormal, .stack_size = 512 * 4, }; -//const osThreadAttr_t attr_r12ds = { -// .name = "r12ds", -// .priority = osPriorityRealtime, -// .stack_size = 128 * 4, -//}; const osThreadAttr_t attr_can = { .name = "can", diff --git a/User/task/user_task.h b/User/task/user_task.h index db66cb3..02fdf1a 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -64,7 +64,13 @@ typedef struct { }raw; - osMessageQueueId_t cmd_ctrl_t; + + /*控制分离*/ + osMessageQueueId_t UP_cmd_ctrl_t; + osMessageQueueId_t CHASSIS_cmd_ctrl_t; + + + osMessageQueueId_t status; } cmd; /* can任务放入、读取，电机或电容的输入输出 */ @@ -78,8 +84,12 @@ typedef struct { } output; struct { - osMessageQueueId_t CAN_feedback;//包括底盘，云台6020,3508,5065,sick,can上设备数据 + /*均包括所有数据，选择调用*/ + osMessageQueueId_t UP_CAN_feedback;//上层 + osMessageQueueId_t CHASSIS_CAN_feedback;//底盘 + } feedback; + osMessageQueueId_t msg_raw; } can; } msgq;