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

增加配合，总过程

Browse Source
This commit is contained in:
ZHAISHUI04 2025-05-24 14:56:15 +08:00
parent 79f6f5f9c0
commit 7499e75a50
4 changed files with 126 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

Binary file not shown.

5
User/Module/config.c
View File

@ -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,

112
User/Module/up.c
View File

@ -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 ;
}
}

38
User/Module/up.h
View File

@ -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