修改了freertos的内存，增加can2上设备数量
This commit is contained in:
parent
3b24dc2760
commit
410d62198f
@ -69,7 +69,7 @@
|
||||
#define configTICK_RATE_HZ ((TickType_t)1000)
|
||||
#define configMAX_PRIORITIES ( 56 )
|
||||
#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 configUSE_TRACE_FACILITY 1
|
||||
#define configUSE_16_BIT_TICKS 0
|
||||
|
||||
@ -195,6 +195,16 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc</ItemText>
|
||||
</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>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
|
||||
Binary file not shown.
3
R2.ioc
3
R2.ioc
@ -107,9 +107,10 @@ FREERTOS.INCLUDE_xEventGroupSetBitFromISR=1
|
||||
FREERTOS.INCLUDE_xSemaphoreGetMutexHolder=1
|
||||
FREERTOS.INCLUDE_xTaskAbortDelay=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.configCHECK_FOR_STACK_OVERFLOW=1
|
||||
FREERTOS.configTOTAL_HEAP_SIZE=0x6000
|
||||
File.Version=6
|
||||
GPIO.groupedBy=Group By Peripherals
|
||||
KeepUserPlacement=false
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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) //底盘逆运动学的解算
|
||||
{
|
||||
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[1] = Vx+Vy+Vw;//左前
|
||||
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
|
||||
}
|
||||
|
||||
//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);
|
||||
|
||||
|
||||
|
||||
@ -58,26 +58,11 @@ typedef struct {
|
||||
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
|
||||
}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中更改后不再变化 */
|
||||
typedef struct
|
||||
{
|
||||
Chassis_Type_e chassis_type; /* */
|
||||
Chassis_Motortype_e motor_type; /**/
|
||||
|
||||
|
||||
/*该部分决定PID的参数整定在config中修改*/
|
||||
@ -131,14 +116,6 @@ typedef struct{
|
||||
fp32 rotor_rpm3508[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;
|
||||
|
||||
@ -154,10 +131,6 @@ typedef struct{
|
||||
struct
|
||||
{
|
||||
fp32 final_3508out[4];
|
||||
fp32 final_pitchout;
|
||||
fp32 final_gimbal_yawout;
|
||||
fp32 final_gimbal_pitchout;
|
||||
|
||||
}final_out;
|
||||
|
||||
struct{
|
||||
|
||||
@ -50,6 +50,35 @@ static const ConfigParam_t param ={
|
||||
.i_limit = 2000.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={
|
||||
@ -81,6 +110,7 @@ static const ConfigParam_t param ={
|
||||
.go1_init_position = -50,
|
||||
.go1_release_threshold =-210,
|
||||
.m2006_Screw_init=0,
|
||||
.Pitch_angle =57,
|
||||
},
|
||||
|
||||
|
||||
@ -116,11 +146,10 @@ static const ConfigParam_t param ={
|
||||
|
||||
|
||||
.can = {
|
||||
.pitch6020 = 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,
|
||||
|
||||
.sick = BSP_CAN_2,
|
||||
|
||||
@ -11,6 +11,8 @@
|
||||
#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 CAN2_G3508_ID (0x200)
|
||||
|
||||
|
||||
// 定义继电器控制
|
||||
#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_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_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 ;
|
||||
|
||||
@ -62,10 +71,17 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
|
||||
u->motorfeedback .Encoder .angle = can ->Oid.angle ;
|
||||
u->motorfeedback .Encoder .ecd = can ->Oid.ecd ;
|
||||
|
||||
for(int i=0;i<4;i++){
|
||||
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);
|
||||
for(int i=0;i<4;i++){ //范围0x201--0x204
|
||||
u->motorfeedback .DJmotor_feedback[i].id =i+CAN2_G3508_ID+1;
|
||||
u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_dribble_motor3508 .as_array [i].rotor_speed ;
|
||||
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;
|
||||
@ -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_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;
|
||||
}
|
||||
int8_t VESC_M5065_Control(UP_t *u,fp32 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)
|
||||
{
|
||||
//电机控制 ，传进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_M2006speed ,
|
||||
u->motor_target .Shoot_M2006_angle );
|
||||
DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] ,
|
||||
&u->pid .Pitch_M2006_angle ,
|
||||
&u->pid .Pitch_M2006_speed ,
|
||||
u->motor_target .Pitch_M2006_angle );
|
||||
// DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
|
||||
// &u->pid .Pitch_M2006_angle ,
|
||||
// &u->pid .Pitch_M2006_speed ,
|
||||
// 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 );
|
||||
|
||||
|
||||
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 [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;
|
||||
}
|
||||
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; //状态更新,开始夹球
|
||||
|
||||
// /*运球*/
|
||||
@ -245,10 +282,11 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
break ;
|
||||
case UP_Adjust:
|
||||
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 .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init;
|
||||
// u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init;
|
||||
break ;
|
||||
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.m2006_Screw_init += c->Vy;
|
||||
u->PitchContext.PitchConfig.Pitch_angle += c->Vy;
|
||||
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -56,6 +56,7 @@ typedef struct {
|
||||
fp32 go1_init_position; // GO电机触发位置,0,初始位置
|
||||
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
|
||||
fp32 m2006_Screw_init;
|
||||
fp32 Pitch_angle;
|
||||
} PitchConfig_t;
|
||||
|
||||
/* 投球控制上下文 */
|
||||
@ -124,6 +125,14 @@ typedef struct
|
||||
pid_param_t Pitch_M2006_speed_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;
|
||||
PitchConfig_t PitchConfig_Config;
|
||||
|
||||
@ -134,7 +143,7 @@ typedef struct
|
||||
{
|
||||
fp32 ecd;
|
||||
fp32 rpm;
|
||||
|
||||
uint8_t id;
|
||||
fp32 orig_angle;
|
||||
fp32 last_angle;
|
||||
int32_t round_cnt;
|
||||
@ -152,9 +161,6 @@ typedef struct{
|
||||
/*投篮过程*/
|
||||
PitchContext_t PitchContext;
|
||||
|
||||
|
||||
UP_Imu_t pos088;
|
||||
|
||||
CMD_t *cmd;
|
||||
|
||||
struct{
|
||||
@ -166,15 +172,12 @@ typedef struct{
|
||||
|
||||
GO_MotorData_t *go_data;//存放go电机数据
|
||||
|
||||
DJmotor_feedback_t DJmotor_feedback[4];
|
||||
DJmotor_feedback_t DJmotor_feedback[6];
|
||||
|
||||
struct {
|
||||
uint32_t ecd;
|
||||
float angle;
|
||||
|
||||
}Encoder;
|
||||
|
||||
|
||||
}motorfeedback;
|
||||
|
||||
|
||||
@ -188,7 +191,10 @@ typedef struct{
|
||||
fp32 Pitch_M2006_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
fp32 Dribble_M3508_speed;//运球转速
|
||||
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
|
||||
|
||||
fp32 Shoot_Pitch_angle;
|
||||
}motor_target;
|
||||
|
||||
struct{
|
||||
@ -203,6 +209,14 @@ typedef struct{
|
||||
pid_type_def Pitch_M2006_angle;
|
||||
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;
|
||||
GO_MotorCmd_t go_cmd;
|
||||
|
||||
@ -212,7 +226,7 @@ typedef struct{
|
||||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||||
struct
|
||||
{
|
||||
fp32 DJfinal_out[4];
|
||||
fp32 DJfinal_out[6];
|
||||
fp32 final_VESC_5065_M1out;
|
||||
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 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_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);
|
||||
|
||||
|
||||
|
||||
@ -12,11 +12,13 @@
|
||||
#define CAN_MOTOR_CUR_RES (16384)
|
||||
|
||||
|
||||
//#define CAN_GM6020_CTRL_ID_BASE (0x1ff) //禁用
|
||||
#define CAN_PITCH6020_CTRL_ID (0x2ff)
|
||||
|
||||
#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)
|
||||
|
||||
@ -118,12 +120,11 @@ static void CAN_CAN2RxFifoMsgPendingCallback(void) {
|
||||
raw_rx2.hcan =BSP_CAN_GetHandle(BSP_CAN_2);
|
||||
osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
|
||||
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;
|
||||
|
||||
@ -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 ));
|
||||
}
|
||||
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) {
|
||||
if (output == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
int16_t motor1, motor2, motor3, motor4, motor5, motor6, motor7;
|
||||
int16_t motor1, motor2, motor3, motor4;
|
||||
|
||||
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:
|
||||
motor1 =
|
||||
(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,
|
||||
&(can->mailbox.chassis));
|
||||
break;
|
||||
case CAN_MOTOR_UP3508:
|
||||
case CAN_MOTOR_UP3508_Dribble:
|
||||
motor1 =
|
||||
(int16_t)(output->motor_UP3508.named.m1);
|
||||
(int16_t)(output->motor_UP3508_Dribble.named.m1);
|
||||
motor2 =
|
||||
(int16_t)(output->motor_UP3508.named.m2);
|
||||
(int16_t)(output->motor_UP3508_Dribble.named.m2);
|
||||
motor3 =
|
||||
(int16_t)(output->motor_UP3508.named.m3);
|
||||
(int16_t)(output->motor_UP3508_Dribble.named.m3);
|
||||
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.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
@ -263,36 +236,40 @@ 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[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,
|
||||
&(can->mailbox.up));
|
||||
break;
|
||||
case CAN_MOTOR_PITCH6020:
|
||||
motor5 =
|
||||
(int16_t)(output->pitch6020.named.m1);
|
||||
motor6 =
|
||||
(int16_t)(output->pitch6020.named.m2);
|
||||
motor7 =
|
||||
(int16_t)(output->pitch6020.named.m3);
|
||||
raw_tx.tx_header.StdId = CAN_PITCH6020_CTRL_ID;
|
||||
case CAN_MOTOR_UP3508_SHOOT:
|
||||
motor1 =
|
||||
(int16_t)(output->motor_UP3508_shoot.named.m1);
|
||||
motor2 =
|
||||
(int16_t)(output->motor_UP3508_shoot.named.m2);
|
||||
motor3 =
|
||||
(int16_t)(output->motor_UP3508_shoot.named.m3);
|
||||
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.RTR = CAN_RTR_DATA;
|
||||
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
|
||||
|
||||
raw_tx.tx_data[0] = (uint8_t)((motor5 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[1] = (uint8_t)(motor5 & 0xFF);
|
||||
raw_tx.tx_data[2] = (uint8_t)((motor6 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[3] = (uint8_t)(motor6 & 0xFF);
|
||||
raw_tx.tx_data[4] = (uint8_t)((motor7 >> 8) & 0xFF);
|
||||
raw_tx.tx_data[5] = (uint8_t)(motor7 & 0xFF);
|
||||
raw_tx.tx_data[6] = 0;
|
||||
raw_tx.tx_data[7] = 0;
|
||||
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->pitch6020),
|
||||
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_UP3508_shoot),
|
||||
&raw_tx.tx_header, raw_tx.tx_data,
|
||||
&(can->mailbox.chassis));
|
||||
&(can->mailbox.up));
|
||||
break;
|
||||
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -379,16 +356,6 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
|
||||
// 处理CAN1消息
|
||||
if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_1)) {
|
||||
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_M2_ID:
|
||||
case CAN_CHASSIS_M3508_M3_ID:
|
||||
@ -408,30 +375,35 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
|
||||
// 处理CAN2消息
|
||||
if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_2)) {
|
||||
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_M2_ID:
|
||||
case CAN_UP_M3508_M3_ID:
|
||||
case CAN_UP_M3508_M4_ID:
|
||||
|
||||
// 存储消息到对应的电机结构体中
|
||||
index = can_rx->rx_header.StdId - CAN_UP_M3508_M1_ID;
|
||||
can->recive_flag |= 1 << (index);
|
||||
CAN_DJIMotor_Decode(&(can->motor.up_motor3508.as_array[index]), can_rx->rx_data);
|
||||
can->recive_flag |= 1 << ((index+4));
|
||||
CAN_DJIMotor_Decode(&(can->motor.up_dribble_motor3508.as_array[index]), can_rx->rx_data);
|
||||
detect_hook(CHASSIS3508M1_TOE + index);
|
||||
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:
|
||||
// 存储消息到sickfed结构体中
|
||||
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
|
||||
break;
|
||||
case CAN_Encoder_ID:
|
||||
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
|
||||
can->recive_flag |= 1 << 10;
|
||||
|
||||
break ;
|
||||
|
||||
default:
|
||||
|
||||
@ -10,28 +10,18 @@ typedef enum {
|
||||
CAN_CHASSIS_M3508_M3_ID = 0x203, /* 3 */
|
||||
CAN_CHASSIS_M3508_M4_ID = 0x204, /* 4 */
|
||||
|
||||
/**/
|
||||
CAN_G6020_AgvM1 =0x205,
|
||||
CAN_G6020_AgvM2 =0x206,
|
||||
CAN_G6020_AgvM3 =0x207,
|
||||
CAN_G6020_AgvM4 =0x208,
|
||||
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_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_UP_M3508_M5_ID = 0x205, /* 1 */
|
||||
CAN_UP_M3508_M6_ID = 0x206, /* 2 */
|
||||
|
||||
CAN_SICK_ID=0x301,
|
||||
|
||||
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_M2_MSG1 =0x90c,
|
||||
CAN_VESC5065_M3_MSG1 =0x90d,
|
||||
@ -46,10 +36,12 @@ typedef enum {
|
||||
|
||||
|
||||
typedef enum {
|
||||
CAN_MOTOR_PITCH6020,
|
||||
CAN_MOTOR_5065,
|
||||
CAN_MOTOR_CHASSIS3508,
|
||||
CAN_MOTOR_UP3508,
|
||||
|
||||
CAN_MOTOR_UP3508_SHOOT,
|
||||
CAN_MOTOR_UP3508_Dribble,
|
||||
|
||||
CAN_MOTOR_GROUT_NUM,
|
||||
} CAN_MotorGroup_e;
|
||||
|
||||
@ -85,11 +77,10 @@ typedef union{
|
||||
}Vesc_ByteGet;
|
||||
|
||||
typedef struct {
|
||||
// CAN_DJIOutput_t chassis6020;
|
||||
CAN_VescOutput_t chassis5065;
|
||||
CAN_DJIOutput_t motor_CHASSIS3508;
|
||||
CAN_DJIOutput_t motor_UP3508;
|
||||
CAN_DJIOutput_t pitch6020;
|
||||
CAN_DJIOutput_t motor_UP3508_shoot;
|
||||
CAN_DJIOutput_t motor_UP3508_Dribble;
|
||||
|
||||
} CAN_Output_t;
|
||||
|
||||
@ -107,11 +98,12 @@ typedef struct {
|
||||
} CAN_RawTx_t;
|
||||
|
||||
typedef struct {
|
||||
// BSP_CAN_t chassis6020;//禁用
|
||||
BSP_CAN_t chassis5065;
|
||||
BSP_CAN_t motor_CHASSIS3508;
|
||||
BSP_CAN_t motor_UP3508;
|
||||
BSP_CAN_t pitch6020;
|
||||
BSP_CAN_t motor_UP3508_shoot;
|
||||
BSP_CAN_t motor_UP3508_Dribble;
|
||||
|
||||
|
||||
BSP_CAN_t sick;
|
||||
BSP_CAN_t encoder;
|
||||
|
||||
@ -147,7 +139,7 @@ typedef union {
|
||||
CAN_MotorFeedback_t m3;
|
||||
CAN_MotorFeedback_t m4;
|
||||
} named;
|
||||
} CAN_ChassisMotor_t;
|
||||
} CAN_Data_Motor_t;
|
||||
|
||||
typedef union {
|
||||
CAN_MotorFeedback_t as_array[4];
|
||||
@ -161,11 +153,14 @@ typedef union {
|
||||
|
||||
|
||||
typedef struct {
|
||||
CAN_ChassisMotor_t chassis6020;
|
||||
CAN_ChassisMotor_t chassis5065;
|
||||
CAN_ChassisMotor_t up_motor3508;
|
||||
CAN_ChassisMotor_t chassis_motor3508;
|
||||
CAN_ChassisMotor_t pit6020;
|
||||
CAN_Data_Motor_t chassis6020;
|
||||
CAN_Data_Motor_t chassis5065;
|
||||
/*1是指用来运球的，2用来扳机和俯仰*/
|
||||
CAN_Data_Motor_t up_shoot_motor3508;
|
||||
CAN_Data_Motor_t up_dribble_motor3508;
|
||||
|
||||
CAN_Data_Motor_t chassis_motor3508;
|
||||
CAN_Data_Motor_t pit6020;
|
||||
|
||||
} CAN_Motor_t;
|
||||
|
||||
@ -179,7 +174,6 @@ typedef struct {
|
||||
struct {
|
||||
uint32_t chassis;
|
||||
uint32_t up;
|
||||
|
||||
} mailbox;
|
||||
osMessageQueueId_t msgq_raw;
|
||||
|
||||
|
||||
@ -56,19 +56,20 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
||||
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) {
|
||||
CAN_DJIMotor_Control(CAN_MOTOR_PITCH6020,&can_out,&can);
|
||||
}
|
||||
//底盘4个
|
||||
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
|
||||
&(can_out.motor_CHASSIS3508), 0, 0) == osOK) {
|
||||
CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS3508,&can_out,&can);
|
||||
}
|
||||
if (osMessageQueueGet(task_runtime.msgq.can.output.up3508,
|
||||
&(can_out.motor_UP3508), 0, 0) == osOK) {
|
||||
CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can);
|
||||
//上层运球4个，发射2个
|
||||
if (osMessageQueueGet(task_runtime.msgq.can.output.up_dribble_3508,
|
||||
&(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; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
||||
}
|
||||
|
||||
@ -88,12 +88,6 @@ void Task_Chassis(void *argument)
|
||||
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
|
||||
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[1] = chassis.vofa_send[1];
|
||||
vofa_send[2] = chassis.vofa_send[2];
|
||||
|
||||
@ -59,9 +59,9 @@ void Task_Init(void *argument) {
|
||||
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 =
|
||||
task_runtime.msgq.can.output.up_shoot_3508 =
|
||||
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);
|
||||
|
||||
|
||||
|
||||
@ -71,13 +71,6 @@ void Task_up(void *argument)
|
||||
|
||||
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上设备数据获取*/
|
||||
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 );//清空队列
|
||||
osMessageQueuePut(task_runtime.msgq.can.output.up3508, &UP_CAN_out.motor_UP3508 , 0, 0); //发送数据
|
||||
osMessageQueueReset(task_runtime.msgq.can.output.up_dribble_3508 );//运球
|
||||
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);
|
||||
|
||||
|
||||
|
||||
@ -78,9 +78,8 @@ typedef struct {
|
||||
|
||||
osMessageQueueId_t shoot5065;
|
||||
osMessageQueueId_t chassis3508;
|
||||
osMessageQueueId_t pitch6020;
|
||||
osMessageQueueId_t up3508;
|
||||
|
||||
osMessageQueueId_t up_dribble_3508;
|
||||
osMessageQueueId_t up_shoot_3508;
|
||||
} output;
|
||||
struct {
|
||||
/*均包括所有数据，选择调用*/
|
||||
|
||||
Loading…
Reference in New Issue
Block a user