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

Compare commits

base: LYL:0ef286e5f2e8d8019d3d90cfe16f02a05f43dc22
Branches Tags
LYL:main
LYL:new-branch
LYL:up_try
LYL:up_branch
LYL:JIAXING_UP
LYL:xiaomi
..
compare: LYL:0aee725ecec9acbe09de4c51149341f52bc44725
Branches Tags
LYL:new-branch
LYL:up_try
LYL:up_branch
LYL:JIAXING_UP
LYL:main
LYL:xiaomi

No commits in common. "0ef286e5f2e8d8019d3d90cfe16f02a05f43dc22" and "0aee725ecec9acbe09de4c51149341f52bc44725" have entirely different histories.
0ef286e5f2 ... 0aee725ece

11 changed files with 180 additions and 264 deletions
Show Stats Expand all files Collapse all files

68
MDK-ARM/AUTO_CHASSIS.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>3</nTsel> <nTsel>6</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>BIN\CMSIS_AGDI.dll</pMon> <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
@ -145,7 +145,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name></Name> <Name>(105=-1,-1,-1,-1,0)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -158,32 +158,82 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText> <ItemText>raw,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>GO_motor_info</ItemText> <ItemText>UP,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText> <ItemText>can,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText> <ItemText>can_out,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText> <ItemText>bbb,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>aaa</ItemText> <ItemText>aaa,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>CCC</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>GO_motor_info</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cbuf[36]</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>angle</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>can_ZZ</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>flaggg,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<WatchWindow2> <WatchWindow2>

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

Binary file not shown.

8
User/Algorithm/user_math.c
View File

@ -214,14 +214,6 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
} }
} }
void limit(double *a, double b, double c) {
if (*a < b) {
*a = b; // 如果 a 小于下限 b，将 a 设置为 b
} else if (*a > c) {
*a = c; // 如果 a 大于上限 c，将 a 设置为 c
}
// 如果 a 在范围 [b, c] 内，不做任何修改
}
/* 移动向量 */ /* 移动向量 */
MoveVector_t *mv; MoveVector_t *mv;

1
User/Algorithm/user_math.h
View File

@ -159,6 +159,5 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);
uint8_t average(uint8_t arr[], uint8_t n); uint8_t average(uint8_t arr[], uint8_t n);
/*判断是否在误差内*/ /*判断是否在误差内*/
bool is_arrived(float target, float current, float mistake) ; bool is_arrived(float target, float current, float mistake) ;
void limit(double *a, double b, double c) ;
#endif #endif

44
User/Module/config.c
View File

