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 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 */

3
Core/Src/dma.c
View File

@ -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);

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_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.
*/

22
Core/Src/usart.c
View File

@ -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);

97
MDK-ARM/R2.uvoptx
View File

@ -165,6 +165,101 @@
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
</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>
<Tracepoint>
<THDelay>0</THDelay>
@ -252,7 +347,7 @@
<Group>
<GroupName>Application/User/Core</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<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.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

17
User/Module/Chassis.c
View File

@ -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,23 +124,25 @@ 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: //手动控制
/*
在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: //自动雷达
@ -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);

3
User/Module/Chassis.h
View File

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

2
User/Module/config.c
View File

@ -124,6 +124,8 @@ static const ConfigParam_t param ={
.chassis5065 = BSP_CAN_1,
.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++){
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;
// }
//}

3
User/Module/up.h
View File

@ -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

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
extract_data(&data);
}
const GO_MotorData_t *get_GO_measure_point(void)
GO_MotorData_t *get_GO_measure_point(void)
{
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 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 */

50
User/device/can_use.c
View File

@ -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;

19
User/device/can_use.h
View File

@ -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

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;
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_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)
{

7
User/device/cmd.h
View File

@ -9,8 +9,6 @@
typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层
MID_NAVI,//雷达导航
PICK_t,
AUTO,
RELAXED,//异常模式
@ -24,9 +22,8 @@ typedef enum{
AUTO_PICK,
Dribble , //运球
Pitch, //投篮
/*视觉辅助下的投篮*/
Pitch_HAND,
Pitch, //投篮,底盘锁定
Dribbl_transfer
}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);
}
//一问一答sick数据指令
CAN_Sick_Control(&can);
// 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);
}

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);
/*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();

7
User/task/cmd_task.c
View File

@ -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); /*绝对延时 等待下一个唤醒时刻 */

16
User/task/init.c
View File

@ -54,13 +54,15 @@ void Task_Init(void *argument) {
/* 消息队列 */
/* 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);
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);

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());
#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,9 +79,9 @@ 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();

8
User/task/user_task.c
View File

@ -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",

14
User/task/user_task.h
View File

@ -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;