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

修改了freertos的内存，增加can2上设备数量

Browse Source
This commit is contained in:
ZHAISHUI04 2025-05-05 14:54:37 +08:00
parent 3b24dc2760
commit 410d62198f
16 changed files with 239 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Inc/FreeRTOSConfig.h
View File

@ -69,7 +69,7 @@
#define configTICK_RATE_HZ ((TickType_t)1000) #define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 56 ) #define configMAX_PRIORITIES ( 56 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128) #define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)15360) #define configTOTAL_HEAP_SIZE ((size_t)0x6000)
#define configMAX_TASK_NAME_LEN ( 16 ) #define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 1 #define configUSE_TRACE_FACILITY 1
#define configUSE_16_BIT_TICKS 0 #define configUSE_16_BIT_TICKS 0

10
MDK-ARM/R2.uvoptx
View File

@ -195,6 +195,16 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText> <ItemText>cmd_fromnuc</ItemText>
</Ww> </Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>can_out</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>

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

Binary file not shown.

3
R2.ioc
View File

@ -107,9 +107,10 @@ FREERTOS.INCLUDE_xEventGroupSetBitFromISR=1
FREERTOS.INCLUDE_xSemaphoreGetMutexHolder=1 FREERTOS.INCLUDE_xSemaphoreGetMutexHolder=1
FREERTOS.INCLUDE_xTaskAbortDelay=1 FREERTOS.INCLUDE_xTaskAbortDelay=1
FREERTOS.INCLUDE_xTaskGetHandle=1 FREERTOS.INCLUDE_xTaskGetHandle=1
FREERTOS.IPParameters=Tasks01,INCLUDE_pcTaskGetTaskName,INCLUDE_xSemaphoreGetMutexHolder,INCLUDE_vTaskCleanUpResources,INCLUDE_xEventGroupSetBitFromISR,INCLUDE_uxTaskGetStackHighWaterMark2,INCLUDE_xTaskGetHandle,INCLUDE_xTaskAbortDelay,configCHECK_FOR_STACK_OVERFLOW FREERTOS.IPParameters=Tasks01,INCLUDE_pcTaskGetTaskName,INCLUDE_xSemaphoreGetMutexHolder,INCLUDE_vTaskCleanUpResources,INCLUDE_xEventGroupSetBitFromISR,INCLUDE_uxTaskGetStackHighWaterMark2,INCLUDE_xTaskGetHandle,INCLUDE_xTaskAbortDelay,configCHECK_FOR_STACK_OVERFLOW,configTOTAL_HEAP_SIZE
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1 FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1
FREERTOS.configTOTAL_HEAP_SIZE=0x6000
File.Version=6 File.Version=6
GPIO.groupedBy=Group By Peripherals GPIO.groupedBy=Group By Peripherals
KeepUserPlacement=false KeepUserPlacement=false

25
User/Module/Chassis.c
View File

@ -42,19 +42,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
} }
c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_ecd;
c->motorfeedback.rotor_pit6020rpm = can->motor.pit6020.as_array[0].rotor_speed;
c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_ecd;
c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed;
c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_ecd;
c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
// c->sick_dis[0] = can->sickfed.raw_dis[0];
// c->sick_dis[1] = can->sickfed.raw_dis[1];
// c->sick_dis[2] = can->sickfed.raw_dis[2];
// c->sick_dis[3] = can->sickfed.raw_dis[3];
return CHASSIS_OK; return CHASSIS_OK;
} }
@ -98,10 +85,10 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算 void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
{ {
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前 c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后 c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后 c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
c->hopemotorout.OmniSpeedOut[1] = Vx+Vy+Vw;//左前 c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
} }
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake) //bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
@ -228,11 +215,11 @@ 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,8000.0f);
abs_limit_fp(&c->move_vec.Vy,6000.0f); abs_limit_fp(&c->move_vec.Vy,8000.0f);
abs_limit_fp(&c->move_vec.Vw,6000.0f); abs_limit_fp(&c->move_vec.Vw,8000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);

27
User/Module/Chassis.h
View File

