diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index 488f11b..28d9a59 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -471,3 +471,7 @@ [info] Log at : 2025/6/22|14:26:08|GMT+0800 +[info] Log at : 2025/6/23|20:59:34|GMT+0800 + +[info] Log at : 2025/6/24|03:28:50|GMT+0800 + diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index 42438c7..52b5e21 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 3 + 6 @@ -114,13 +114,13 @@ - BIN\CMSIS_AGDI.dll + STLink\ST-LINKIII-KEIL_SWO.dll 0 ST-LINKIII-KEIL_SWO - -U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -U00260035480000034E575152 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 @@ -140,12 +140,12 @@ 0 DLGUARM - + (105=-1,-1,-1,-1,0) 0 CMSIS_AGDI - -X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 @@ -158,73 +158,38 @@ 0 1 - can,0x0A + NUC_send,0x0A 1 1 - chassis,0x0A + SendBuffer,0x10 2 1 - UP,0x0A + f,0x0A 3 1 - NUC_send,0x0A + a,0x0A 4 1 - pid,0x0A + chassis 5 1 - pid_param,0x0A + cmd_fromnuc,0x0A 6 1 - cmd_rc,0x0A - - - 7 - 1 - cbuf,0x10 - - - 8 - 1 - cmd - - - 9 - 1 - rc_ctrl,0x0A - - - 10 - 1 - nuc_raw,0x0A - - - 11 - 1 - cmd_fromnuc,0x0A - - - 12 - 1 nucbuf,0x0A - - 13 - 1 - LD_raw,0x0A - 0 @@ -273,7 +238,7 @@ 0 0 2 - 5000000 + 1000000 diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 244cf2c..011c36e 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Algorithm/error_detect.h b/User/Algorithm/error_detect.h index 76f75a1..48e51db 100644 --- a/User/Algorithm/error_detect.h +++ b/User/Algorithm/error_detect.h @@ -1,6 +1,4 @@ #ifndef ERROR_DETECT_H - - #define ERROR_DETECT_H #include "struct_typedef.h" diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index b5a18dd..f0593f6 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -48,7 +48,7 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam)); PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam)); PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam)); - + PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param)); LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波 LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 @@ -60,8 +60,6 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre return CHASSIS_OK; } -fp32 gyro_kp = 1.0f; -fp32 PIAN = 0; fp32 pian_yaw; void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { fp64 Nor_Vx, Nor_Vy; @@ -107,7 +105,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { c->move_vec.Vw = ctrl->Vw * 6000; break; - case Chassis_Adjust: + case Chassis_Adjust_Sick: sick_calibration(c, ctrl, out); c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0; @@ -127,7 +125,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f); - c->NUC_send.send[0] = 1; + // c->NUC_send.send[0] = 1; break; case AUTO_MID360_Pitch: @@ -138,6 +136,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { c->move_vec.Vy = ctrl->Vx * 6000; break; + case PassBall: // + c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; + c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; + c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; + abs_limit_fp(&c->move_vec.Vx, 5000.0f); + abs_limit_fp(&c->move_vec.Vy, 5000.0f); + abs_limit_fp(&c->move_vec.Vw, 5000.0f); + break ; default: break; @@ -194,7 +200,7 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) fp32 diff_yaw = -(fp32)(sick1 - sick2); fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set); - fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); + fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0; @@ -219,37 +225,20 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) return CHASSIS_OK; } - -pid_type_def pid; -pid_param_t pid_param={ - .p = 10.0f, - .i = 0.00f, - .d = 0.00f, - .i_limit = 2000.0f, - .out_limit =5000.0f, -}; - fp32 cur_angle,pian_angel; int8_t Chassis_AngleCompensate(Chassis_t *c) { if (c == NULL) return CHASSIS_ERR_NULL; -// int is_in;//是否初始化 -// if(is_in==0){ - PID_init(&pid,PID_POSITION,&pid_param); -// is_in=1; -// } - - - + static fp32 pian_angel,cur_angle; if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0)) { - pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); } else { cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); - pian_angel=0; + pian_angel=0; } - pian_yaw = PID_calc(&pid,pian_angel,0); + pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0); } diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index e2e2e4a..6020adc 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -82,8 +82,8 @@ typedef struct pid_param_t Sick_CaliWparam; pid_param_t Sick_CaliYparam; - pid_param_t Sick_CaliXparam; - + pid_param_t Sick_CaliXparam; + pid_param_t Chassis_AngleAdjust_param; sickparam_t sickparam; @@ -100,6 +100,7 @@ typedef struct }ChassisMove_Vec; typedef struct { + fp32 send[4]; }NUC_send_t; @@ -154,7 +155,7 @@ typedef struct{ /*存在较低误差*/ pid_type_def chassis_PICKWzPID_LOW; - + pid_type_def Chassis_AngleAdjust; pid_type_def SickCaliWzPID; pid_type_def SickCaliVxPID; diff --git a/User/Module/config.c b/User/Module/config.c index ef628ab..d748f9c 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -58,28 +58,6 @@ static const ConfigParam_t param ={ .out_limit = 3000.0f, }, -.Dribble_M3508_speed_param={ //三摩擦速度环 - .p = 30.0f, - .i = 0.45f, - .d = 0.0f, - .i_limit = 3000.0f, - .out_limit = 5000.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={ .id =0, @@ -93,21 +71,8 @@ static const ConfigParam_t param ={ }, -/*上层其他参数*/ - /*运球*/ - .DribbleCfg = { - .dribble_flag=0, - .m3508_init_ang = -5, - .m3508_translate_angle = 1000, - .rev_spd=-2000, - .spd= 4200, // 转动速度 - .init_spd=0, +/*上层其他参数*/ - .light_3508_flag=0,//3508平移处的光电，0初始，1到位置 - .light_ball_flag=0,//检测球的flag - - -}, /*投球*/ .PitchCfg = { .m2006_init =-150, //释放的角度 @@ -167,11 +132,19 @@ static const ConfigParam_t param ={ .i_limit = 500.0f, .out_limit =3000.0f, }, + .Chassis_AngleAdjust_param={ + .p = 10.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit =1000.0f, + + }, .sickparam={ .w_error=5, .xy_error=5, - .x_set=927, - .y_set=1255, + .x_set=600, + .y_set=100, }, diff --git a/User/Module/up.c b/User/Module/up.c index f9d6b73..e5608d7 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -5,11 +5,6 @@ #include "bsp_delay.h" /*接线 -上层用到三个光电 -1.PE9 发射用 输出高电平 LIGHTA -2.PE11 运球上3508 输出高电平 LIGHTB -3.PE13 三摩擦检测球 输出高电平LIGHTC - */ @@ -41,24 +36,10 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) 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 )); - for(int i=0;i<3;i++){ - PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param )); - } u->go_cmd =u->param ->go_cmd ; LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f); - - // 初始化上层状态机 - if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球 - u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值 - u->DribbleCont .DribbleState = DRIBBLE_PREPARE; - u->DribbleCont .is_init = 1; - - } if (!u->Pitch_Cfg .is_init) { u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值 @@ -95,10 +76,6 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { } u->cmd =c; - /*光电状态更新*/ - u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); - u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); - return 0; } @@ -202,21 +179,10 @@ 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_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]); - DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]); - DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]); - 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,0x206,&u->motorfeedback .DJmotor_feedback[5] , -// &u->pid .Pitch_M2006_angle , -// &u->pid .Pitch_M2006_speed , -// u->motor_target .Pitch_M2006_angle ); DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed, (PID_calc (&(u->pid .Pitch_M2006_angle ), u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle)) @@ -252,7 +218,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) /*指针简化*/ PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; - DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig; up_motor_target_t *target=&u->motor_target ; static int is_pitch=1; @@ -278,17 +243,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ; u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 - /*运球*/ - - RELAY1_TOGGLE(0);//关闭气缸 - for(int i=0;i<3;i++){ - target->Dribble_M3508_speed[i]=0; - } - target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle; - u->DribbleCont .DribbleState = DRIBBLE_PREPARE; //重置最初状态 - - - break; case Pitch : @@ -309,17 +263,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos; target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; break ; - case Dribble: - { - - if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){ - u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN; - } - //光电状态更新 - Dribble_Process(u,out); - }break ; - - + default: break; @@ -334,13 +278,9 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) { u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 } - /*根据距离实时计算所需pos*/ - + /*根据距离实时计算所需pos*/ pitch_cfg ->go_release_pos=c->pos; -// if(u->CoContext .CoState == CoPITCH){ -// u->CoContext.CoState=CoPITCH_MID360; -// } -// Co_Process(u,out); + Pitch_Process(u,out); break ; @@ -349,9 +289,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) target->Shoot_M2006_angle =pitch_cfg ->m2006_init ; u->Pitch_Cfg .PitchState = PITCH_PREPARE; - u->DribbleCont .DribbleState = DRIBBLE_PREPARE; - - + break ; } @@ -371,8 +309,6 @@ return 0; - - int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) { @@ -425,187 +361,3 @@ return 0; - - - - - -int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) -{ - static int common_speed_flag=0;//是否共速 - static int common_speed_reverse_flag=0;//是否共速 - - DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig; - up_motor_target_t *target=&u->motor_target ; - DribbleState_t *DribbleState=&u->DribbleCont.DribbleState; - - switch (u->DribbleCont.DribbleState) { - - case DRIBBLE_TRANSLATE: - target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动 - - if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 - { - u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态 - } - break; - case DRIBBLE_PROCESS_DOWN: - - target->Dribble_M3508_speed[0]=DribbleCfg->spd; - target->Dribble_M3508_speed[1]=DribbleCfg->spd; - target->Dribble_M3508_speed[2]=DribbleCfg->spd; - - if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm, - u->motorfeedback .DJmotor_feedback [1].rpm, - u->motorfeedback .DJmotor_feedback [2].rpm, - DribbleCfg->spd, - DribbleCfg->spd, - DribbleCfg->spd, 50.0f,50) - ) { - RELAY1_TOGGLE(1); //速度达到后打开气缸 - common_speed_flag =1; - } - if(common_speed_flag){ - if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转 - target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd; - target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd; - target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd; - - *DribbleState=DRIBBLE_PROCESS_UP; - - } - - } - - - break; - case DRIBBLE_PROCESS_UP: - common_speed_flag =0; - - if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&& - (u->motorfeedback .DJmotor_feedback [1].rpm<-500)&& - (u->motorfeedback .DJmotor_feedback [2].rpm<-500) - ){ - common_speed_reverse_flag=1; - - } - if(common_speed_reverse_flag){ - if(DribbleCfg->light_ball_flag == 1){ - *DribbleState=DRIBBLE_DONE; - RELAY1_TOGGLE(0); //关闭气缸 - } - } - break ; - case DRIBBLE_DONE: - common_speed_reverse_flag=0; - for(int i=0;i<3;i++){ - target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0 - } - - /*标志位清零*/ - DribbleCfg->light_ball_flag=0; - DribbleCfg->light_3508_flag=0; - - - break; - default: - // 处理未知状态 - break; - } - return 0; -} -// CoPREPARE, // 准备阶段 -// CoTRANSLATE_OUT,//上方平移,去运球 -// CoDRIBBLE, //运球阶段 -// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 -// CoPITCH, //发射 -// CoDONE, -//int8_t Trans_Process (UP_t *u, CAN_Output_t *out) -//{ -// -//} -int8_t Co_Process(UP_t *u, CAN_Output_t *out) -{ - switch(u->CoContext .CoState ) - { - case CoPREPARE: - - break ; - case CoTRANSLATE_OUT: - u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 - - if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主 - { - u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态 - u->Pitch_Cfg .PitchState = PITCH_START; - - u->CoContext .CoState =CoDRIBBLE; - } - break; - - case CoDRIBBLE: - Dribble_Process(u,out); //状态停在DRIBBLE_DONE - Pitch_Process(u,out); //状态停在PITCH_LAUNCHING - //状态停在对应位置时,平移去给发射送球 - if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING) - { - u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang; - } - if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f)) - { - u->motor_target.Dribble_M3508_speed[0]=1000; - u->motor_target.Dribble_M3508_speed[1]=1000; - u->motor_target.Dribble_M3508_speed[2]=1000; - - u->CoContext .CoState =CoTRANSLATE_IN; - } - - break ; - case CoTRANSLATE_IN: - - if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm, - u->motorfeedback .DJmotor_feedback [1].rpm, - u->motorfeedback .DJmotor_feedback [2].rpm, - u->motor_target.Dribble_M3508_speed [0], - u->motor_target.Dribble_M3508_speed [1], - u->motor_target.Dribble_M3508_speed[2], - 50.0f,100) - ) { - RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射 - } - if(u->DribbleCont .DribbleConfig.light_ball_flag == 0) - { - u->motor_target.Dribble_M3508_speed[0]=0; - u->motor_target.Dribble_M3508_speed[1]=0; - u->motor_target.Dribble_M3508_speed[2]=0; - u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 - - } - - if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端 - { - - u->CoContext .CoState =CoPITCH; - - } - break ; - - case CoPITCH: - - - - case CoPITCH_MID360: - - u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init; - - break ; - break ; - - } - - - - - -} - diff --git a/User/Module/up.h b/User/Module/up.h index b3265cc..219461a 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -27,27 +27,6 @@ */ -/*配合过程状态*/ -typedef enum { - CoPREPARE, // 准备阶段 - CoTRANSLATE_OUT,//上方平移,去运球 - CoDRIBBLE, //运球和蓄力阶段,再平移回去 - CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 - CoPITCH, //发射 - CoPITCH_MID360, //雷达发射 - CoDONE, - - -}CoState_t; -/*总配合上下文*/ -typedef struct { - CoState_t CoState; - - uint8_t is_init ; -} CoCon_t; - - - /* 投球状态定义 */ typedef enum { @@ -72,16 +51,7 @@ typedef struct { fp32 go_release_pos;//go松开，发射位置 } PitchCfg_t; -//fp32 go_pull_pos; -// fp32 m2006_init; // M2006初始角度 -// fp32 m2006_trig; // M2006触发角度 -// fp32 go_init; // GO初始位置 -// fp32 go_release; // GO释放阈值 -// fp32 go_recv; // GO接球位置 -// fp32 screw_init; // 螺杆初始值 -// fp32 pitch_ang; // 投球角度 -// fp32 pull_spd; // 拉动速度 -/* 投球控制上下文 */ + typedef struct { PitchState_t PitchState; PitchCfg_t PitchConfig; @@ -99,33 +69,6 @@ typedef enum { DRIBBLE_DONE // 运球结束 } DribbleState_t; -/* 参数配置结构体 */ -typedef struct { - int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到 - - fp32 m3508_init_ang; // 3508平移前位置 - fp32 m3508_translate_angle; // 平移后位置 - fp32 init_spd; - fp32 spd; // 转动速度 - fp32 rev_spd; - - - /*光电标志位，初始值均为0，触发为1*/ - int light_3508_flag;//3508平移处的光电，0初始，1到位置 - int light_ball_flag;//检测球的flag - -} DribbleCfg_t; - -/* 状态机上下文 */ -typedef struct { - DribbleState_t DribbleState; - DribbleCfg_t DribbleConfig; - - uint8_t is_init; - -} DribbleCont_t; - - typedef struct { BMI088_t bmi088; @@ -147,14 +90,7 @@ typedef struct 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;//平移用的角度环 - - DribbleCfg_t DribbleCfg; PitchCfg_t PitchCfg; GO_MotorCmd_t go_cmd; @@ -168,8 +104,6 @@ typedef struct{ fp32 Pitch_M2006_angle; fp32 go_shoot; - fp32 Dribble_M3508_speed[3];//运球转速 - fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度 fp32 Shoot_Pitch_angle; @@ -189,11 +123,6 @@ typedef struct{ pid_type_def Pitch_Angle_M2006_speed; - pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环 - - pid_type_def Dribble_translate_M3508_speed;//平移用的速度环 - pid_type_def Dribble_translate_M3508_angle;//平移用的角度环 - }up_pid_t; typedef struct @@ -213,9 +142,6 @@ typedef struct{ uint8_t up_task_run; const UP_Param_t *param; - CoCon_t CoContext; - /*运球过程*/ - DribbleCont_t DribbleCont; /*投篮过程*/ Pitch_Cfg_t Pitch_Cfg; @@ -272,9 +198,7 @@ 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 Dribble_Process(UP_t *u, CAN_Output_t *out); int8_t Pitch_Process(UP_t *u, CAN_Output_t *out); -int8_t Co_Process(UP_t *u, CAN_Output_t *out); #endif diff --git a/User/device/can_use.c b/User/device/can_use.c index eb86332..b00a8d4 100644 --- a/User/device/can_use.c +++ b/User/device/can_use.c @@ -2,6 +2,9 @@ /* CAN总线上的设备1到7 将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制 + + + 底盘4个3508，3个dt35接收板，终端电阻勿开，会出现数据收发异常情况 */ /* Includes ----------------------------------------------------------------- */ @@ -11,13 +14,9 @@ #include "error_detect.h" #define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */ #define CAN_MOTOR_CUR_RES (16384) - - #define CAN_PITCH6020_CTRL_ID (0x2ff) - #define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200) - -/*CAN2上有6个大疆电机*/ +/*CAN2上预留>4电机*/ #define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff) #define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200) @@ -57,20 +56,33 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback, feedback->torque_current = raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; } -static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, +static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id, const uint8_t *raw) { if (feedback == NULL || raw == NULL) return; static uint16_t sick_rec[4]; + switch (id) + { + case CAN_SICK_ID_1: + feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]); + + break ; + + case CAN_SICK_ID_2: + feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]); + + break ; + + case CAN_SICK_ID_3: + feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]); + + break ; + + case CAN_SICK_ID_4: + feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]); + + break ; + } - sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]); - sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]); - sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]); - sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]); - - feedback ->raw_dis [0]= sick_rec[0]; - feedback ->raw_dis [1]= sick_rec[1]; - feedback ->raw_dis [2]= sick_rec[2]; - feedback ->raw_dis [3]= sick_rec[3]; } void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, const uint8_t *raw) { @@ -282,26 +294,7 @@ void CAN_Encoder_Control(CAN_t *can) break; } return DEVICE_OK; -} - -//用来问答接收来自sick的数据 -void CAN_Sick_Control(CAN_t *can) -{ - raw_tx.tx_header.StdId = 0x301; - raw_tx.tx_header.IDE = CAN_ID_STD; - raw_tx.tx_header.DLC = 0x08; - raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡ - raw_tx.tx_data[0] = 0x00; - raw_tx.tx_data[1] = 0x00; - raw_tx.tx_data[2] = 0x00; - raw_tx.tx_data[3] = 0x00; - raw_tx.tx_data[4] = 0x00; - raw_tx.tx_data[5] = 0x00; - raw_tx.tx_data[6] = 0x00; - raw_tx.tx_data[7] = 0x00; - - HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->sick),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.chassis)); -} +} int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){ if (output == NULL) return DEVICE_ERR_NULL; @@ -374,7 +367,19 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data); detect_hook(CHASSIS3508M1_TOE + index); break; - + case CAN_SICK_ID_1: + CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data); + break; + case CAN_SICK_ID_2: + CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data); + break; + case CAN_SICK_ID_3: + CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data); + break; + case CAN_SICK_ID_4: + CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data); + + break; default: break; @@ -405,9 +410,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) { CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data); break; - case CAN_SICK_ID: - 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; diff --git a/User/device/can_use.h b/User/device/can_use.h index 9339e47..82cb896 100644 --- a/User/device/can_use.h +++ b/User/device/can_use.h @@ -18,7 +18,10 @@ typedef enum { CAN_UP_M3508_M5_ID = 0x205, /* 1 */ CAN_UP_M3508_M6_ID = 0x206, /* 2 */ - CAN_SICK_ID=0x305, + CAN_SICK_ID_1=0x301, + CAN_SICK_ID_2=0x302, + CAN_SICK_ID_3=0x303, + CAN_SICK_ID_4=0x304, CAN_Encoder_ID=0x01, @@ -192,12 +195,10 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group,CAN_Output_t *output,CAN_t *c int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can); -void CAN_Sick_Control(CAN_t *can); - void CAN_Encoder_Control(CAN_t *can); void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, const uint8_t *raw) ; -static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, +static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id, const uint8_t *raw) ; #endif diff --git a/User/device/cmd.c b/User/device/cmd.c index c7b9a10..7646fbe 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -146,7 +146,7 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) { if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust; //左上，右上，手动调整 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust_Sick; //左上，右上，手动调整 if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust; } @@ -204,10 +204,10 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){ } else if(cmd ->CMD_CtrlType ==RCcontrol){ /*手动下的*/ - if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ; + if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Normal ; else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch; else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust; - else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust; + else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust_Sick; else cmd ->CMD_mode =Normal; } } diff --git a/User/device/cmd.h b/User/device/cmd.h index 6e7df12..d48c436 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -28,9 +28,11 @@ typedef enum{ AUTO_MID360_Adjust,//雷达调整 UP_Adjust,//上层调整 - Chassis_Adjust, + Chassis_Adjust_Sick, + + PassBall, - Dribble , //运球 + Pitch, //投篮,底盘锁定 diff --git a/User/device/nuc.c b/User/device/nuc.c index a1c723f..27eda9c 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -7,7 +7,7 @@ static volatile uint32_t drop_message = 0; static osThreadId_t thread_alert; uint8_t nucbuf[31]; -static char SendBuffer[19]; + char SendBuffer[7]; static void NUC_IdleCallback(void) { @@ -33,22 +33,22 @@ int8_t NUC_StartSending(fp32 *data) { union { - float x[4]; - char data[16]; + float x[1]; + char data[4]; }instance; - for (int i = 0; i < 4; i++) { - instance.x[i] = data[i]; - } +// for (int i = 0; i < 1; i++) { + instance.x[0] = data[0]; +// } - SendBuffer[0] = 0xFC; //发送ID + SendBuffer[0] = 0xFC; //帧头 SendBuffer[1] = 0x01; //控制帧 - for(int i = 2; i < 18; i++) + for(int i = 2; i < 6; i++) { SendBuffer[i] = instance.data[i-2]; } - SendBuffer[18] = 0xFD; //结束符 + SendBuffer[6] = 0xFD; //帧尾 if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)SendBuffer, @@ -146,6 +146,15 @@ int8_t NUC_RawParse(CMD_NUC_t *n){ n->MID360.angle = instance.x[4];// n->MID360.flag = nucbuf[23];// + + + + + + + + + break; diff --git a/User/device/rc.c b/User/device/rc.c index 84c5658..89066fa 100644 --- a/User/device/rc.c +++ b/User/device/rc.c @@ -1,5 +1,9 @@ /* - DR16接收机 + 遥控 + dr16：dbus 乐迪：s-ubs + dr16掉线不输出信号，可以使用超时检测方式判断掉线 + 即RC_SBUS_WaitDmaCplt + 乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式 */ diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 94de5f9..54c5af5 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -14,7 +14,7 @@ NUC_send_t NUC_send; #endif fp32 send[4]={11.0f,45.0,1.f,4.0f}; - +int a=0; void Task_nuc(void *argument){ (void)argument; /**/ @@ -33,6 +33,7 @@ void Task_nuc(void *argument){ task_runtime.last_up_time.nuc= tick; #endif + a++; NUC_StartReceiving(); NUC_StartSending(NUC_send.send); // NUC_StartSending (send); diff --git a/User/task/rc_task.c b/User/task/rc_task.c index 15586ef..f6d978e 100644 --- a/User/task/rc_task.c +++ b/User/task/rc_task.c @@ -33,7 +33,7 @@ static LD_Data_t LD; * \brief dr16接收机 * * \param argument 未使用 - */int aaaaaaaaaaaaaaaaaaa=0; + */ void Task_rc(void *argument) { (void)argument; /* 未使用，消除警告 */ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC; @@ -48,7 +48,6 @@ void Task_rc(void *argument) { #endif /* 开启DMA */ RC_StartDmaRecv(); -aaaaaaaaaaaaaaaaaaa++; if (RC_WaitDmaCplt(30)) { RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);