diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h
index 19445a7..d9fcec8 100644
--- a/Core/Inc/FreeRTOSConfig.h
+++ b/Core/Inc/FreeRTOSConfig.h
@@ -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
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 3ee1328..9610097 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -195,6 +195,16 @@
1
cmd_fromnuc
+
+ 9
+ 1
+ up_cmd,0x0A
+
+
+ 10
+ 1
+ can_out
+
0
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 5768f97..a717ef1 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/R2.ioc b/R2.ioc
index 1e268a9..730c0b8 100644
--- a/R2.ioc
+++ b/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
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index f05467b..338b2e3 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -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);
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index 31a6dd8..a6c3e11 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -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{
diff --git a/User/Module/config.c b/User/Module/config.c
index 263d6ec..d0e7dd9 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -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,
},
@@ -113,18 +143,17 @@ 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,
- .encoder=BSP_CAN_2,
+ .encoder=BSP_CAN_2,
},
diff --git a/User/Module/up.c b/User/Module/up.c
index 1019da9..568ace0 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -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,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 .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;
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_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;
+
}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/User/Module/up.h b/User/Module/up.h
index efd0b09..b394340 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -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;
/* 投球控制上下文 */
@@ -122,7 +123,15 @@ typedef struct
pid_param_t Shoot_M2006_angle_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;
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;
@@ -151,10 +160,7 @@ typedef struct{
DribbleContext_t DribbleContext;
/*投篮过程*/
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;
-
-
+ }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);
diff --git a/User/device/can_use.c b/User/device/can_use.c
index c6e47c2..6ebb56e 100644
--- a/User/device/can_use.c
+++ b/User/device/can_use.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,13 +120,12 @@ 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;
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 ));
}
-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,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[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;
-
- HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->pitch6020),
+
+ 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->motor_UP3508_shoot),
&raw_tx.tx_header, raw_tx.tx_data,
- &(can->mailbox.chassis));
- break;
+ &(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:
diff --git a/User/device/can_use.h b/User/device/can_use.h
index e6f1993..8bdfe47 100644
--- a/User/device/can_use.h
+++ b/User/device/can_use.h
@@ -9,29 +9,19 @@ typedef enum {
CAN_CHASSIS_M3508_M2_ID = 0x202, /* 2 */
CAN_CHASSIS_M3508_M3_ID = 0x203, /* 3 */
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_G6020_AgvM1 =0x205,
- 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_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;
@@ -178,8 +173,7 @@ typedef struct {
const CAN_Params_t *param;
struct {
uint32_t chassis;
- uint32_t up;
-
+ uint32_t up;
} mailbox;
osMessageQueueId_t msgq_raw;
diff --git a/User/task/can_task.c b/User/task/can_task.c
index c21e9cf..fcfc4ca 100644
--- a/User/task/can_task.c
+++ b/User/task/can_task.c
@@ -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); /* 运行结束，等待下一个周期唤醒 */
}
diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c
index be1d133..40f6485 100644
--- a/User/task/chassis_task.c
+++ b/User/task/chassis_task.c
@@ -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];
diff --git a/User/task/init.c b/User/task/init.c
index e63b787..4f20970 100644
--- a/User/task/init.c
+++ b/User/task/init.c
@@ -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);
diff --git a/User/task/up_task.c b/User/task/up_task.c
index 506576d..c784f32 100644
--- a/User/task/up_task.c
+++ b/User/task/up_task.c
@@ -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);
diff --git a/User/task/user_task.h b/User/task/user_task.h
index 2108081..928f2ef 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -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 {
/*均包括所有数据，选择调用*/