diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 5c9f8b8..60f8a97 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Module/config.c b/User/Module/config.c index 25b841a..65a65cd 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -97,7 +97,7 @@ static const ConfigParam_t param ={ /*运球*/ .DribbleConfig_Config = { .dribble_flag=0, - .m3508_init_angle = 50, + .m3508_init_angle = -5, .m3508_translate_angle = -930, .m3508_dribble_Reverse_speed=-3500, . m3508_dribble_speed= 4000, // 转动速度 @@ -112,7 +112,8 @@ static const ConfigParam_t param ={ .PitchConfig_Config = { .m2006_init_angle =-90, .m2006_trigger_angle =0, - .go1_init_position = -50, + .go1_init_position = -50, + .go1_Receive_ball = -5, //偏下 .go1_release_threshold =-210, .m2006_Screw_init=0, .Pitch_angle =56, diff --git a/User/Module/up.c b/User/Module/up.c index fe3a170..df7dcd1 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -276,7 +276,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) is_pitch=0; } //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删 u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 /*运球*/ @@ -285,7 +284,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) for(int i=0;i<3;i++){ u->motor_target.Dribble_M3508_speed[i]=0; } - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle; + u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle; u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态 @@ -321,10 +320,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE; } //光电状态更新 - u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); - u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); - a=HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); - b=HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); Dribble_Process(u,out); }break ; @@ -379,12 +374,14 @@ return 0; -int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) { switch(u->PitchContext .PitchState){ case PITCH_START: +// u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold; if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 @@ -437,7 +434,9 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) switch (u->DribbleContext.DribbleState) { case DRIBBLE_TRANSLATE: - if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f)) + u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + + if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 { u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态 } @@ -460,13 +459,12 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) common_speed_flag =1; } if(common_speed_flag){ - if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转 + if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转 u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP; - } } @@ -474,12 +472,14 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) break; case DRIBBLE_PROCESS_UP: - if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&& - (u->motorfeedback .DJmotor_feedback [1].rpm>0)&& - (u->motorfeedback .DJmotor_feedback [2].rpm>0) + common_speed_flag =0; + + if((u->motorfeedback .DJmotor_feedback [0].rpm<-2000)&& + (u->motorfeedback .DJmotor_feedback [1].rpm>2000)&& + (u->motorfeedback .DJmotor_feedback [2].rpm>2000) ){ - if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){ + if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){ u->DribbleContext .DribbleState=DRIBBLE_DONE; RELAY1_TOGGLE(0); //关闭气缸 } @@ -502,5 +502,89 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) } return 0; } +// CoPREPARE, // 准备阶段 +// CoTRANSLATE_OUT,//上方平移,去运球 +// CoDRIBBLE, //运球阶段 +// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 +// CoPITCH, //发射 +// CoDONE, + +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->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + + if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 + { + u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态 + u->PitchContext .PitchState = PITCH_START; + + u->CoContext .CoState =CoDRIBBLE; + } + break; + + case CoDRIBBLE: + Dribble_Process(u,out); //状态停在DRIBBLE_DONE + Pitch_Process(u,out); //状态停在PITCH_PULL_TRIGGER + //状态停在对应位置时,平移去给发射送球 + if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING) + { + u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle; + } + if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,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->DribbleContext .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->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + + } + + if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端 + { + u->CoContext .CoState =CoPITCH; + } + break ; + + case CoPITCH: + + u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; + + + break ; + + } + + + +} + diff --git a/User/Module/up.h b/User/Module/up.h index f06865c..d915ebf 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -26,16 +26,24 @@ 运行函数，switch(状态) 运行相应函数 - - - - - - - - - */ +/*配合过程状态,co是合作的意思*/ +typedef enum { + CoPREPARE, // 准备阶段 + CoTRANSLATE_OUT,//上方平移,去运球 + CoDRIBBLE, //运球和蓄力阶段,再平移回去 + CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 + CoPITCH, //发射 + CoDONE, + + +}CoState_t; +/*总配合上下文*/ +typedef struct { + CoState_t CoState; + + uint8_t is_initialized ; +} CoContext_t; @@ -46,7 +54,9 @@ typedef enum { PITCH_START, //启动，拉扳机 PITCH_PULL_TRIGGER, PITCH_LAUNCHING, // 发射中 + PITCH_COMPLETE // 完成 + } PitchState_t; /* 投球参数配置 */ @@ -55,8 +65,10 @@ typedef struct { fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机 fp32 go1_init_position; // GO电机触发位置,0,初始位置 fp32 go1_release_threshold; // go上升去合并扳机，扳机位置 + fp32 go1_Receive_ball; //用在配合过程，用来接平移后的球 fp32 m2006_Screw_init; fp32 Pitch_angle; + } PitchConfig_t; /* 投球控制上下文 */ @@ -108,8 +120,6 @@ typedef struct { uint8_t is_initialized; - - } DribbleContext_t; @@ -164,7 +174,8 @@ typedef struct{ uint8_t up_task_run; const UP_Param_t *param; - /*运球过程*/ + CoContext_t CoContext; + /*运球过程*/ DribbleContext_t DribbleContext; /*投篮过程*/ PitchContext_t PitchContext; @@ -262,7 +273,8 @@ int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *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,CMD_t *c); +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out); +int8_t Co_Process(UP_t *u, CAN_Output_t *out); #endif