@ -58,26 +58,11 @@ typedef struct {
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制） AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}ChassisImu_t; }ChassisImu_t;
/*底盘的类型*/
typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦轮 */
CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
CHASSIS_TYPE_AGV, /* AGV舵轮 */
} Chassis_Type_e;
/*底盘的电机轮组*/
typedef enum {
DJI_M3508,
DJI_G6020,
AGV_Group,
}Chassis_Motortype_e;
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */ /* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct typedef struct
{ {
Chassis_Type_e chassis_type; /* */
Chassis_Motortype_e motor_type; /**/
/*该部分决定PID的参数整定在config中修改*/ /*该部分决定PID的参数整定在config中修改*/
@ -131,14 +116,6 @@ typedef struct{
fp32 rotor_rpm3508[4]; fp32 rotor_rpm3508[4];
fp32 rotor_current3508[4]; fp32 rotor_current3508[4];
fp32 rotor_pit6020angle;
fp32 rotor_pit6020rpm;
fp32 rotor_gimbal_yawangle;
fp32 rotor_gimbal_yawrpm;
fp32 rotor_gimbal_pitchangle;
fp32 rotor_gimbal_pitchrpm;
}motorfeedback; }motorfeedback;
@ -154,10 +131,6 @@ typedef struct{
struct struct
{ {
fp32 final_3508out[4]; fp32 final_3508out[4];
fp32 final_pitchout;
fp32 final_gimbal_yawout;
fp32 final_gimbal_pitchout;
}final_out; }final_out;
struct{ struct{

39
User/Module/config.c
View File

@ -50,6 +50,35 @@ static const ConfigParam_t param ={
.i_limit = 2000.0f, .i_limit = 2000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
.p = 20.0f,
.i = 0.05f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 3000.0f,
},
.Dribble_M3508_speed_param={ //三摩擦速度环
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Dribble_translate_M3508_angle_param= { //平移用3508角度环
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.go_cmd={ .go_cmd={
@ -81,6 +110,7 @@ static const ConfigParam_t param ={
.go1_init_position = -50, .go1_init_position = -50,
.go1_release_threshold =-210, .go1_release_threshold =-210,
.m2006_Screw_init=0, .m2006_Screw_init=0,
.Pitch_angle =57,
}, },
@ -113,18 +143,17 @@ static const ConfigParam_t param ={
}, },
.can = { .can = {
.pitch6020 = BSP_CAN_1,
.motor_CHASSIS3508 = BSP_CAN_1, .motor_CHASSIS3508 = BSP_CAN_1,
.motor_UP3508 = BSP_CAN_2, . motor_UP3508_shoot= BSP_CAN_2,
.motor_UP3508_Dribble= BSP_CAN_2,
// .chassis6020 = BSP_CAN_1,//禁用
.chassis5065 = BSP_CAN_1, .chassis5065 = BSP_CAN_1,
.sick = BSP_CAN_2, .sick = BSP_CAN_2,
.encoder=BSP_CAN_2, .encoder=BSP_CAN_2,
}, },

96
User/Module/up.c
View File

@ -11,6 +11,8 @@
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度 #define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度 #define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
#define CAN2_G3508_ID (0x200)
// 定义继电器控制 // 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) #define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
@ -27,9 +29,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param )); PID_init (&u->pid .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param ));
PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param )); PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param ));
PID_init (&u->pid .Pitch_Angle_M2006_speed ,PID_POSITION ,&(u->param->Pitch_Angle_M2006_speed_param));
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param )); PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param )); PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param ));
PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
PID_init (&u->pid .Dribble_M3508_speed ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
u->go_cmd =u->param ->go_cmd ; u->go_cmd =u->param ->go_cmd ;
@ -62,12 +71,19 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
u->motorfeedback .Encoder .angle = can ->Oid.angle ; u->motorfeedback .Encoder .angle = can ->Oid.angle ;
u->motorfeedback .Encoder .ecd = can ->Oid.ecd ; u->motorfeedback .Encoder .ecd = can ->Oid.ecd ;
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){ //范围0x201--0x204
u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_motor3508 .as_array [i].rotor_speed ; u->motorfeedback .DJmotor_feedback[i].id =i+CAN2_G3508_ID+1;
u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_motor3508.as_array [i].rotor_ecd ; u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_dribble_motor3508 .as_array [i].rotor_speed ;
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR2006_ECD_TO_ANGLE); u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_dribble_motor3508.as_array [i].rotor_ecd ;
} DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR3508_ECD_TO_ANGLE);
}
for(int i=0;i<2;i++){ //范围0x205--0x206
u->motorfeedback .DJmotor_feedback[i+4].id =i+CAN2_G3508_ID+4+1;
u->motorfeedback .DJmotor_feedback[i+4].rpm =can ->motor .up_shoot_motor3508 .as_array [i].rotor_speed ;
u->motorfeedback .DJmotor_feedback[i+4].ecd =can ->motor .up_shoot_motor3508.as_array [i].rotor_ecd ;
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i+4], MOTOR2006_ECD_TO_ANGLE);
}
u->cmd =c; u->cmd =c;
return 0; return 0;
@ -106,10 +122,17 @@ int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle
delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle); delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle); delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
u->final_out .DJfinal_out [id-1]=delta_speed; u->final_out .DJfinal_out [id-CAN2_G3508_ID-1]=delta_speed;
return 0;
}
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed)
{
fp32 delta_speed;
delta_speed = PID_calc(Speed_pid , f->rpm , target_speed);
u->final_out .DJfinal_out [id-CAN2_G3508_ID-1]=delta_speed;
return 0; return 0;
} }
int8_t VESC_M5065_Control(UP_t *u,fp32 speed) int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
{ {
u->motor_target .VESC_5065_M1_rpm =speed; u->motor_target .VESC_5065_M1_rpm =speed;
@ -174,21 +197,33 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{ {
//电机控制 ，传进can //电机控制 ，传进can
DJ_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] , DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
u->motor_target .Dribble_translate_M3508_angle );
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
&u->pid .Shoot_M2006angle , &u->pid .Shoot_M2006angle ,
&u->pid .Shoot_M2006speed , &u->pid .Shoot_M2006speed ,
u->motor_target .Shoot_M2006_angle ); u->motor_target .Shoot_M2006_angle );
DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] , // DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
&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 );
u->final_out .DJfinal_out [5]=PID_calc (&(u->pid .Pitch_Angle_M2006_speed ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle);
GO_SendData(u,u->motor_target .go_shoot,5 ); GO_SendData(u,u->motor_target .go_shoot,5 );
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
out ->motor_UP3508.as_array[i]=u->final_out.DJfinal_out [i] ; out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
} }
for(int i=0;i<2;i++){
out ->motor_UP3508_shoot.as_array[i]=u->final_out.DJfinal_out [i+4] ;
}
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ; out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ; out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
@ -220,6 +255,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
is_pitch=0; is_pitch=0;
} }
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
// /*运球*/ // /*运球*/
@ -245,10 +282,11 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ; break ;
case UP_Adjust: case UP_Adjust:
u->PitchContext.PitchConfig.go1_init_position += c->Vx ; u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy; // u->PitchContext.PitchConfig.m2006_Screw_init = c->Vy;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init; // u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init;
break ; break ;
case Dribble: case Dribble:
{ {
@ -342,30 +380,14 @@ int8_t Pitch_Adjust(UP_t *u, CAN_Output_t *out,CMD_t *c)
{ {
u->PitchContext.PitchConfig.go1_init_position += c->Vx ; u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy; u->PitchContext.PitchConfig.Pitch_angle += c->Vy;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
} }

40
User/Module/up.h
View File

@ -56,6 +56,7 @@ typedef struct {
fp32 go1_init_position; // GO电机触发位置,0,初始位置 fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置 fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
fp32 m2006_Screw_init; fp32 m2006_Screw_init;
fp32 Pitch_angle;
} PitchConfig_t; } PitchConfig_t;
/* 投球控制上下文 */ /* 投球控制上下文 */
@ -122,7 +123,15 @@ typedef struct
pid_param_t Shoot_M2006_angle_param; pid_param_t Shoot_M2006_angle_param;
pid_param_t Pitch_M2006_speed_param; pid_param_t Pitch_M2006_speed_param;
pid_param_t Pitch_M2006_angle_param; pid_param_t Pitch_M2006_angle_param;
pid_param_t Pitch_Angle_M2006_speed_param;
pid_param_t Dribble_M3508_speed_param;//三摩擦用的速度环
pid_param_t Dribble_translate_M3508_speed_param;//平移用的速度环
pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
DribbleConfig_t DribbleConfig_Config; DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config; PitchConfig_t PitchConfig_Config;
@ -134,7 +143,7 @@ typedef struct
{ {
fp32 ecd; fp32 ecd;
fp32 rpm; fp32 rpm;
uint8_t id;
fp32 orig_angle; fp32 orig_angle;
fp32 last_angle; fp32 last_angle;
int32_t round_cnt; int32_t round_cnt;
@ -151,10 +160,7 @@ typedef struct{
DribbleContext_t DribbleContext; DribbleContext_t DribbleContext;
/*投篮过程*/ /*投篮过程*/
PitchContext_t PitchContext; PitchContext_t PitchContext;
UP_Imu_t pos088;
CMD_t *cmd; CMD_t *cmd;
struct{ struct{
@ -166,15 +172,12 @@ typedef struct{
GO_MotorData_t *go_data;//存放go电机数据 GO_MotorData_t *go_data;//存放go电机数据
DJmotor_feedback_t DJmotor_feedback[4]; DJmotor_feedback_t DJmotor_feedback[6];
struct { struct {
uint32_t ecd; uint32_t ecd;
float angle; float angle;
}Encoder;
}Encoder;
}motorfeedback; }motorfeedback;
@ -188,7 +191,10 @@ typedef struct{
fp32 Pitch_M2006_angle; fp32 Pitch_M2006_angle;
fp32 go_shoot; fp32 go_shoot;
fp32 Dribble_M3508_speed;//运球转速
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
fp32 Shoot_Pitch_angle;
}motor_target; }motor_target;
struct{ struct{
@ -203,6 +209,14 @@ typedef struct{
pid_type_def Pitch_M2006_angle; pid_type_def Pitch_M2006_angle;
pid_type_def Pitch_M2006_speed; pid_type_def Pitch_M2006_speed;
pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed;//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
}pid; }pid;
GO_MotorCmd_t go_cmd; GO_MotorCmd_t go_cmd;
@ -212,7 +226,7 @@ typedef struct{
/*经PID计算后的实际发送给电机的实时输出值*/ /*经PID计算后的实际发送给电机的实时输出值*/
struct struct
{ {
fp32 DJfinal_out[4]; fp32 DJfinal_out[6];
fp32 final_VESC_5065_M1out; fp32 final_VESC_5065_M1out;
fp32 final_VESC_5065_M2out; fp32 final_VESC_5065_M2out;
@ -236,6 +250,8 @@ 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 DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);

136
User/device/can_use.c
View File

@ -12,11 +12,13 @@
#define CAN_MOTOR_CUR_RES (16384) #define CAN_MOTOR_CUR_RES (16384)
//#define CAN_GM6020_CTRL_ID_BASE (0x1ff) //禁用
#define CAN_PITCH6020_CTRL_ID (0x2ff) #define CAN_PITCH6020_CTRL_ID (0x2ff)
#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200) #define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
#define CAN_G3508_CTRL_ALL_UP_ID (0x1ff)
/*CAN2上有6个大疆电机*/
#define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff)
#define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200)
#define CAN_VESC_CTRL_ID_BASE (0x300) #define CAN_VESC_CTRL_ID_BASE (0x300)
@ -118,13 +120,12 @@ static void CAN_CAN2RxFifoMsgPendingCallback(void) {
raw_rx2.hcan =BSP_CAN_GetHandle(BSP_CAN_2); raw_rx2.hcan =BSP_CAN_GetHandle(BSP_CAN_2);
osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0); osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
} }
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
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(12, sizeof(CAN_RawRx_t), NULL); can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL);
can->param = param; can->param = param;
CAN_FilterTypeDef can_filter = {0}; CAN_FilterTypeDef can_filter = {0};
@ -175,42 +176,14 @@ void CAN_Encoder_Control(CAN_t *can)
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->encoder),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.up )); 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;
int16_t motor1, motor2, motor3, motor4, motor5, motor6, motor7; int16_t motor1, motor2, motor3, motor4;
switch (group) { switch (group) {
// case CAN_MOTOR_CHASSIS6020: //(总线0x1ff的6020禁用，怕和3508冲突)
// motor1 =
// (int16_t)(output->chassis6020.named.m1);
// motor2 =
// (int16_t)(output->chassis6020.named.m2);
// motor3 =
// (int16_t)(output->chassis6020.named.m3);
// motor4 =
// (int16_t)(output->chassis6020.named.m4);
// raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
// raw_tx.tx_header.IDE = CAN_ID_STD;
// raw_tx.tx_header.RTR = CAN_RTR_DATA;
// raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
// raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
// raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
// raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
// raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
// raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
// raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
// raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
// raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
// HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
// &raw_tx.tx_header, raw_tx.tx_data,
// &(can->mailbox.chassis));
// break;
case CAN_MOTOR_CHASSIS3508: case CAN_MOTOR_CHASSIS3508:
motor1 = motor1 =
(int16_t)(output->motor_CHASSIS3508.named.m1); (int16_t)(output->motor_CHASSIS3508.named.m1);
@ -239,17 +212,17 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
&raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.chassis)); &(can->mailbox.chassis));
break; break;
case CAN_MOTOR_UP3508: case CAN_MOTOR_UP3508_Dribble:
motor1 = motor1 =
(int16_t)(output->motor_UP3508.named.m1); (int16_t)(output->motor_UP3508_Dribble.named.m1);
motor2 = motor2 =
(int16_t)(output->motor_UP3508.named.m2); (int16_t)(output->motor_UP3508_Dribble.named.m2);
motor3 = motor3 =
(int16_t)(output->motor_UP3508.named.m3); (int16_t)(output->motor_UP3508_Dribble.named.m3);
motor4 = motor4 =
(int16_t)(output->motor_UP3508.named.m4); (int16_t)(output->motor_UP3508_Dribble.named.m4);
raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_UP_ID; raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_UP_Dribble_ID;
raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
@ -263,35 +236,39 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF); raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF); raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_UP3508), HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_UP3508_Dribble),
&raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.up)); &(can->mailbox.up));
break; break;
case CAN_MOTOR_PITCH6020: case CAN_MOTOR_UP3508_SHOOT:
motor5 = motor1 =
(int16_t)(output->pitch6020.named.m1); (int16_t)(output->motor_UP3508_shoot.named.m1);
motor6 = motor2 =
(int16_t)(output->pitch6020.named.m2); (int16_t)(output->motor_UP3508_shoot.named.m2);
motor7 = motor3 =
(int16_t)(output->pitch6020.named.m3); (int16_t)(output->motor_UP3508_shoot.named.m3);
raw_tx.tx_header.StdId = CAN_PITCH6020_CTRL_ID; motor4 =
(int16_t)(output->motor_UP3508_shoot.named.m4);
raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_UP_SHOOT_ID;
raw_tx.tx_header.IDE = CAN_ID_STD; raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA; raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE; raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((motor5 >> 8) & 0xFF); raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(motor5 & 0xFF); raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((motor6 >> 8) & 0xFF); raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(motor6 & 0xFF); raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
raw_tx.tx_data[4] = (uint8_t)((motor7 >> 8) & 0xFF); raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
raw_tx.tx_data[5] = (uint8_t)(motor7 & 0xFF); raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
raw_tx.tx_data[6] = 0; raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
raw_tx.tx_data[7] = 0; raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->pitch6020), HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_UP3508_shoot),
&raw_tx.tx_header, raw_tx.tx_data, &raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.chassis)); &(can->mailbox.up));
break; break;
default: default:
break; break;
@ -379,16 +356,6 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
// 处理CAN1消息 // 处理CAN1消息
if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_1)) { if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_1)) {
switch (can_rx->rx_header.StdId) { switch (can_rx->rx_header.StdId) {
case CAN_G6020_AgvM1:
case CAN_G6020_AgvM2:
case CAN_G6020_AgvM3:
case CAN_G6020_AgvM4:
// 存储消息到对应的电机结构体中
index = can_rx->rx_header.StdId - CAN_G6020_AgvM1;
can->recive_flag |= 1 << (index);
CAN_DJIMotor_Decode(&(can->motor.chassis6020.as_array[index]), can_rx->rx_data);
break;
case CAN_CHASSIS_M3508_M1_ID: case CAN_CHASSIS_M3508_M1_ID:
case CAN_CHASSIS_M3508_M2_ID: case CAN_CHASSIS_M3508_M2_ID:
case CAN_CHASSIS_M3508_M3_ID: case CAN_CHASSIS_M3508_M3_ID:
@ -408,30 +375,35 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
// 处理CAN2消息 // 处理CAN2消息
if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_2)) { if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_2)) {
switch (can_rx->rx_header.StdId) { switch (can_rx->rx_header.StdId) {
case CAN_G6020_Pitch:
// 存储消息到对应的电机结构体中
index = can_rx->rx_header.StdId - CAN_G6020_Pitch;
can->recive_flag |= 1 << (index);
CAN_DJIMotor_Decode(&(can->motor.pit6020.as_array[index]), can_rx->rx_data);
detect_hook(PITCH6020_TOE + index);
break;
case CAN_UP_M3508_M1_ID: case CAN_UP_M3508_M1_ID:
case CAN_UP_M3508_M2_ID: case CAN_UP_M3508_M2_ID:
case CAN_UP_M3508_M3_ID: case CAN_UP_M3508_M3_ID:
case CAN_UP_M3508_M4_ID: case CAN_UP_M3508_M4_ID:
// 存储消息到对应的电机结构体中 // 存储消息到对应的电机结构体中
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+4));
CAN_DJIMotor_Decode(&(can->motor.up_motor3508.as_array[index]), can_rx->rx_data); CAN_DJIMotor_Decode(&(can->motor.up_dribble_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index); detect_hook(CHASSIS3508M1_TOE + index);
break; break;
case CAN_UP_M3508_M5_ID:
case CAN_UP_M3508_M6_ID:
// 存储消息到对应的电机结构体中
index = can_rx->rx_header.StdId - CAN_UP_M3508_M5_ID;
can->recive_flag |= 1 << ((index+8));
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break;
case CAN_SICK_ID: case CAN_SICK_ID:
// 存储消息到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: case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data); CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10;
break ; break ;
default: default:

60
User/device/can_use.h
View File

@ -9,29 +9,19 @@ typedef enum {
CAN_CHASSIS_M3508_M2_ID = 0x202, /* 2 */ CAN_CHASSIS_M3508_M2_ID = 0x202, /* 2 */
CAN_CHASSIS_M3508_M3_ID = 0x203, /* 3 */ CAN_CHASSIS_M3508_M3_ID = 0x203, /* 3 */
CAN_CHASSIS_M3508_M4_ID = 0x204, /* 4 */ CAN_CHASSIS_M3508_M4_ID = 0x204, /* 4 */
CAN_UP_M3508_M1_ID = 0x201, /* 1 */
CAN_UP_M3508_M2_ID = 0x202, /* 2 */
CAN_UP_M3508_M3_ID = 0x203, /* 3 */
CAN_UP_M3508_M4_ID = 0x204, /* 4 */
/**/ CAN_UP_M3508_M5_ID = 0x205, /* 1 */
CAN_G6020_AgvM1 =0x205, CAN_UP_M3508_M6_ID = 0x206, /* 2 */
CAN_G6020_AgvM2 =0x206,
CAN_G6020_AgvM3 =0x207,
CAN_G6020_AgvM4 =0x208,
CAN_UP_M3508_M1_ID = 0x205, /* 1 */
CAN_UP_M3508_M2_ID = 0x206, /* 2 */
CAN_UP_M3508_M3_ID = 0x207, /* 3 */
CAN_UP_M3508_M4_ID = 0x208, /* 4 */
CAN_SICK_ID=0x301, CAN_SICK_ID=0x301,
CAN_Encoder_ID=0x01, CAN_Encoder_ID=0x01,
CAN_G6020_Pitch =0x209,
// CAN_VESC5065_M1 =0x211, //vesc的数据指令使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
// CAN_VESC5065_M2 =0x212,
// CAN_VESC5065_M3 =0x213,
// CAN_VSEC5065_M4 =0x214,
CAN_VESC5065_M1_MSG1 =0x90b, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 CAN_VESC5065_M1_MSG1 =0x90b, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
CAN_VESC5065_M2_MSG1 =0x90c, CAN_VESC5065_M2_MSG1 =0x90c,
CAN_VESC5065_M3_MSG1 =0x90d, CAN_VESC5065_M3_MSG1 =0x90d,
@ -46,10 +36,12 @@ typedef enum {
typedef enum { typedef enum {
CAN_MOTOR_PITCH6020,
CAN_MOTOR_5065, CAN_MOTOR_5065,
CAN_MOTOR_CHASSIS3508, CAN_MOTOR_CHASSIS3508,
CAN_MOTOR_UP3508,
CAN_MOTOR_UP3508_SHOOT,
CAN_MOTOR_UP3508_Dribble,
CAN_MOTOR_GROUT_NUM, CAN_MOTOR_GROUT_NUM,
} CAN_MotorGroup_e; } CAN_MotorGroup_e;
@ -85,11 +77,10 @@ typedef union{
}Vesc_ByteGet; }Vesc_ByteGet;
typedef struct { typedef struct {
// CAN_DJIOutput_t chassis6020;
CAN_VescOutput_t chassis5065; CAN_VescOutput_t chassis5065;
CAN_DJIOutput_t motor_CHASSIS3508; CAN_DJIOutput_t motor_CHASSIS3508;
CAN_DJIOutput_t motor_UP3508; CAN_DJIOutput_t motor_UP3508_shoot;
CAN_DJIOutput_t pitch6020; CAN_DJIOutput_t motor_UP3508_Dribble;
} CAN_Output_t; } CAN_Output_t;
@ -107,11 +98,12 @@ typedef struct {
} CAN_RawTx_t; } CAN_RawTx_t;
typedef struct { typedef struct {
// BSP_CAN_t chassis6020;//禁用
BSP_CAN_t chassis5065; BSP_CAN_t chassis5065;
BSP_CAN_t motor_CHASSIS3508; BSP_CAN_t motor_CHASSIS3508;
BSP_CAN_t motor_UP3508; BSP_CAN_t motor_UP3508_shoot;
BSP_CAN_t pitch6020; BSP_CAN_t motor_UP3508_Dribble;
BSP_CAN_t sick; BSP_CAN_t sick;
BSP_CAN_t encoder; BSP_CAN_t encoder;
@ -147,7 +139,7 @@ typedef union {
CAN_MotorFeedback_t m3; CAN_MotorFeedback_t m3;
CAN_MotorFeedback_t m4; CAN_MotorFeedback_t m4;
} named; } named;
} CAN_ChassisMotor_t; } CAN_Data_Motor_t;
typedef union { typedef union {
CAN_MotorFeedback_t as_array[4]; CAN_MotorFeedback_t as_array[4];
@ -161,11 +153,14 @@ typedef union {
typedef struct { typedef struct {
CAN_ChassisMotor_t chassis6020; CAN_Data_Motor_t chassis6020;
CAN_ChassisMotor_t chassis5065; CAN_Data_Motor_t chassis5065;
CAN_ChassisMotor_t up_motor3508; /*1是指用来运球的，2用来扳机和俯仰*/
CAN_ChassisMotor_t chassis_motor3508; CAN_Data_Motor_t up_shoot_motor3508;
CAN_ChassisMotor_t pit6020; CAN_Data_Motor_t up_dribble_motor3508;
CAN_Data_Motor_t chassis_motor3508;
CAN_Data_Motor_t pit6020;
} CAN_Motor_t; } CAN_Motor_t;
@ -178,8 +173,7 @@ typedef struct {
const CAN_Params_t *param; const CAN_Params_t *param;
struct { struct {
uint32_t chassis; uint32_t chassis;
uint32_t up; uint32_t up;
} mailbox; } mailbox;
osMessageQueueId_t msgq_raw; osMessageQueueId_t msgq_raw;

17
User/task/can_task.c
View File

@ -56,19 +56,20 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
osMessageQueueReset(task_runtime.msgq.can.feedback.UP_CAN_feedback ); osMessageQueueReset(task_runtime.msgq.can.feedback.UP_CAN_feedback );
osMessageQueuePut(task_runtime.msgq.can.feedback.UP_CAN_feedback , &can, 0, 0); osMessageQueuePut(task_runtime.msgq.can.feedback.UP_CAN_feedback , &can, 0, 0);
/*电机控制*/ /*电机控制*/
if (osMessageQueueGet(task_runtime.msgq.can.output.pitch6020, //底盘4个
&(can_out.pitch6020),0,0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_PITCH6020,&can_out,&can);
}
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508, if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
&(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.up3508, //上层运球4个，发射2个
&(can_out.motor_UP3508), 0, 0) == osOK) { if (osMessageQueueGet(task_runtime.msgq.can.output.up_dribble_3508,
CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can); &(can_out.motor_UP3508_Dribble), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_UP3508_Dribble,&can_out,&can);
}
if (osMessageQueueGet(task_runtime.msgq.can.output.up_shoot_3508,
&(can_out.motor_UP3508_shoot), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_UP3508_SHOOT,&can_out,&can);
} }
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
} }

6
User/task/chassis_task.c
View File

@ -88,12 +88,6 @@ void Task_Chassis(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列 osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.can.output.pitch6020);
osMessageQueuePut(task_runtime.msgq.can.output.pitch6020, &out.pitch6020, 0, 0);
osMessageQueueReset(task_runtime.msgq.can.output.pitch6020 );
osMessageQueuePut(task_runtime.msgq.can.output.pitch6020 , &out.pitch6020, 0, 0);
vofa_send[0] = chassis.vofa_send[0]; vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1]; vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2]; vofa_send[2] = chassis.vofa_send[2];

