LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改can中原数据消息队列失效问题，消息队列涉及上下层的都分离，包括can，cmd

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-24 21:22:56 +08:00
parent e2bc674c7d
commit eb6f11b02c
25 changed files with 532 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Core/Inc/stm32f4xx_it.h
View File

@ -69,6 +69,7 @@ void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void); void CAN2_RX1_IRQHandler(void);
void OTG_FS_IRQHandler(void); void OTG_FS_IRQHandler(void);
void DMA2_Stream5_IRQHandler(void); void DMA2_Stream5_IRQHandler(void);
void DMA2_Stream6_IRQHandler(void);
void DMA2_Stream7_IRQHandler(void); void DMA2_Stream7_IRQHandler(void);
void USART6_IRQHandler(void); void USART6_IRQHandler(void);
/* USER CODE BEGIN EFP */ /* USER CODE BEGIN EFP */

3
Core/Src/dma.c
View File

@ -59,6 +59,9 @@ void MX_DMA_Init(void)
/* DMA2_Stream5_IRQn interrupt configuration */ /* DMA2_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0); HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn); 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 */ /* DMA2_Stream7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0); HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn); HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn);

15
Core/Src/stm32f4xx_it.c
View File

@ -69,6 +69,7 @@ extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx; extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart3_rx; extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx; extern DMA_HandleTypeDef hdma_usart6_rx;
extern DMA_HandleTypeDef hdma_usart6_tx;
extern UART_HandleTypeDef huart1; extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart6; extern UART_HandleTypeDef huart6;
/* USER CODE BEGIN EV */ /* USER CODE BEGIN EV */
@ -418,6 +419,20 @@ void DMA2_Stream5_IRQHandler(void)
/* USER CODE END DMA2_Stream5_IRQn 1 */ /* 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. * @brief This function handles DMA2 stream7 global interrupt.
*/ */

22
Core/Src/usart.c
View File