@ -38,14 +38,14 @@ static const ConfigParam_t param_chassis ={
}, },
.M3508_angle_param = { .M3508_angle_param = {
.p = 10.0f, .p = 10.0f,
.i = 0.5f, .i = 0.0f,
.d = 0.0f, .d = 1.5f,
.i_limit = 1000.0f, .i_limit = 1000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.M3508_speed_param={ .M3508_speed_param={
.p = 6.0f, .p = 5.1f,
.i = 0.0f, .i = 0.02f,
.d = 3.2f, .d = 3.2f,
.i_limit = 200.0f, .i_limit = 200.0f,
.out_limit =6000.0f, .out_limit =6000.0f,
@ -68,44 +68,34 @@ static const ConfigParam_t param_chassis ={
.go_param[0]={ .go_param[0]={
.rev = 0, .rev = 0,
.T=0.79, .T=0.1,
.W=0.1, .W=0.1,
.K_P=0.2, .K_P=0.1,
.K_W=0.1, .K_W=0.1,
}, },
.go_param[1]={ .go_param[1]={
.rev = 0, .rev = 0,
.T=0, .T=0.1,
.W=0, .W=0.1,
.K_P=1.0f, .K_P=0.10f,
.K_W=0.1, .K_W=0.1,
},
.go1_position_param={
.p =0.1f,
.i =0.1f,
.d =0.0f,
.i_limit = 2.0f,
.out_limit = 10.0f
}, },
/*上层其他参数*/ /*上层其他参数*/
/*运球*/ /*运球*/
.DribbleConfig_Config = { .DribbleConfig_Config = {
.m3508_init_angle = 50, .m3508_init_angle = 0,
.m3508_high_angle = 1200, .m3508_high_angle = 900,
.go2_init_angle = 0, .go2_init_angle = 35,
.go2_flip_angle = -250, .go2_flip_angle = -130,
.flip_timing = 200, .flip_timing = 200,
.go2_release_threshold = -550.0f, .release_threshold = -1.2f,
}, },
/*投球*/ /*投球*/
.PitchConfig_Config = { .PitchConfig_Config = {
.m2006_init_angle =-150, .m2006_init_angle =-140,
.m2006_trigger_angle =0, .m2006_trigger_angle =0,
.go1_init_position = -500, .go1_init_position = 0,
.go1_release_threshold =-1900, .go1_release_threshold =-2050,
.m2006_Screw_init=0
}, },

235
User/Module/up.c
View File

@ -34,10 +34,10 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param )); PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param )); PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
PID_init (&u->pid .GO1_position ,PID_POSITION,&(u->param->go1_position_param ));
// for(int i=0;i<2;i++){ //go初始位置设置为0 for(int i=0;i<2;i++){ //go初始位置设置为0
// GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W ); GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );
// } }
// 初始化上层状态机 // 初始化上层状态机
@ -45,13 +45,15 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE; 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) { if (!u->PitchContext .is_initialized) {
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值 u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
u->PitchContext .is_initialized = 1; u->PitchContext .is_initialized = 1;
u->PitchContext .Pitch_run_num =0;//记录运行次数
} }
@ -197,11 +199,9 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
/*go电机控制*/ /*go电机控制*/
int8_t GO_SendData(UP_t *u,int id,float pos) int8_t GO_SendData(UP_t *u,int id,float pos)
{ {
fp32 deal_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 );
// deal_pos = PID_calc (&u->pid .GO1_position ,u->motorfeedback .GO_motor_info [0]->Pos ,pos); return 0;
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 ,pos,u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
return 0;
} }
@ -217,6 +217,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
GO_SendData(u,0 ,u->motor_target .go_shoot ); GO_SendData(u,0 ,u->motor_target .go_shoot );
GO_SendData(u,1 ,u->motor_target .go_spin);
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
@ -240,60 +241,52 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{ {
if(u ==NULL) return 0; if(u ==NULL) return 0;
static int is_pitch=1;
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
case UP_RCcontrol: //在手动模式下 case UP_RCcontrol: //在手动模式下
switch (c-> CMD_UP_mode ) switch (c->CMD_UP_mode )
{ {
case Normal : case Normal :
/*投篮*/ /*投篮*/
if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0;
}
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 /*运球*/
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_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;
// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态
// RELAY1_TOGGLE (1);//夹球，0夹1开 RELAY1_TOGGLE (1);//接球，1开0关
// RELAY2_TOGGLE (0);//球，0接1收 RELAY2_TOGGLE (1);//夹球，0关1开
//
break; break;
case Pitch : case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动 Pitch_Process(u,out);
{ u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
}
Pitch_Process(u,out,c);
break ; break ;
case Dribble: case Dribble:
{ {
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */ /*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
Dribble_Process(u,out);
if(u->DribbleContext.DribbleState== Dribble_PREPARE){ u->DribbleContext .DribbleState =STATE_GRAB_BALL;//置标志位用于启动运球
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
}
Dribble_Process(u,out);
}break ;
case Dribbl_transfer:
Dribbl_transfer_a(u,out); }break ;
case PICK_Pitch_START :
Pitch_Process(u,out);
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动视觉辅助投篮
break;
} }
break; break;
@ -309,57 +302,42 @@ return 0;
} }
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{ {
static fp32 go1_position,M2006_Screw_position ;
static int is_initialize=1;
if(is_initialize)
{
go1_position=u->PitchContext .PitchConfig .go1_init_position ;
M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
is_initialize=0;
}
switch(u->PitchContext .PitchState){ switch(u->PitchContext .PitchState){
case PITCH_START: case PITCH_START:
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_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改 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->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
u->PitchContext .PitchState =PITCH_PULL_TRIGGER; //更改标志位
u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
}//更改标志位
break ; break ;
case PITCH_PULL_TRIGGER: case PITCH_PULL_TRIGGER:
if(u->motorfeedback .M2006 .total_angle <5) //当2006的总角度小于5，可以认为已经勾上
if(u->motorfeedback .M2006 .total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{ {
u->motor_target .go_shoot = go1_position;//go下拉 u->motor_target .go_shoot =u->PitchContext.PitchConfig .go1_init_position;//go下拉
u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006 u->PitchContext.PitchState =PITCH_LAUNCHING;//转移标志位
go1_position = go1_position + c->Vx ;
M2006_Screw_position=M2006_Screw_position+ c->Vy;
} }
break ; 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 ;
@ -374,47 +352,24 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{ {
switch (u->DribbleContext.DribbleState) { switch (u->DribbleContext.DribbleState) {
case STATE_GRAB_BALL://开始 case STATE_GRAB_BALL://开始
RELAY1_TOGGLE (0);//夹球 RELAY2_TOGGLE (0);//夹球
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起 u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle; u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
if((u->motorfeedback .M3508 .total_angle >400)) { if((u->motorfeedback .M3508 .total_angle >400)) {
RELAY2_TOGGLE (1); RELAY1_TOGGLE (0);
if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){ if((u->motorfeedback.GO_motor_info[1]->angle) <-53){
u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变 u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
} }
@ -423,13 +378,13 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
case STATE_RELEASE_BALL: case STATE_RELEASE_BALL:
RELAY1_TOGGLE (1);//松球 RELAY2_TOGGLE (1);//松球
if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){ if((u->motorfeedback.GO_motor_info[1]->angle) <-60){
u->motor_target.go_spin =-40; //恢复go2位置 u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_init_angle ; //恢复go2位置
u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变 u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
} }
@ -437,25 +392,22 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
break; break;
case STATE_CATCH_PREP: case STATE_CATCH_PREP:
if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){ if((u->motorfeedback.GO_motor_info[1]->angle) > -0){
u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降 u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; //当go2到初始位置，3508降
RELAY2_TOGGLE (0);//接球 RELAY1_TOGGLE (1);//接球
} }
if(u->motorfeedback .M3508 .total_angle <51.0f){ if(u->motorfeedback .M3508 .total_angle <5){
RELAY1_TOGGLE (0);//夹球，0夹1开 u->DribbleContext .DribbleState = STATE_CATCH_DONE;
u->DribbleContext .DribbleState = STATE_TRANSFER;
} }
break; break;
break ;
case STATE_CATCH_DONE:
RELAY2_TOGGLE (1);//松球
RELAY1_TOGGLE (0);
u->DribbleContext .Dribble_run_num ++;//运行次数增加
break;
default: default:
break; break;
@ -464,42 +416,3 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
return 0; return 0;
} }
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
{
switch (u->DribbleContext.DribbleState) {
case STATE_TRANSFER:
if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
{
u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
}
if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
{
RELAY1_TOGGLE (1);//夹球，0夹1开
if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
{
u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
}
}
break ;
case STATE_CATCH_DONE:
RELAY1_TOGGLE (1);//夹球，0夹1开
RELAY2_TOGGLE (0);//夹球，0接1收
u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
break;
break;
}
}

19
User/Module/up.h
View File

@ -44,7 +44,7 @@
typedef enum { typedef enum {
PITCH_PREPARE, // 准备阶段 PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机 PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER, PITCH_PULL_TRIGGER, // 扳机拉动
PITCH_LAUNCHING, // 发射中 PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成 PITCH_COMPLETE // 完成
} PitchState_t; } PitchState_t;
@ -55,7 +55,6 @@ 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 m2006_Screw_init;
} PitchConfig_t; } PitchConfig_t;
/* 投球控制上下文 */ /* 投球控制上下文 */
@ -64,6 +63,7 @@ typedef struct {
PitchConfig_t PitchConfig; PitchConfig_t PitchConfig;
uint8_t is_initialized ; uint8_t is_initialized ;
uint8_t Pitch_run_num;
} PitchContext_t; } PitchContext_t;
@ -80,7 +80,6 @@ typedef enum {
STATE_RELEASE_BALL, // 释放球体 STATE_RELEASE_BALL, // 释放球体
STATE_CATCH_PREP, // 接球准备 STATE_CATCH_PREP, // 接球准备
STATE_CATCH_BALL, // 接球动作 STATE_CATCH_BALL, // 接球动作
STATE_TRANSFER, //转移球
STATE_CATCH_DONE //完成 STATE_CATCH_DONE //完成
} DribbleState_t; } DribbleState_t;
@ -91,8 +90,7 @@ typedef struct {
fp32 go2_init_angle; // GO2初始角度 fp32 go2_init_angle; // GO2初始角度
fp32 go2_flip_angle; // GO2翻转角度 fp32 go2_flip_angle; // GO2翻转角度
fp32 flip_timing; // 翻转触发时机 fp32 flip_timing; // 翻转触发时机
// fp32 fp32 release_threshold; // 释放球
fp32 go2_release_threshold; // 释放球
} DribbleConfig_t; } DribbleConfig_t;
/* 状态机上下文 */ /* 状态机上下文 */
@ -101,6 +99,7 @@ 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;
@ -155,7 +154,6 @@ typedef struct
pid_param_t M3508_angle_param; pid_param_t M3508_angle_param;
GO_param_t go_param[2]; GO_param_t go_param[2];
pid_param_t go1_position_param;
DribbleConfig_t DribbleConfig_Config; DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config; PitchConfig_t PitchConfig_Config;
@ -179,7 +177,7 @@ typedef struct{
uint8_t up_task_run; uint8_t up_task_run;
const UP_Param_t *param; const UP_Param_t *param;
int state; int state;
@ -244,8 +242,6 @@ typedef struct{
pid_type_def M3508_angle; pid_type_def M3508_angle;
pid_type_def M3508_speed; pid_type_def M3508_speed;
pid_type_def GO1_position;
}pid; }pid;
@ -264,7 +260,7 @@ typedef struct{
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
fp32 vofa_send[8]; fp32 vofa_send[8];
@ -282,10 +278,9 @@ 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,CMD_t *c); 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);
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
#endif #endif

12
User/device/GO_M8010_6_Driver.c
View File

@ -18,7 +18,7 @@ void GO_M8010_init (void){
for (uint8_t id= 0; id <GO_NUM ;id++) for (uint8_t id= 0; id <GO_NUM ;id++)
{ {
GO_motor_info[id].id = id; GO_motor_info[id].id = id;
GO_motor_info[id].mode = 1; //foc闭环 GO_motor_info[id].mode = 1; //位置模式
GO_motor_info[id].correct = 1; GO_motor_info[id].correct = 1;
GO_motor_info[id].MError = 0; GO_motor_info[id].MError = 0;
GO_motor_info[id].Temp = 0; GO_motor_info[id].Temp = 0;
@ -158,16 +158,14 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5]; int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5];
motor->W = W; motor->W = W;
motor->W = (motor->W * 6.28319f )/(256*6.33f); motor->W = (motor->W * 6.28319f )/(256*6.33f);
int32_t Pos = (Temp_buffer[7] << 0) | int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
(Temp_buffer[8] << 8) | motor->Pos = Pos;
(Temp_buffer[9] << 16) | motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
(Temp_buffer[10] << 24);
if(Pos > 0x7FFFFFFF) Pos -= 0xFFFFFFFF; // 溢出修正
motor->Pos = ((float)Pos / 32768.0f) * (6.28319f / 6.33f);
int8_t Temp = Temp_buffer[11] & 0xFF; int8_t Temp = Temp_buffer[11] & 0xFF;
motor->Temp = Temp; motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7; motor->MError = Temp_buffer[12] & 0x7;
motor ->angle = AngleChange(DEGREE,motor ->Pos);
} }
//力位混合控制 //力位混合控制

