机器人武术擂台---无差别组(五)推棋子算法
做一份笔记,大佬勿喷。
作者:sumjess
注意:本博客是以《2018年华北五省(市、自治区)大学生机器人大赛竞赛规则》为基础而写的
@@@@@@@@@一共写2个版本的推棋子算法-----STM32、K60@@@@@@@@
特别提醒:K60是基于山外库书写!
一、总述:
推棋子有一个笔者想尝试的办法,就是使用陀螺仪,这样可以让车在场地内做到定位功能,可以让车走遍及整场。但是由于时间问题,暂时还是使用最普遍的16个光电开关的方案。
无论是推棋子还是互推,前提都是不掉下擂台,所以在所有的光电开关中,起到防掉落功能的光电开关的优先级应该为最高,所以,可能会有人想用中断来防掉落,小编认为此举是没有必要的,因为在主函数中,伪代码就是:光电开关检测-->电机执行,所以小编认为用不用中断都一样,其他的光电开关就是检测棋子的作用啦。
所以整体算法部分代码就是:
(if) 如果检测到有掉落危险,那么执行指令脱离危险。
(else if) 否则检测哪个方向有棋子,哪侧有棋子向哪个方向开去。
(else) 否则慢速直行
所以不难发现,算法都是嵌套,用的都是if,当然在这里也可以用状态机。所以小编STM32的程序用的是if嵌套,K60用的是状态机。
二、算法部分:
@@@@@@@@@STM32的推棋子算法@@@@@@@@
#define delays 380
void algor_SUM_UP(void)
{
if(up_for_lift==0&&up_for_right==0&&up_back_left==0&&up_back_right==0)//没有掉落危险
{
/后侧检测
if(back_1==0||back_2==0)//后侧
{
delay_us(200);//去抖
if(back_1==0||back_2==0)//确定后侧
{
drive_Back_H();//高速倒车
}
}
else
{
;
}
/前面检测
if(for_1==0||for_2==0||for_3==0)//前面
{
delay_us(200);
if(for_1==0||for_2==0||for_3==0)//确定前面
{
drive_For_H();//高速直行
}
}
else
{
;
}
/后侧检测--如果没有,那么直行
if(back_1==0||back_2==0)//后侧
{
delay_us(200);
if(back_1==0||back_2==0)//确定后侧
{
drive_Back_H();//高速后退
}
}
else
{
drive_For_L();//低速直行
}
/前面检测--如果确实有,那么高速直行;如果没有,看最下面的else2;如果一瞬间有,看下看面的else1;
if(for_1==0||for_2==0||for_3==0)//前面
{
delay_us(200);
if(for_1==0||for_2==0||for_3==0)//确定前面
{
drive_For_H();//高速直行
}
else//else1,检测左边,如果有左转;如果一瞬间有,看下面的else3;
{
if(left_1==0||left_2==0||for_lift==0||back_lift==0)//左侧
{
delay_us(200);
if(left_1==0||left_2==0||for_lift==0||back_lift==0)//确定左侧
{
drive_Left_S();//超级左转
delay_ms(delays);
}
}
else//else3,如果有右转;如果一瞬间有低速直行;
{
if(right_1==0||right_2==0||for_right==0||back_right==0)//右侧
{
delay_us(200);
if(right_1==0||right_2==0||for_right==0||back_right==0)//确定右侧
{
drive_Right_S();//超级右转
delay_ms(delays);
}
else
{
drive_For_L();//低速直行
}
}
}
}
}
else//检测左侧,如果有左转;如果一瞬间有,检测右侧;如果右侧确实有,右转;如果右侧一瞬间有,那么低速直行;
{
if(left_1==0||left_2==0||for_lift==0||back_lift==0)//左侧
{
delay_us(200);
if(left_1==0||left_2==0||for_lift==0||back_lift==0)//确定左侧
{
drive_Left_S();//左转
delay_ms(delays);
}
}
else
{
if(right_1==0||right_2==0||for_right==0||back_right==0)//右侧
{
delay_us(200);
if(right_1==0||right_2==0||for_right==0||back_right==0)//确定右侧
{
drive_Right_S();//右转
delay_ms(delays);
}
else
{
drive_For_L();//低速直行
}
}
}
}
}
else
{
if(up_for_lift==1||up_for_right==1)//前方有掉落危险
{
delay_us(200);
if(up_for_lift==1||up_for_right==1)//确定有掉落危险
{
drive_Stop();//停下驱动
drive_Back_H();//高速倒车
delay_ms(700);//300ms
drive_Right();//右转(可以设置向左向右算法)
delay_ms(500);//250ms
}
}
if(up_back_left==1||up_back_right==1)//后方有掉落危险
{
delay_us(200);
if(up_back_left==1||up_back_right==1)//确定有掉落危险
{
drive_Stop();//停下驱动J
drive_For_M();//快速直行
delay_ms(700);//300msJ
drive_Right();//右转(可以设置向左向右算法)J
delay_ms(500);//250msJ
}
}
}
}
@@@@@@@@@K60的推棋子算法@@@@@@@@@@
1、扫描函数:
uint8 opt_qua_Scan(void)//排位模式扫描程序
{
int sensorState;
if( left_forward_edge == 1 ) { sensorState = 1; }//左前边缘
else if ( right_forward_edge == 1 ) { sensorState = 2; }//右前边缘
else if ( left_back_edge == 1 ) { sensorState = 3; }//左后边缘
else if ( right_back_edge == 1 ) { sensorState = 4; }//右后边缘
else if ( left_forward == 0 ) { sensorState = 5; }//左后
else if ( forward_middle_left == 0 ) { sensorState = 6; }//前中左
else if ( forward_middle_right == 0 ) { sensorState = 7; }//前中右
else if ( right_forward == 0 ) { sensorState = 8; }//右前
else if ( left_1 == 0 ) { sensorState = 9; }//左一
else if ( right_1 == 0 ) { sensorState = 10; }//右一
else if ( left_2 == 0 ) { sensorState = 11; }//左二
else if ( right_2 == 0 ) { sensorState = 12; }//右二
else if ( left_back == 0 ) { sensorState = 13; }//左后
else if ( back_middle_left == 0 ) { sensorState = 14; }//后中左
else if ( back_middle_right == 0 ) { sensorState = 15; }//后中右
else if ( right_back == 0 ) { sensorState = 16; }//右后
else { sensorState = 17; }//巡逻模式
return sensorState;
}
2、执行函数:
t=opt_qua_Scan(); //得到键值
switch(t)
{
case 1:
{
//右后
sumjess_back_right_fastest();
break;
}
case 2:
{
//左后
sumjess_back_left_fastest();
break;
}
case 3:
{
//右前
sumjess_forward_right_fastest();
break;
}
case 4:
{
//右前
sumjess_forward_right_fastest();
break;
}
case 5:
{
//左后
sumjess_back_left_faster();
break;
}
case 6:
{
//前左
sumjess_forward_left_fast();
break;
}
case 7:
{
//前右
sumjess_forward_right_faster();
break;
}
case 8:
{
//前右
sumjess_forward_right_fastest();
break;
}
case 9:
{
//前左
sumjess_back_left_fastest();
break;
}
case 10:
{
//右后
sumjess_back_right_fastest();
break;
}
case 11:
{
//前左
sumjess_back_left_fastest();
break;
}
case 12:
{
//前右
sumjess_forward_right_fastest();
break;
}
case 13:
{
//左后
sumjess_back_left_faster();
break;
}
case 14:
{
//左后
sumjess_back_left_faster();
break;
}
case 15:
{
//右后
sumjess_back_right_fastest();
break;
}
case 16:
{
//右后
sumjess_back_right_faster();
break;
}
case 17:
{
//中速前进
sumjess_forward_faster();
break;
}
default:
break;//跳出
}
注:以上所有配置不一定通用,请各位读者按照自己板子的型号和原理图进行调整。