@ -31,6 +31,7 @@ DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart1_rx; DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart3_rx; DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart6_rx; DMA_HandleTypeDef hdma_usart6_rx;
DMA_HandleTypeDef hdma_usart6_tx;
/* USART1 init function */ /* USART1 init function */
@ -103,7 +104,7 @@ void MX_USART6_UART_Init(void)
/* USER CODE END USART6_Init 1 */ /* USER CODE END USART6_Init 1 */
huart6.Instance = USART6; huart6.Instance = USART6;
huart6.Init.BaudRate = 115200; huart6.Init.BaudRate = 4000000;
huart6.Init.WordLength = UART_WORDLENGTH_8B; huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1; huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE; huart6.Init.Parity = UART_PARITY_NONE;
@ -278,6 +279,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart6_rx); __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 */ /* USART6 interrupt Init */
HAL_NVIC_SetPriority(USART6_IRQn, 5, 0); HAL_NVIC_SetPriority(USART6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART6_IRQn); HAL_NVIC_EnableIRQ(USART6_IRQn);
@ -352,6 +371,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
/* USART6 DMA DeInit */ /* USART6 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx); HAL_DMA_DeInit(uartHandle->hdmarx);
HAL_DMA_DeInit(uartHandle->hdmatx);
/* USART6 interrupt Deinit */ /* USART6 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART6_IRQn); HAL_NVIC_DisableIRQ(USART6_IRQn);

97
MDK-ARM/R2.uvoptx
View File

@ -165,6 +165,101 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText> <ItemText>dr16,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>UP_t,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl_up,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>task_runtime,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>can_rx,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>can_out,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>gcan</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>raw_tx,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>raw_rx2,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>can_rx,0x0A</ItemText>
</Ww>
<Ww>
<count>20</count>
<WinNumber>1</WinNumber>
<ItemText>can_out</ItemText>
</Ww>
<Ww>
<count>21</count>
<WinNumber>1</WinNumber>
<ItemText>UP_CAN_out,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -252,7 +347,7 @@
<Group> <Group>
<GroupName>Application/User/Core</GroupName> <GroupName>Application/User/Core</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

BIN
MDK-ARM/R2/R2.axf Normal file
View File

Binary file not shown.

14
R2.ioc
View File

@ -28,7 +28,8 @@ Dma.Request2=USART3_RX
Dma.Request3=USART6_RX Dma.Request3=USART6_RX
Dma.Request4=USART1_TX Dma.Request4=USART1_TX
Dma.Request5=USART1_RX 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.Direction=DMA_PERIPH_TO_MEMORY
Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.SPI1_RX.0.Instance=DMA2_Stream2 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.PeriphInc=DMA_PINC_DISABLE
Dma.USART6_RX.3.Priority=DMA_PRIORITY_VERY_HIGH 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_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_pcTaskGetTaskName=1
FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=1 FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=1
FREERTOS.INCLUDE_vTaskCleanUpResources=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_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_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_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.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.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 NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true

17
User/Module/Chassis.c
View File

@ -21,10 +21,10 @@
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){ static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */ 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 待添加 //此处源代码处做了pid的reset 待添加
c->ctrl =ctrl->CMD_CtrlType ; c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
c->mode =ctrl->CMD_mode ; c->chassis_ctrl.mode =ctrl->CMD_mode ;
return CHASSIS_OK; 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; if (can == NULL) return CHASSIS_ERR_NULL;
for (uint8_t i = 0; i < 4; i++) for (uint8_t i = 0; i < 4; i++)
{ {
c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed; c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
} }
@ -124,23 +124,25 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
//此处对imu加滤波做修正 //此处对imu加滤波做修正
c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z); 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里对数据进行处理 包括方向和油门 在cmd里对数据进行处理 包括方向和油门
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/ 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
if(c->chassis_ctrl .mode != Pitch){
c->move_vec.Vw = ctrl->Vw*6000; c->move_vec.Vw = ctrl->Vw*6000;
c->move_vec.Vx = ctrl->Vy*6000; c->move_vec.Vx = ctrl->Vy*6000;
c->move_vec.Vy = ctrl->Vx*6000; c->move_vec.Vy = ctrl->Vx*6000;
}
break; break;
case AUTO : //在自动模式下 case AUTO : //在自动模式下
switch(c->mode ){ switch(c->chassis_ctrl.mode ){
case AUTO_NAVI: //自动雷达 case AUTO_NAVI: //自动雷达
@ -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); abs_limit_fp(&c->move_vec.Vx,6000.0f);

3
User/Module/Chassis.h
View File

@ -118,9 +118,12 @@ typedef struct{
const Chassis_Param_t *param; //一些固定的参数 const Chassis_Param_t *param; //一些固定的参数
ChassisImu_t pos088; //088的实时姿态 ChassisImu_t pos088; //088的实时姿态
struct{
CMD_CtrlType_t ctrl; CMD_CtrlType_t ctrl;
CMD_mode_t mode; CMD_mode_t mode;
}chassis_ctrl;
ChassisMove_Vec move_vec; //由控制任务决定 ChassisMove_Vec move_vec; //由控制任务决定
struct{ struct{

2
User/Module/config.c
View File

@ -124,6 +124,8 @@ static const ConfigParam_t param ={
.chassis5065 = BSP_CAN_1, .chassis5065 = BSP_CAN_1,
.sick = BSP_CAN_2, .sick = BSP_CAN_2,
.encoder=BSP_CAN_2,
}, },
}; };

251
User/Module/up.c
View File

@ -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++){ 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].rpm =can ->motor .up_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].ecd =can ->motor .up_motor3508.as_array [i].rotor_ecd ;
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR2006_ECD_TO_ANGLE); 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电机控制*/ /*go电机控制*/
int8_t GO_SendData(UP_t *u, float pos, float limit) 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 int is_calibration=0;
static fp32 error=0; //误差量 static fp32 error=0; //误差量
GO_MotorData_t *GO_calibration;//校准前，原始数据 // GO_MotorData_t *GO_calibration;//校准前，原始数据
GO_calibration = get_GO_measure_point() ; u->motorfeedback .go_data = get_GO_measure_point() ;
if(is_calibration==0) // if(is_calibration==0)
{ // {
is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 ); // is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
u->go_cmd.Pos = -50; //上电之后跑 // u->go_cmd.Pos = -50; //上电之后跑
error= GO_calibration->Pos ; // error= GO_calibration->Pos ;
} // }
u->motorfeedback .go_data = GO_calibration; // u->motorfeedback .go_data = GO_calibration;
u->motorfeedback .go_data ->Pos= error ; // u->motorfeedback .go_data ->Pos= error ;
u->go_cmd.Pos = pos; // u->go_cmd.Pos = pos;
// 读取参数 // 读取参数
float tff = u->go_cmd.T; // 前馈力矩 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_angle ,
&u->pid .Pitch_M2006_speed , &u->pid .Pitch_M2006_speed ,
u->motor_target .Pitch_M2006_angle ); 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++){ 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 ; u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0; 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; break;
case Pitch : case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动 if (u->PitchContext .PitchState ==PITCH_PREPARE)
{ {
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
} }
Pitch_Process(u,out,c);
break ; 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; 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;
// }
//}

3
User/Module/up.h
View File

@ -138,7 +138,7 @@ typedef struct
fp32 orig_angle; fp32 orig_angle;
fp32 last_angle; fp32 last_angle;
int32_t round_cnt; int32_t round_cnt;
uint16_t init_cnt; int init_cnt;
fp32 total_angle; fp32 total_angle;
}DJmotor_feedback_t; }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 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_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 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 #endif

