机器人武术擂台---无差别组（五）推棋子算法
机器人武术擂台---无差别组（五）推棋子算法
做一份笔记，大佬勿喷。
作者：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;//跳出
}
注：以上所有配置不一定通用，请各位读者按照自己板子的型号和原理图进行调整。