4
User/task/init.c
View File

@ -59,9 +59,9 @@ void Task_Init(void *argument) {
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 = task_runtime.msgq.can.output.up_shoot_3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
task_runtime.msgq.can.output.pitch6020 = task_runtime.msgq.can.output.up_dribble_3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);

14
User/task/up_task.c
View File

@ -71,13 +71,6 @@ void Task_up(void *argument)
osDelay(1); osDelay(1);
/*imu数据获取*/
osMessageQueueGet(task_runtime.msgq.imu.eulr, &UP.pos088.imu_eulr, NULL, 0);
osMessageQueueGet(task_runtime.msgq.imu.gyro, &UP.pos088.bmi088.gyro,NULL, 0);
osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
/*can上设备数据获取*/ /*can上设备数据获取*/
osMessageQueueGet(task_runtime.msgq.can.feedback.UP_CAN_feedback, &can, NULL, 0); osMessageQueueGet(task_runtime.msgq.can.feedback.UP_CAN_feedback, &can, NULL, 0);
@ -91,8 +84,11 @@ void Task_up(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.up3508 );//清空队列 osMessageQueueReset(task_runtime.msgq.can.output.up_dribble_3508 );//运球
osMessageQueuePut(task_runtime.msgq.can.output.up3508, &UP_CAN_out.motor_UP3508 , 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.can.output.up_dribble_3508, &UP_CAN_out.motor_UP3508_Dribble , 0, 0);
osMessageQueueReset(task_runtime.msgq.can.output.up_shoot_3508 );//发射
osMessageQueuePut(task_runtime.msgq.can.output.up_shoot_3508, &UP_CAN_out.motor_UP3508_shoot , 0, 0);
osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0); osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0);

5
User/task/user_task.h
View File

@ -78,9 +78,8 @@ typedef struct {
osMessageQueueId_t shoot5065; osMessageQueueId_t shoot5065;
osMessageQueueId_t chassis3508; osMessageQueueId_t chassis3508;
osMessageQueueId_t pitch6020; osMessageQueueId_t up_dribble_3508;
osMessageQueueId_t up3508; osMessageQueueId_t up_shoot_3508;
} output; } output;
struct { struct {
/*均包括所有数据，选择调用*/ /*均包括所有数据，选择调用*/