2
User/device/GO_M8010_6_Driver.c
View File

@ -102,7 +102,7 @@ void USART6_RxCompleteCallback(void )
memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
extract_data(&data); extract_data(&data);
} }
const GO_MotorData_t *get_GO_measure_point(void) GO_MotorData_t *get_GO_measure_point(void)
{ {
return &data; return &data;
} }

4
User/device/GO_M8010_6_Driver.h
View File

@ -107,10 +107,12 @@ typedef struct
void modify_data(GO_MotorCmd_t *motor_s); void modify_data(GO_MotorCmd_t *motor_s);
void extract_data(GO_MotorData_t *motor_r); 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 GO_M8010_send_data( GO_MotorCmd_t *cmd);
void USART6_RxCompleteCallback(void ); void USART6_RxCompleteCallback(void );
#endif /*__GO_M8010_6_H */ #endif /*__GO_M8010_6_H */

50
User/device/can_use.c
View File

@ -30,9 +30,11 @@
#define CAN_M3508_MAX_ABS_CUR (20) #define CAN_M3508_MAX_ABS_CUR (20)
#define CAN_M2006_MAX_ABS_CUR (10) #define CAN_M2006_MAX_ABS_CUR (10)
static CAN_RawRx_t raw_rx1;//原始的can数据 #define CAN_ENCODER_RESOLUTION (32768) //欧艾迪编码器分辨率
static CAN_RawRx_t raw_rx2;
CAN_RawTx_t raw_tx; CAN_RawRx_t raw_rx1;//原始的can数据
CAN_RawRx_t raw_rx2;
CAN_RawTx_t raw_tx;
/*用于can原始数据，传入消息队列*/ /*用于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->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
feedback->temp = raw[6]; feedback->temp = raw[6];
feedback->rotor_ecd = raw_ecd / (float)CAN_MOTOR_ENC_RES * 360.0f; feedback->rotor_ecd = raw_ecd ;
feedback->torque_current = feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; 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]); 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, static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
const uint8_t *raw) 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, HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_RX_FIFO0,
&raw_rx1.rx_header, raw_rx1.rx_data); &raw_rx1.rx_header, raw_rx1.rx_data);
raw_rx1.hcan =BSP_CAN_GetHandle(BSP_CAN_1); raw_rx1.hcan =BSP_CAN_GetHandle(BSP_CAN_1);
osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0); 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) { int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
if (can == NULL) return DEVICE_ERR_NULL; 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; can->param = param;
@ -141,7 +160,21 @@ int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
gcan = can; gcan = can;
return DEVICE_OK; 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, int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
CAN_t *can) { CAN_t *can) {
if (output == NULL) return DEVICE_ERR_NULL; 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; index = can_rx->rx_header.StdId - CAN_CHASSIS_M3508_M1_ID;
can->recive_flag |= 1 << (index); 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); detect_hook(CHASSIS3508M1_TOE + index);
break; 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; index = can_rx->rx_header.StdId - CAN_UP_M3508_M1_ID;
can->recive_flag |= 1 << (index); 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); detect_hook(CHASSIS3508M1_TOE + index);
break; break;
@ -397,6 +430,9 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
// 存储消息到sickfed结构体中 // 存储消息到sickfed结构体中
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data); CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break; break;
case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
break ;
default: default:
break; break;

19
User/device/can_use.h
View File

@ -23,6 +23,8 @@ typedef enum {
CAN_SICK_ID=0x301, CAN_SICK_ID=0x301,
CAN_Encoder_ID=0x01,
CAN_G6020_Pitch =0x209, CAN_G6020_Pitch =0x209,
// CAN_VESC5065_M1 =0x211, //vesc的数据指令使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 // 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 motor_UP3508;
BSP_CAN_t pitch6020; BSP_CAN_t pitch6020;
BSP_CAN_t sick; BSP_CAN_t sick;
BSP_CAN_t encoder;
} CAN_Params_t; } CAN_Params_t;
@ -127,6 +131,13 @@ typedef struct {
uint16_t raw_dis[4]; uint16_t raw_dis[4];
int changed_dis[4]; int changed_dis[4];
}CAN_SickFeedback_t; }CAN_SickFeedback_t;
/* encoder编码器反馈信息 */
typedef struct {
uint32_t ecd;
float angle;
}CAN_EncoderFeedback_t;
typedef union { typedef union {
CAN_MotorFeedback_t as_array[4]; CAN_MotorFeedback_t as_array[4];
@ -152,7 +163,8 @@ typedef union {
typedef struct { typedef struct {
CAN_ChassisMotor_t chassis6020; CAN_ChassisMotor_t chassis6020;
CAN_ChassisMotor_t chassis5065; CAN_ChassisMotor_t chassis5065;
CAN_ChassisMotor_t motor3508; CAN_ChassisMotor_t up_motor3508;
CAN_ChassisMotor_t chassis_motor3508;
CAN_ChassisMotor_t pit6020; CAN_ChassisMotor_t pit6020;
} CAN_Motor_t; } CAN_Motor_t;
@ -161,6 +173,7 @@ typedef struct {
uint32_t recive_flag; uint32_t recive_flag;
CAN_Motor_t motor; CAN_Motor_t motor;
CAN_EncoderFeedback_t Oid;
CAN_SickFeedback_t sickfed; CAN_SickFeedback_t sickfed;
const CAN_Params_t *param; const CAN_Params_t *param;
struct { 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_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 #endif

22
User/device/cmd.c
View File

@ -163,24 +163,22 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
{ {
cmd ->CMD_CtrlType =RCcontrol; cmd ->CMD_CtrlType =RCcontrol;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的 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_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Pitch_HAND; //左上，右上，手动调整
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，手动调整
} }
else if(rc->sw_l==CMD_SW_MID) else if(rc->sw_l==CMD_SW_MID)
{ {
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达 cmd ->CMD_CtrlType =AUTO;
if(rc->sw_r ==CMD_SW_MID) if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达
{
cmd ->CMD_CtrlType =RCcontrol; if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
cmd ->CMD_mode =Normal; //左中，右中，无模式
} if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉
if(rc->sw_r ==CMD_SW_DOWN)
{
cmd ->CMD_mode =Normal; //左中，右下，无模式
cmd ->CMD_CtrlType =RCcontrol;
}
} }
else if(rc->sw_l==CMD_SW_DOWN) else if(rc->sw_l==CMD_SW_DOWN)
{ {

7
User/device/cmd.h
View File

@ -9,8 +9,6 @@
typedef enum{ typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层 RCcontrol,//遥控器控制，左按键上，控制上层
MID_NAVI,//雷达导航
PICK_t,
AUTO, AUTO,
RELAXED,//异常模式 RELAXED,//异常模式
@ -24,9 +22,8 @@ typedef enum{
AUTO_PICK, AUTO_PICK,
Dribble , //运球 Dribble , //运球
Pitch, //投篮 Pitch, //投篮,底盘锁定
/*视觉辅助下的投篮*/
Pitch_HAND,
Dribbl_transfer Dribbl_transfer
}CMD_mode_t; }CMD_mode_t;

14
User/task/can_task.c
View File

@ -44,13 +44,17 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
{ {
CAN_StoreMsg(&can, &can_rx); CAN_StoreMsg(&can, &can_rx);
} }
//一问一答sick数据指令 //一问一答sick数据指令
CAN_Sick_Control(&can); // CAN_Sick_Control(&can);
CAN_Encoder_Control(&can);
/*can设备数据存入队列*/ /*can设备数据存入队列*/
osMessageQueueReset(task_runtime.msgq.can.feedback.CAN_feedback ); osMessageQueueReset(task_runtime.msgq.can.feedback.CHASSIS_CAN_feedback );
osMessageQueuePut(task_runtime.msgq.can.feedback.CAN_feedback , &can, 0, 0); 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, if (osMessageQueueGet(task_runtime.msgq.can.output.pitch6020,
&(can_out.pitch6020),0,0) == osOK) { &(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_out.motor_CHASSIS3508), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS3508,&can_out,&can); 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_out.motor_UP3508), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can); CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can);
} }

5
User/task/chassis_task.c
View File

@ -67,10 +67,11 @@ void Task_Chassis(void *argument)
osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0); osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
/*can上设备数据获取*/ /*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遥控器*/ /*底盘控制信息获取,目前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() 被调用*/ /*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock(); osKernelLock();

7
User/task/cmd_task.c
View File

@ -12,7 +12,6 @@ CMD_NUC_t Nuc;
static CMD_t cmd; static CMD_t cmd;
static CMD_RC_t rc_ctrl; static CMD_RC_t rc_ctrl;
static CMD_NUC_t Nuc; static CMD_NUC_t Nuc;
static CMD_ACTION_t cmd_ops_out;
#endif #endif
@ -53,8 +52,10 @@ void Task_cmd(void *argument){
osKernelUnlock(); /* 同上 解锁RTOS内核 */ osKernelUnlock(); /* 同上 解锁RTOS内核 */
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */ /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
osMessageQueueReset(task_runtime.msgq.cmd.cmd_ctrl_t); osMessageQueueReset(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t);
osMessageQueuePut(task_runtime.msgq.cmd.cmd_ctrl_t,&cmd,0,0); 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; /*计算下一个唤醒时刻*/ tick += delay_tick; /*计算下一个唤醒时刻*/
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */ osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */

16
User/task/init.c
View File

@ -54,13 +54,15 @@ void Task_Init(void *argument) {
/* 消息队列 */ /* 消息队列 */
/* can */ /* can */
task_runtime.msgq.can.feedback.CAN_feedback =
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); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.output.chassis3508 = task_runtime.msgq.can.output.chassis3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); 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 = task_runtime.msgq.can.output.pitch6020 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
@ -77,8 +79,12 @@ void Task_Init(void *argument) {
/*cmd */ /*cmd */
task_runtime.msgq.cmd.raw.rc = task_runtime.msgq.cmd.raw.rc =
osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL); 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); 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 = task_runtime.msgq.cmd.raw.nuc =
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL); osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);

