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-16 19:34:06 +08:00
parent 47ac0cf2a0
commit b2efd7c822
5 changed files with 155 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
MDK-ARM/R2.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel> <nTsel>3</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
@ -140,7 +140,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name>(105=-1,-1,-1,-1,0)</Name> <Name></Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>

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 = { .DribbleConfig_Config = {
.dribble_flag=0, .dribble_flag=0,
.m3508_init_angle = 50, .m3508_init_angle = -5,
.m3508_translate_angle = -930, .m3508_translate_angle = -930,
.m3508_dribble_Reverse_speed=-3500, .m3508_dribble_Reverse_speed=-3500,
. m3508_dribble_speed= 4000, // 转动速度 . m3508_dribble_speed= 4000, // 转动速度
@ -112,7 +112,8 @@ static const ConfigParam_t param ={
.PitchConfig_Config = { .PitchConfig_Config = {
.m2006_init_angle =-170, .m2006_init_angle =-170,
.m2006_trigger_angle =0, .m2006_trigger_angle =0,
.go1_init_position = -50, .go1_init_position = -50,
.go1_Receive_ball = -5, //偏下
.go1_release_threshold =-210, .go1_release_threshold =-210,
.m2006_Screw_init=0, .m2006_Screw_init=0,
.Pitch_angle =56, .Pitch_angle =56,

146
User/Module/up.c
View File

@ -93,7 +93,10 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
} }
u->cmd =c; u->cmd =c;
/*光电状态更新*/
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);
return 0; return 0;
} }
@ -270,7 +273,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
is_pitch=0; is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删 } //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; 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; //状态更新,开始夹球 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/ /*运球*/
@ -279,18 +281,23 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
for(int i=0;i<3;i++){ for(int i=0;i<3;i++){
u->motor_target.Dribble_M3508_speed[i]=0; 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; //重置最初状态 u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break; break;
case Pitch : case Pitch :
// if (u->PitchContext .PitchState ==PITCH_PREPARE)
// {
// u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
// }
// Pitch_Process(u,out);
if (u->PitchContext .PitchState ==PITCH_PREPARE) if (u->PitchContext .PitchState ==PITCH_PREPARE)
{ {
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 u->CoContext .CoState =CoTRANSLATE_OUT;
} }
Co_Process(u,out);
Pitch_Process(u,out,c);
break ; break ;
case UP_Adjust: //测试用 case UP_Adjust: //测试用
@ -308,10 +315,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE; 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); Dribble_Process(u,out);
}break ; }break ;
@ -341,12 +344,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){ switch(u->PitchContext .PitchState){
case PITCH_START: 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; u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
@ -361,11 +366,21 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{ {
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball;
if(is_reached (u->motorfeedback .go_data ->Pos ,u->motor_target .go_shoot ,1.0f))
{
u->PitchContext .PitchState=PITCH_LAUNCHING;
}
} }
break ;
break ;
case PITCH_LAUNCHING: //等待发射
break ;
case PITCH_COMPLETE: //发射完成
break ;
} }
@ -389,7 +404,9 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
switch (u->DribbleContext.DribbleState) { switch (u->DribbleContext.DribbleState) {
case DRIBBLE_TRANSLATE: 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;//到达位置后，转移状态 u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
} }
@ -412,13 +429,12 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
common_speed_flag =1; common_speed_flag =1;
} }
if(common_speed_flag){ 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[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[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[2]=-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; u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
} }
} }
@ -426,12 +442,14 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
break; break;
case DRIBBLE_PROCESS_UP: case DRIBBLE_PROCESS_UP:
if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&& common_speed_flag =0;
(u->motorfeedback .DJmotor_feedback [1].rpm>0)&&
(u->motorfeedback .DJmotor_feedback [2].rpm>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; u->DribbleContext .DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸 RELAY1_TOGGLE(0); //关闭气缸
} }
@ -454,5 +472,89 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
} }
return 0; 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(状态) 运行相应函数 运行函数，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_START, //启动，拉扳机
PITCH_PULL_TRIGGER, PITCH_PULL_TRIGGER,
PITCH_LAUNCHING, // 发射中 PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成 PITCH_COMPLETE // 完成
} PitchState_t; } PitchState_t;
/* 投球参数配置 */ /* 投球参数配置 */
@ -55,8 +65,10 @@ typedef struct {
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机 fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置 fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置 fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
fp32 go1_Receive_ball; //用在配合过程，用来接平移后的球
fp32 m2006_Screw_init; fp32 m2006_Screw_init;
fp32 Pitch_angle; fp32 Pitch_angle;
} PitchConfig_t; } PitchConfig_t;
/* 投球控制上下文 */ /* 投球控制上下文 */
@ -108,8 +120,6 @@ typedef struct {
uint8_t is_initialized; uint8_t is_initialized;
} DribbleContext_t; } DribbleContext_t;
@ -164,7 +174,8 @@ typedef struct{
uint8_t up_task_run; uint8_t up_task_run;
const UP_Param_t *param; const UP_Param_t *param;
/*运球过程*/ CoContext_t CoContext;
/*运球过程*/
DribbleContext_t DribbleContext; DribbleContext_t DribbleContext;
/*投篮过程*/ /*投篮过程*/
PitchContext_t PitchContext; 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 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 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 #endif