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

Compare commits

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

3 Commits
0aee725ece ... 0ef286e5f2

Author SHA1 Message Date
ZHAISHUI04
0ef286e5f2 更新 2025-04-14 21:11:43 +08:00
ZHAISHUI04
37dc1ac1e9 目的，无线串口发送需要数据 2025-04-13 14:55:49 +08:00
ZHAISHUI04
3d6cb3d799 最终了，不大改了，改改参数就这样了 2025-04-07 19:58:25 +08:00
11 changed files with 264 additions and 180 deletions
Show Stats Expand all files Collapse all files

68
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<nTsel>3</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
@ -145,7 +145,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>DLGUARM</Key>
<Name>(105=-1,-1,-1,-1,0)</Name>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -158,82 +158,32 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>raw,0x0A</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>GO_motor_info</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
<ItemText>dr16,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>can_out,0x0A</ItemText>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>bbb,0x0A</ItemText>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<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>
<ItemText>aaa</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>

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

Binary file not shown.

8
User/Algorithm/user_math.c
View File

@ -214,6 +214,14 @@ 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;

1
User/Algorithm/user_math.h
View File

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

44
User/Module/config.c
View File

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

233
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_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
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 );
}
PID_init (&u->pid .GO1_position ,PID_POSITION,&(u->param->go1_position_param ));
// 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 );
// }
// 初始化上层状态机
@ -45,15 +45,13 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE;
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;//记录运行次数
}
@ -199,9 +197,11 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
/*go电机控制*/
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 );
return 0;
fp32 deal_pos;
// deal_pos = PID_calc (&u->pid .GO1_position ,u->motorfeedback .GO_motor_info [0]->Pos ,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 ,pos,u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
return 0;
}
@ -217,7 +217,6 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
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++){
@ -241,52 +240,60 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
if(u ==NULL) return 0;
static int is_pitch=1;
switch (c->CMD_CtrlType )
{
case UP_RCcontrol: //在手动模式下
switch (c->CMD_UP_mode )
switch (c-> CMD_UP_mode )
{
case Normal :
/*投篮*/
if(is_pitch){
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 .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_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->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态
RELAY1_TOGGLE (1);//接球，1开0关
RELAY2_TOGGLE (1);//夹球，0关1开
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//球，0接1收
//
break;
case Pitch :
Pitch_Process(u,out);
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动
{
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
}
Pitch_Process(u,out,c);
break ;
case Dribble:
{
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
Dribble_Process(u,out);
u->DribbleContext .DribbleState =STATE_GRAB_BALL;//置标志位用于启动运球
if(u->DribbleContext.DribbleState== Dribble_PREPARE){
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
}
Dribble_Process(u,out);
}break ;
case PICK_Pitch_START :
case Dribbl_transfer:
Pitch_Process(u,out);
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动视觉辅助投篮
break;
Dribbl_transfer_a(u,out);
}
break;
@ -302,42 +309,57 @@ 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){
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位置到达最上面，这里的检测条件可以更改
if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
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 ;
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 =u->PitchContext.PitchConfig .go1_init_position;//go下拉
u->PitchContext.PitchState =PITCH_LAUNCHING;//转移标志位
u->motor_target .go_shoot = go1_position;//go下拉
u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006
go1_position = go1_position + c->Vx ;
M2006_Screw_position=M2006_Screw_position+ c->Vy;
}
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 ;
@ -352,24 +374,47 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
switch (u->DribbleContext.DribbleState) {
case STATE_GRAB_BALL://开始
RELAY2_TOGGLE (0);//夹球
RELAY1_TOGGLE (0);//夹球
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
if((u->motorfeedback .M3508 .total_angle >400)) {
RELAY1_TOGGLE (0);
RELAY2_TOGGLE (1);
if((u->motorfeedback.GO_motor_info[1]->angle) <-53){
if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
}
@ -378,13 +423,13 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
case STATE_RELEASE_BALL:
RELAY2_TOGGLE (1);//松球
RELAY1_TOGGLE (1);//松球
if((u->motorfeedback.GO_motor_info[1]->angle) <-60){
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_init_angle ; //恢复go2位置
if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
u->motor_target.go_spin =-40; //恢复go2位置
u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
}
@ -392,22 +437,25 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
break;
case STATE_CATCH_PREP:
if((u->motorfeedback.GO_motor_info[1]->angle) > -0){
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; //当go2到初始位置，3508降
if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
RELAY1_TOGGLE (1);//接球
RELAY2_TOGGLE (0);//接球
}
if(u->motorfeedback .M3508 .total_angle <5){
u->DribbleContext .DribbleState = STATE_CATCH_DONE;
if(u->motorfeedback .M3508 .total_angle <51.0f){
RELAY1_TOGGLE (0);//夹球，0夹1开
u->DribbleContext .DribbleState = STATE_TRANSFER;
}
break;
break ;
case STATE_CATCH_DONE:
RELAY2_TOGGLE (1);//松球
RELAY1_TOGGLE (0);
u->DribbleContext .Dribble_run_num ++;//运行次数增加
break;
default:
break;
@ -416,3 +464,42 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
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 {
PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER, // 扳机拉动
PITCH_PULL_TRIGGER,
PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成
} PitchState_t;
@ -55,6 +55,7 @@ typedef struct {
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
fp32 m2006_Screw_init;
} PitchConfig_t;
/* 投球控制上下文 */
@ -63,7 +64,6 @@ typedef struct {
PitchConfig_t PitchConfig;
uint8_t is_initialized ;
uint8_t Pitch_run_num;
} PitchContext_t;
@ -80,6 +80,7 @@ typedef enum {
STATE_RELEASE_BALL, // 释放球体
STATE_CATCH_PREP, // 接球准备
STATE_CATCH_BALL, // 接球动作
STATE_TRANSFER, //转移球
STATE_CATCH_DONE //完成
} DribbleState_t;
@ -90,7 +91,8 @@ typedef struct {
fp32 go2_init_angle; // GO2初始角度
fp32 go2_flip_angle; // GO2翻转角度
fp32 flip_timing; // 翻转触发时机
fp32 release_threshold; // 释放球
// fp32
fp32 go2_release_threshold; // 释放球
} DribbleConfig_t;
/* 状态机上下文 */
@ -99,7 +101,6 @@ typedef struct {
DribbleConfig_t DribbleConfig;
uint8_t is_initialized;
uint8_t Dribble_run_num;
} DribbleContext_t;
@ -154,6 +155,7 @@ typedef struct
pid_param_t M3508_angle_param;
GO_param_t go_param[2];
pid_param_t go1_position_param;
DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config;
@ -177,7 +179,7 @@ typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
int state;
int state;
@ -242,6 +244,8 @@ typedef struct{
pid_type_def M3508_angle;
pid_type_def M3508_speed;
pid_type_def GO1_position;
}pid;
@ -260,7 +264,7 @@ typedef struct{
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
fp32 vofa_send[8];
fp32 vofa_send[8];
@ -278,9 +282,10 @@ 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 UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed);
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);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
#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++)
{
GO_motor_info[id].id = id;
GO_motor_info[id].mode = 1; //位置模式
GO_motor_info[id].mode = 1; //foc闭环
GO_motor_info[id].correct = 1;
GO_motor_info[id].MError = 0;
GO_motor_info[id].Temp = 0;
@ -158,14 +158,16 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5];
motor->W = W;
motor->W = (motor->W * 6.28319f )/(256*6.33f);
int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
motor->Pos = Pos;
motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
int32_t Pos = (Temp_buffer[7] << 0) |
(Temp_buffer[8] << 8) |
(Temp_buffer[9] << 16) |
(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;
motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7;
motor ->angle = AngleChange(DEGREE,motor ->Pos);
}
//力位混合控制

31
User/device/cmd.c
View File

@ -43,18 +43,30 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
/*遥控器，上下层通用，按键控制，统一到cmd*/
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
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)) {
cmd->CMD_CtrlType =RELAXED;
}
else if(rc->sw_l==CMD_SW_UP)
{
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_DOWN) cmd ->CMD_UP_mode =Dribble; //左上，右上，运球
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_HAND; //左上，右上，手动调整
}
else if(rc->sw_l==CMD_SW_MID)
@ -74,14 +86,19 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
}
else if(rc->sw_l==CMD_SW_DOWN)
{
cmd ->CMD_CtrlType =PICK_t;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =PICK_Pitch_Normal; //左下，右上，投篮
cmd ->CMD_CtrlType =UP_RCcontrol;
if(rc->sw_r ==CMD_SW_UP) 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 =PICK_Pitch_START; //左下，右上，投篮
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，无模式
}
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;
}

17
User/device/cmd.h
View File

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

11
User/task/up_task.c
View File

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