14
User/task/up_task.c
View File

@ -57,16 +57,6 @@ void Task_up(void *argument)
task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
#endif #endif
UP_UpdateFeedback(&UP, &can,&up_cmd) ; 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_control(&UP,&UP_CAN_out,&up_cmd);
// UP.motor_target .go_shoot =aaa; // UP.motor_target .go_shoot =aaa;
@ -89,9 +79,9 @@ void Task_up(void *argument)
osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0); osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
/*can上设备数据获取*/ /*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() 被调用*/ /*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock(); osKernelLock();

8
User/task/user_task.c
View File

@ -7,6 +7,9 @@
对于内存分配的一些理论规则： 对于内存分配的一些理论规则：
此处堆栈存放的都是一些局部变量和全局变量，需要根据线程自身的需求大小进行设定。 此处堆栈存放的都是一些局部变量和全局变量，需要根据线程自身的需求大小进行设定。
涉及上下层的消息队列，不使用多个任务读取同一消息队列的方式
建立各自使用的消息队列，避免影响，涉及cmd控制，can消息
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -30,11 +33,6 @@ const osThreadAttr_t attr_chassis = {
.priority = osPriorityAboveNormal, .priority = osPriorityAboveNormal,
.stack_size = 512 * 4, .stack_size = 512 * 4,
}; };
//const osThreadAttr_t attr_r12ds = {
// .name = "r12ds",
// .priority = osPriorityRealtime,
// .stack_size = 128 * 4,
//};
const osThreadAttr_t attr_can = { const osThreadAttr_t attr_can = {
.name = "can", .name = "can",

14
User/task/user_task.h
View File

@ -64,7 +64,13 @@ typedef struct {
}raw; }raw;
osMessageQueueId_t cmd_ctrl_t;
/*控制分离*/
osMessageQueueId_t UP_cmd_ctrl_t;
osMessageQueueId_t CHASSIS_cmd_ctrl_t;
osMessageQueueId_t status; osMessageQueueId_t status;
} cmd; } cmd;
/* can任务放入、读取，电机或电容的输入输出 */ /* can任务放入、读取，电机或电容的输入输出 */
@ -78,8 +84,12 @@ typedef struct {
} output; } output;
struct { struct {
osMessageQueueId_t CAN_feedback;//包括底盘，云台6020,3508,5065,sick,can上设备数据 /*均包括所有数据，选择调用*/
osMessageQueueId_t UP_CAN_feedback;//上层
osMessageQueueId_t CHASSIS_CAN_feedback;//底盘
} feedback; } feedback;
osMessageQueueId_t msg_raw;
} can; } can;
} msgq; } msgq;