31
User/device/cmd.c
View File

@ -43,30 +43,18 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
/*遥控器，上下层通用，按键控制，统一到cmd*/ /*遥控器，上下层通用，按键控制，统一到cmd*/
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
if(cmd == NULL) return -1; if(cmd == NULL) return -1;
const CMD_CtrlType_t prev_CtrlType = cmd->CMD_CtrlType ;
const CMD_UP_mode_t prev_mode = cmd->CMD_UP_mode ;//保存旧状态
cmd->Vx = rc->ch_r_x;
cmd->Vy = rc->ch_r_y;
cmd->Vw = rc->ch_l_x;
cmd->poscamear = rc->ch_l_y;
cmd->key_ctrl_l = rc->sw_l;
cmd->key_ctrl_r = rc->sw_r ;
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
cmd->CMD_CtrlType =RELAXED; cmd->CMD_CtrlType =RELAXED;
} }
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 =Pitch; //左上，右上，投篮，设置好的 if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮
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 =Pitch_HAND; //左上，右上，手动调整 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)
@ -86,19 +74,14 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
} }
else if(rc->sw_l==CMD_SW_DOWN) else if(rc->sw_l==CMD_SW_DOWN)
{ {
cmd ->CMD_CtrlType =UP_RCcontrol; 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; //左下，右上，投篮
} }
if (prev_CtrlType!= cmd->CMD_CtrlType ) cmd ->last_CMD_CtrlType = prev_CtrlType;
if (prev_mode!=cmd->CMD_UP_mode ) cmd ->last_CMD_UP_mode =prev_mode;
return 0; return 0;
} }

