在这一章,我来带领大家学习机器人巡线。首先请大家按照要求拼装好小车。我们巡线采用的是安装7个探头来精准巡线。
测量光度值
在学习小车寻路之前,我们必须要学习一下模拟探头。什么是模拟探头?模拟探头的原理是:根据探测地面光的大小转换成不同的值给小车。大家都知道黑色吸光,白色反光,这样模拟探头探测到黑色区域或白色区域所转换的值就不一样了,我们探头可以通过不停的检测前方的光度值,来判断小车目前所在黑线的位置,进而使用程序智能地调整小车的位置。我们可以在小车巡线之前,先要多次测量黑线的值和白线的值,以两者的均值作为中间值,当超过中间值我们就认为是黑线,小于中间值我们就认为是白线。
因为天气和环境的变化原因,我们每天实验都需要测量光值,下面是我写的一套智能测量灰度值的程序:
#include <LNDZ.h> int ll, l, m, r, rr, hr, hl; int ll2, l2, m2, r2, rr2, hr2, hl2; lc lcd; ir ykq; der jst; void init() { //B_start(); ykq.start(); lcd.begin(16, 2); lcd.bg(1); ll2 = l2 = m2 = r2 = rr2 = hr2 = hl2 = 0; } void repeat() { ll = AR(4); //请注意这里传感器与模拟输入口编号的对应 l = AR(2); m = AR(1); r = AR(3); rr = AR(5); hr = AR(6);//中间左边探头接6号模拟端口 hl = AR(7); //中间右边探头接7号模拟端口 lcd.setCursor(0, 1); lcd.print(hl); lcd.setCursor(4, 1); lcd.print(ll); lcd.setCursor(3, 0); lcd.print(l); lcd.setCursor(7, 0); lcd.print(m); lcd.setCursor(11, 0); lcd.print(r); lcd.setCursor(9, 1); lcd.print(rr); lcd.setCursor(13, 1); lcd.print(hr); delay(200); lcd.clear(); if (ykq.decode(&jst)) { switch (jst.value) { case one: { ll2 += ll; l2 += l; m2 += m; r2 += r; rr2 += rr; beep(500); } break; case two: { hr2 += hr; hl2 += hl; beep(500); } break; case three: { lcd.setCursor(0, 1); lcd.print(hl2/2); lcd.setCursor(4, 1); lcd.print(ll2/2); lcd.setCursor(3, 0); lcd.print(l2/2); lcd.setCursor(7, 0); lcd.print(m2/2); lcd.setCursor(11, 0); lcd.print(r2/2); lcd.setCursor(9, 1); lcd.print(rr2/2); lcd.setCursor(13, 1); lcd.print(hr2/2); beep(500); while (1); } break; default: break; } ykq.next(); } }
聪明的同学查看了代码就会明白这个意思:遥控按下1机器人会记录前方5个探头的光值,按下2机器人会记录中间2个探头的光值。我们需要按两下1和2分别测量前5个探头的黑色值白色值和后2个探头的黑色值白色值。(小车鸣叫了500ms证明成功取到值)然后按下3按钮小车会自动计算出中间值打印到屏幕上。
顺序如下:
42135分别是前5个探头光值从左向右排,左下和右下分别是后面两个探头。
自动寻轨
在学习了寻路的原理,我们就来实现一次圆形轨道寻路吧!
在实验之前我们先学习motor(40,40);这个函数,这个函数第一个参数代表左轮速度,第二个则代表的是右轮速度。(范围-100~100)下面正式列出代码:
#include <LNDZ.h> bool dx = true; bool l, m, r, ll, rr; int data[5] = {210 , 245 , 220 , 256 , 259};//这个值每次实验都要测量的 void init() { B_start(); } void repeat() { lookUpLine();//基本寻线 } void lookUpLine() { readFrontData();//反复地读数据 if (m) motor(35, 35);//如果中间探头碰到黑线前进 if (l) motor(0, 30);//如果中间左边探头碰到黑线小幅度左转 if (r) motor(30, 0); if (ll)motor(-30, 30); //如果最左边探头碰到黑线大幅度左转 if (rr)motor(30, -30); } void readFrontData() { //从每个端口读数据,并判断是否大于中间值,大于则是黑线存为1,小于则是白线存为0 ll = AR(4) > data[0]; l = AR(2) > data[1]; m = AR(1) > data[2]; r = AR(3) > data[3]; rr = AR(5) > data[4]; }
复杂路口处理
硬件上
首先,为了提高在复杂路口转弯的精确性,我们放弃了传统的五探头寻线,我们自己研制出了一套“七眼”寻线的方法,并对前五个探头的位置进行了适当调整,而且在小车底部的中间增加了两个探头。我的探头布置如下:(如图用字母表示探头)
用前五个探头中间的R、M、L探头进行基本寻轨,前面的RR、LL个探头和底部中间的HR、HL探头进行复杂路口细节处理。考虑到和传统的五探头寻线相比机器效率会有所降低,开销上会有些上升。所以我们设计了相应的程序算法来提高效率,减少开销。
程序上
我们在程序上做了大量改进和优化。研制了一套适用于这种小车寻轨的智能算法。
在对“七眼”寻轨提升效率和减小开销上,在基本寻轨时用R、M、L三个探头判断,旁边的RR和LL用于对路口的判断。在处理复杂或特殊路口时,我们才会用到HR、HL探头。
整体我们采用分治的思想,将每一个复杂的路口拆分成若干个相对简单小步骤来处理。我通过大量的实验和思考,设计出了能处理绝大部分复杂路口的小步骤的代码和使用框架,并用宏定义了这些小步骤,采用这些用宏定义的小步骤,进行创造性的组合能够精准的通过复杂路口。
我们在处理这些小步骤的,我们利用RR、LL、M、HR、HL探头进行“卡位”。下面给出一个复杂路口的示例:
路口示例图
#include <LNDZ.h> /*黑白线转换的变量(黑线寻轨true,白线寻轨false)*/ bool b_w = true; bool l, m, r, ll, rr, hl, hr; /*4,2,1,3,5,7,6探头(LL,L,M,R,RR,HL,HR)的光值*/ int data[7] = {338 , 328 , 268 , 289 , 245, 400 , 350}; /*宏命名规则 LEFT和RINGHT是转弯的方向,LINGHT_L、LEFT_LINGHT_M和LINGHT_R是用哪个哪个灯进行判断分别对应(ll,m,rr); LEFT_TURN和RINGHT_TURN是分别靠ll和rr探头进行判断和转弯; FORWARD_L、FORWARD_R和FORWARD_M前进直到(hl,hr,hl || hr)到达过了黑线才结束(用hl,hr去做判断),通常用于小车精确探出半个车身,利于转弯; _WHILE_START_(x)和_WEND_; 每新的路口进入新的循环(每个循环对不同的路口进行处理) STOP_LOOK;小车停留一段时间,通常用于多个动作组合中防止小车打滑; */ #define LEFT_LINGHT_L; readFrontData(); while(ll == 0){readFrontData();motor(-40, 40);} #define LEFT_LINGHT_M; readFrontData(); while(m == 0){readFrontData();motor(-40, 40);} #define RINGHT_LINGHT_R; readFrontData(); while(rr == 0){readFrontData();motor(40, -40);} #define RINGHT_LINGHT_M; readFrontData(); while(m == 0) {readFrontData();motor(40, -40);} #define LEFT_TURN; readFrontData(); while(ll == 0){readFrontData();motor(-40, 40);} while (m == 0){readFrontData();motor(-40, 40);} #define RINGHT_TURN; readFrontData(); while(rr == 0){readFrontData();motor(40, -40);} while (m == 0){readFrontData();motor(40, -40);} #define FORWARD_L; readBackData(); while(hl == 0){readBackData();motor(40, 40);} while(hl == 1){readBackData();motor(40, 40);} #define FORWARD_R; readBackData(); while(hr == 0){readBackData();motor(40, 40);} while(hr == 1){readBackData();motor(40, 40);} #define FORWARD_M; readBackData(); while(hr == 0 || hl == 0){readBackData();motor(40, 40);} while(hr == 1 || hl == 1){readBackData();motor(40, 40);} #define _WHILE_START_(x) while (true) { lookUpLine(); #define _WEND_; } #define FORWARD_DELAY(x); motor(40,40);delay(x); #define STOP_LOOK; motor(0,0);beep(300);motor(20,20); #define STOP; motor(0, 0);while (1); void init() { B_start(); } void repeat() { _WHILE_START_(1)//路口循环,里面宏定义了基本寻轨 /*(ll,m)或(ll,l)都判断到了说明抵达了路口B,下面写对路口的处理*/ if ((ll + m) == 2 || (rr + ll) == 2) { FORWARD_L; /*小车探出半身,用hl去判断*/ STOP_LOOK; /*停止200ms,防止打滑*/ RINGHT_TURN; /*用右边和中间探头右转,让小车正向朝D*/ FORWARD_DELAY(100); /*延时向前一小段*/ STOP_LOOK; /*停止200ms,防止打滑*/ RINGHT_TURN; /*用右边和中间探头右转,让小车正向朝C*/ break; /*跳出循环,进入下一个路口的循环*/ } _WEND_; _WHILE_START_(1) /*(ll,m)或(rr,ll)都判断到了说明抵达了路口C,下面写对路口的处理*/ if ((ll + m) == 2 || (ll + l) == 2) { FORWARD_L; /*小车探出半身,用hl去判断*/ STOP_LOOK; /*停止200ms,防止打滑*/ LEFT_TURN; /*用左边和中间探头右转,让小车正向朝D*/ break; } _WEND_; STOP; /*小车停止*/ } /*基本寻轨*/ void lookUpLine() { readFrontData(); if (m) motor(35, 35); if (l) motor(0, 30); if (r) motor(30, 0); } void readBackData() { hr = b_w?(AR(6)>data[6]):!(AR(6)>data[6]); hl = b_w?(AR(7)>data[5]):!(AR(7)>data[5]); } void readFrontData() { ll = b_w?(AR(4)>data[0]):!(AR(4) > data[0]); l = b_w?(AR(2)>data[1]):!(AR(2)>data[1]); m = b_w?(AR(1)>data[2]):!(AR(1)>data[2]); r = b_w?(AR(3)>data[3]):!(AR(3)>data[3]); rr = b_w?(AR(5)>data[4]):!(AR(5)>data[4]); }
查看程序我们对每个路口都进行了处理,保证了小车的出错率能够降到最低,上面是我演示的直角左转和右转,在遇到了如菱角、米字、不规则路口我们要根据实践情况进行转,思考怎么才能使出错率降到最低。
下面是一段循规实例:
代码:
#include <LNDZ.h> /*黑白线转换的变量(黑线寻轨true,白线寻轨false)*/ bool b_w = true; bool l, m, r, ll, rr, hl, hr; /*4,2,1,3,5,7,6探头(LL,L,M,R,RR,HL,HR)的光值*/ int data[7] = {338 , 328 , 268 , 289 , 245, 400 , 350}; /*宏命名规则 LEFT和RINGHT是转弯的方向,LINGHT_L、LEFT_LINGHT_M和LINGHT_R是用哪个哪个灯进行判断分别对应(ll,m,rr); LEFT_TURN和RINGHT_TURN是分别靠ll和rr探头进行判断和转弯; FORWARD_L、FORWARD_R和FORWARD_M前进直到(hl,hr,hl || hr)到达过了黑线才结束(用hl,hr去做判断),通常用于小车精确探出半个车身,利于转弯; _WHILE_START_(x)和_WEND_; 每新的路口进入新的循环(每个循环对不同的路口进行处理) STOP_LOOK;小车停留一段时间,通常用于多个动作组合中防止小车打滑; */ #define LEFT_LINGHT_L; readFrontData(); while(ll == 0){readFrontData();motor(-40, 40);} #define LEFT_LINGHT_M; readFrontData(); while(m == 0){readFrontData();motor(-40, 40);} #define RINGHT_LINGHT_R; readFrontData(); while(rr == 0){readFrontData();motor(40, -40);} #define RINGHT_LINGHT_M; readFrontData(); while(m == 0) {readFrontData();motor(40, -40);} #define LEFT_TURN; readFrontData(); while(ll == 0){readFrontData();motor(-40, 40);} while (m == 0){readFrontData();motor(-40, 40);} #define RINGHT_TURN; readFrontData(); while(rr == 0){readFrontData();motor(40, -40);} while (m == 0){readFrontData();motor(40, -40);} #define FORWARD_L; readBackData(); while(hl == 0){readBackData();motor(40, 40);} while(hl == 1){readBackData();motor(40, 40);} #define FORWARD_R; readBackData(); while(hr == 0){readBackData();motor(40, 40);} while(hr == 1){readBackData();motor(40, 40);} #define FORWARD_M; readBackData(); while(hr == 0 || hl == 0){readBackData();motor(40, 40);} while(hr == 1 || hl == 1){readBackData();motor(40, 40);} #define _WHILE_START_(x) while (true) { lookUpLine(); #define _WEND_; } #define FORWARD_DELAY(x); motor(40,40);delay(x); #define STOP_LOOK; motor(0,0);beep(300);motor(20,20); #define STOP; motor(0, 0);while (1); void init() { B_start(); } void repeat() { while (true) { readFrontData(); if (l || r || m || ll || rr) { FORWARD_M; break; } motor(5, 5); } _WHILE_START_(1) if ((ll + rr) == 2 || (ll + m) == 2) { FORWARD_R; STOP_LOOK; break; } _WEND_; _WHILE_START_(1) if ((rr + r) ==2 || (rr + m) == 2) { FORWARD_R; STOP_LOOK; RINGHT_TURN; break; } _WEND_; _WHILE_START_(1) if ((ll + rr) == 2 || (rr + r) == 2) { FORWARD_R; STOP_LOOK; LEFT_TURN; STOP_LOOK; LEFT_TURN; break; } _WEND_; _WHILE_START_('A') if ((ll + rr) == 2 || (rr + r) == 2) { FORWARD_R; STOP_LOOK; RINGHT_TURN; break; } _WEND_; _WHILE_START_(1) if ((ll + m) == 2 || (ll + l) == 2 || (rr + ll) == 2) { FORWARD_L; STOP_LOOK; RINGHT_TURN; FORWARD_DELAY(100); STOP_LOOK; RINGHT_TURN; break; } _WEND_; _WHILE_START_(1) if ((ll + m) == 2 || (ll + l) == 2) { FORWARD_L; STOP_LOOK; LEFT_TURN; break; } _WEND_; _WHILE_START_(1) if ((ll + m) == 2 || (ll + l) == 2) { FORWARD_L; STOP_LOOK; RINGHT_TURN; STOP_LOOK; readBackData(); while (hr == 0) { readBackData(); motor(-40, -40); } STOP_LOOK; RINGHT_TURN; break; } _WEND_; _WHILE_START_("十字路口") if ((ll + rr) == 2 || (l + r) == 2) { FORWARD_L; STOP_LOOK; motor(-30, 0); break; } _WEND_; _WHILE_START_(1) if ((ll + l) == 2 || (ll + m) == 2) { FORWARD_M; STOP_LOOK; break; } _WEND_; _WHILE_START_(1) motor(15, 40); if ((ll + l) == 2 || (ll + m) == 2) { FORWARD_L; STOP_LOOK; LEFT_TURN; break; } _WEND_; STOP; } void lookUpLine() { readFrontData(); if (m) motor(35, 35); if (l) motor(0, 30); if (r) motor(30, 0); } void readBackData() { hr = AR(6) > data[6]; hl = AR(7) > data[5]; } void readFrontData() { ll = AR(4) > data[0]; l = AR(2) > data[1]; m = AR(1) > data[2]; r = AR(3) > data[3]; rr = AR(5) > data[4]; }
超声波
想必大家在初中的时候就对超声波有所了解,下面我来通过一段代码告诉大家超声波是如何实现的。
#include <LNDZ.h> ult csb;//定义一个超声波设备 int d; void init() { B_star(); } void repeat() { d=csb.dis();//得到前方物体的距离 if (d>0 && d<8)//判断距离是否在0~8之间,如果在则绕过物体 { motor(10,60);//圆周运动 } else { motor(40,40); } }
在需要实现蔽障的实验中就要运用超声波。