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

重新写的流程，状态机实现，部分数据需要改

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-05 01:31:02 +08:00
parent fe1c02b130
commit 0aee725ece
7 changed files with 142 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -120,7 +120,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key> <Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U00160029510000164E574E32 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131091 -TC12000000 -TT12000000 -TP21 -TDS8005 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name> <Name>-U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131091 -TC12000000 -TT12000000 -TP21 -TDS8005 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -235,11 +235,6 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>flaggg,0x0A</ItemText> <ItemText>flaggg,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>ddd</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<WatchWindow2> <WatchWindow2>
<Ww> <Ww>

BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
View File

Binary file not shown.

7
User/Module/config.c
View File

@ -90,6 +90,13 @@ static const ConfigParam_t param_chassis ={
.flip_timing = 200, .flip_timing = 200,
.release_threshold = -1.2f, .release_threshold = -1.2f,
}, },
/*投球*/
.PitchConfig_Config = {
.m2006_init_angle =-140,
.m2006_trigger_angle =0,
.go1_init_position = 0,
.go1_release_threshold =-2050,
},
}, },

205
User/Module/up.c
View File

@ -11,23 +11,7 @@
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度 #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 MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
/*投球*/
#define M2006_INIT_ANGLE (-120) //初始和发射
#define GO1_INIT_POSITION (0) //go初始
#define M2006_TRIGGER (0) //扳机
#define GO_POSITION_TRIGGER (-300) //go发射控制值
#define GO_POSITION_PITCH_FD (-4.8f) //反馈检测
/*运球*/
#define M3508_INIT_ANGLE (0) //3508
#define GO2_INIT_ANGLE (0) //go2的初始角度
#define M3508_HIGH_ANGLE (900) //3508，升起角度
#define GO2_Flip_timing (200) // go的翻转时机，以3508角度反馈值为准
#define GO2_Flip_ANGLE (-160) //go2翻转角度
#define BALL_REL_TIME (-1.2) //球放开时机，以go的反馈值为准
// 定义继电器控制 // 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) #define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) #define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
@ -56,14 +40,21 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
} }
// 初始化状态机 // 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机 if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = STATE_GRAB_BALL; //状态更新,开始夹球 u->DribbleContext .DribbleState = Dribble_PREPARE;
u->DribbleContext .is_initialized = 1; u->DribbleContext .is_initialized = 1;
u->DribbleContext .Dribble_run_num =0;//记录运行次数
} }
if (!u->PitchContext .is_initialized) {
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
u->PitchContext .is_initialized = 1;
u->PitchContext .Pitch_run_num =0;//记录运行次数
}
} }
@ -95,7 +86,7 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
/*上层电机控制,使用can1的id1和2*/ /*上层大疆电机角度控制,使用can1的id1和2*/
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) { int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
// 获取当前编码器角度 // 获取当前编码器角度
int8_t cnt=0; int8_t cnt=0;
@ -186,7 +177,8 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm; u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm; u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
return 0;
} }
@ -200,6 +192,8 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
u->final_out .final_pitchout = u->final_out .final_pitchout =
PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed); PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
u->motor_target .rotor_pit6020angle =angle ; u->motor_target .rotor_pit6020angle =angle ;
return 0;
} }
/*go电机控制*/ /*go电机控制*/
@ -207,6 +201,7 @@ int8_t GO_SendData(UP_t *u,int id,float pos)
{ {
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id] .T ,u->param->go_param[id] .W ,AngleChange(RADIAN,pos),u->param->go_param [id].K_P ,u->param->go_param[id] .K_W ); GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id] .T ,u->param->go_param[id] .W ,AngleChange(RADIAN,pos),u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
return 0;
} }
@ -233,7 +228,8 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ; out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ; out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
return 0;
} }
@ -241,31 +237,6 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{ {
@ -278,16 +249,14 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
switch (c->CMD_UP_mode ) switch (c->CMD_UP_mode )
{ {
case Normal : case Normal :
/*投篮*/
u->Oper_control_state .Pitch_flag =Not_started_Pit; u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
u->Oper_control_state .last_state = Not_started_Pit; u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
/*运球*/
u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;
u->motor_target .go_shoot =0;
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
u->motor_target .go_spin =u->DribbleContext.DribbleConfig .go2_init_angle ;
u->motor_target .M3508_angle =0;
u->DribbleContext .DribbleState = STATE_GRAB_BALL; //状态更新,开始夹球
RELAY1_TOGGLE (1);//接球，1开0关 RELAY1_TOGGLE (1);//接球，1开0关
RELAY2_TOGGLE (1);//夹球，0关1开 RELAY2_TOGGLE (1);//夹球，0关1开
@ -295,70 +264,29 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break; break;
case Pitch_pull : case Pitch :
if(u->Oper_control_state .last_state == Not_started_Pit) Pitch_Process(u,out);
{ u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
u->motor_target .go_shoot =-2050;
u->motor_target .M2006_angle =-140;
if(u->motorfeedback .GO_motor_info[0]->Pos < (-35.20)) //到达位置后再扣扳机
u->motor_target .go_shoot =GO_POSITION_TRIGGER;
if(u->motorfeedback .GO_motor_info[0]->Pos < (GO_POSITION_PITCH_FD)) //到达位置后再扣扳机
{
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
if(u->motorfeedback .M2006.total_angle>-5)
{//避免没勾上就拉
u->motor_target .go_shoot =GO1_INIT_POSITION;
u->Oper_control_state .Pitch_flag = Launch_Ready ;
u->Oper_control_state .last_state = Launch_Ready;
}
}
}
break ; break ;
case Pitch_shoot :
if (u->Oper_control_state .last_state == Launch_Ready)
{
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
u->Oper_control_state .Pitch_flag = Done_Pit ;
u->Oper_control_state .last_state = Done_Pit;
}
break;
case Dribble: case Dribble:
{ {
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */ /*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
Dribble_Process(u,out); Dribble_Process(u,out);
u->DribbleContext .DribbleState =STATE_GRAB_BALL;//置标志位用于启动运球
}break ; }break ;
case PICK_Pitch_START :
Pitch_Process(u,out);
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动视觉辅助投篮
break;
} }
break; break;
@ -369,11 +297,60 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
return 0;
} }
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{
switch(u->PitchContext .PitchState){
case PITCH_START:
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
if(u->motorfeedback .GO_motor_info[0]->Pos < -35.0) //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
u->PitchContext .PitchState =PITCH_PULL_TRIGGER; //更改标志位
break ;
case PITCH_PULL_TRIGGER:
if(u->motorfeedback .M2006 .total_angle <5) //当2006的总角度小于5，可以认为已经勾上
{
u->motor_target .go_shoot =u->PitchContext.PitchConfig .go1_init_position;//go下拉
u->PitchContext.PitchState =PITCH_LAUNCHING;//转移标志位
}
break ;
case PITCH_LAUNCHING:
if(u->motorfeedback .GO_motor_info [0]->Pos <2.0f)
{
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;//给-140角度发射
u->PitchContext .PitchState =PITCH_COMPLETE;
}
break ;
case PITCH_COMPLETE://完成运行动作后，恢复2006，-140度，go，pos0位置
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;//给-140角度
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_init_position;
u->PitchContext .Pitch_run_num ++;//运行次数+1
break ;
}
return 0;
}
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{ {
@ -428,7 +405,7 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
case STATE_CATCH_DONE: case STATE_CATCH_DONE:
RELAY2_TOGGLE (1);//松球 RELAY2_TOGGLE (1);//松球
RELAY1_TOGGLE (0); RELAY1_TOGGLE (0);
u->DribbleContext .Dribble_run_num ++;//运行次数增加
break; break;

55
User/Module/up.h
View File

@ -40,6 +40,31 @@
/* 投球状态定义 */
typedef enum {
PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER, // 扳机拉动
PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成
} PitchState_t;
/* 投球参数配置 */
typedef struct {
fp32 m2006_init_angle; // M2006初始角度-140
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
} PitchConfig_t;
/* 投球控制上下文 */
typedef struct {
PitchState_t PitchState;
PitchConfig_t PitchConfig;
uint8_t is_initialized ;
uint8_t Pitch_run_num;
} PitchContext_t;
@ -47,17 +72,10 @@
typedef enum
{
Not_started_Pit=0,//未开始
Launch_Ready=1, //准备发射
Launch_complete=2,//发射完成
Done_Pit=3 //已完成
}Pitch_flag_t;
/*运球*/ /*运球*/
typedef enum { typedef enum {
Dribble_PREPARE,
STATE_GRAB_BALL, // 夹球升起阶段 STATE_GRAB_BALL, // 夹球升起阶段
STATE_RELEASE_BALL, // 释放球体 STATE_RELEASE_BALL, // 释放球体
STATE_CATCH_PREP, // 接球准备 STATE_CATCH_PREP, // 接球准备
@ -81,19 +99,10 @@ typedef struct {
DribbleConfig_t DribbleConfig; DribbleConfig_t DribbleConfig;
uint8_t is_initialized; uint8_t is_initialized;
uint8_t Dribble_run_num;
} DribbleContext_t; } DribbleContext_t;
/*运行控制中的控制*/
typedef struct{
/*投球过程*/
Pitch_flag_t Pitch_flag;
char last_state;
} Oper_control_state_t;
typedef struct { typedef struct {
@ -147,7 +156,7 @@ typedef struct
GO_param_t go_param[2]; GO_param_t go_param[2];
DribbleConfig_t DribbleConfig_Config; DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config;
}UP_Param_t; }UP_Param_t;
typedef struct typedef struct
@ -174,16 +183,15 @@ typedef struct{
/*运球过程*/ /*运球过程*/
DribbleContext_t DribbleContext; DribbleContext_t DribbleContext;
/*投篮过程*/
PitchContext_t PitchContext;
UP_Imu_t pos088; UP_Imu_t pos088;
/*控制及状态*/
CMD_t *cmd; CMD_t *cmd;
Oper_control_state_t Oper_control_state;//上层机构的运行状态
struct{ struct{
fp32 rotor_pit6020ecd; fp32 rotor_pit6020ecd;
@ -270,6 +278,7 @@ 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 ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle); int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed); int8_t UP_M3508_speed(UP_t *u,fp32 speed);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out); int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);

12
User/device/cmd.c
View File

@ -52,9 +52,9 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
else if(rc->sw_l==CMD_SW_UP) else if(rc->sw_l==CMD_SW_UP)
{ {
cmd ->CMD_CtrlType =UP_RCcontrol; cmd ->CMD_CtrlType =UP_RCcontrol;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左上，右上，投篮 if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Pitch_pull; //左上，右中，无模式 if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左上，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_shoot; //左上，右上，投篮 if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左上，右上，运球
} }
else if(rc->sw_l==CMD_SW_MID) else if(rc->sw_l==CMD_SW_MID)
@ -68,16 +68,16 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
} }
if(rc->sw_r ==CMD_SW_DOWN) if(rc->sw_r ==CMD_SW_DOWN)
{ {
cmd ->CMD_UP_mode =Dribble; //左中，右上，运球 cmd ->CMD_UP_mode =Normal; //左中，右下，无模式
cmd ->CMD_CtrlType =UP_RCcontrol; cmd ->CMD_CtrlType =UP_RCcontrol;
} }
} }
else if(rc->sw_l==CMD_SW_DOWN) else if(rc->sw_l==CMD_SW_DOWN)
{ {
cmd ->CMD_CtrlType =PICK_t; cmd ->CMD_CtrlType =PICK_t;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮 if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =PICK_Pitch_Normal; //左下，右上，投篮
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式 if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮 if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =PICK_Pitch_START; //左下，右上，投篮
} }

7
User/device/cmd.h
View File

@ -22,8 +22,11 @@ typedef enum{
Normal, //无模式 Normal, //无模式
Dribble , //运球 Dribble , //运球
Pitch_pull , //投篮 Pitch, //投篮
Pitch_shoot , //投篮
/*视觉辅助下的投篮*/
PICK_Pitch_Normal,
PICK_Pitch_START
}CMD_UP_mode_t; }CMD_UP_mode_t;
typedef struct { typedef struct {