17
User/device/cmd.h
View File

@ -25,9 +25,9 @@ typedef enum{
Pitch, //投篮 Pitch, //投篮
/*视觉辅助下的投篮*/ /*视觉辅助下的投篮*/
Pitch_HAND, PICK_Pitch_Normal,
PICK_Pitch_START
Dribbl_transfer
}CMD_UP_mode_t; }CMD_UP_mode_t;
typedef struct { typedef struct {
uint8_t status_fromnuc; uint8_t status_fromnuc;
@ -81,11 +81,7 @@ typedef struct {
fp32 key_ctrl_l; fp32 key_ctrl_l;
fp32 key_ctrl_r; fp32 key_ctrl_r;
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 poscamear;
/*视觉*/ /*视觉*/
struct { struct {
uint8_t cmd_status; uint8_t cmd_status;
@ -103,8 +99,7 @@ typedef struct {
CMD_UP_mode_t CMD_UP_mode; CMD_UP_mode_t CMD_UP_mode;
CMD_CtrlType_t CMD_CtrlType; CMD_CtrlType_t CMD_CtrlType;
CMD_CtrlType_t last_CMD_CtrlType;
CMD_UP_mode_t last_CMD_UP_mode;
} CMD_t; } CMD_t;
@ -114,7 +109,5 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ; int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ;
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ;
#endif #endif

9
User/task/up_task.c
View File

@ -67,16 +67,19 @@ void Task_up(void *argument)
// //
// GO_SendData(&UP, 1,CCC);
// GO_SendData(&UP, 0,aaa);
// UP_control(&UP,&UP_CAN_out,&up_cmd); UP_control(&UP,&UP_CAN_out,&up_cmd);
UP.motor_target .go_shoot =aaa; // UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb ; // UP.motor_target .M2006_angle =bbb ;
// UP_control(&UP,&UP_CAN_out,&up_cmd);
// UP.motor_target .M3508_angle =CCC; // UP.motor_target .M3508_angle =CCC;
// UP.motor_target .go_shoot =aaa; // UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb; // UP.motor_target .M2006_angle =bbb;
// UP.motor_target .go_spin =ddd; // UP.motor_target .go_spin =ddd;
//
ALL_Motor_Control(&UP,&UP_CAN_out); ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1); osDelay(1);