第四章小车寻路的那些骚操作

Posted 2020-10-24 stringsir

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了第四章小车寻路的那些骚操作相关的知识，希望对你有一定的参考价值。

在这一章，我来带领大家学习机器人巡线。首先请大家按照要求拼装好小车。我们巡线采用的是安装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];
}

复杂路口处理

为了让机器能够适应复杂路口减小出错的频率，我采用特殊路口特殊处理的办法。

首先讲解一下我们的寻路模式

#include <LNDZ.h>
bool dx = true;
bool l, m, r, ll, rr;hr,hl;
int data[7] = {210 , 245 , 220 , 256 , 259,300,300};//这个值每次实验都要测量的

void init()
{
  B_start();
}
void repeat()
{
  while(true)//第一个路口
{
lookUpLine();//基本寻线，因为两个马达的速度不一
if(ll){//发现了第一个路口,这里假设是最左边的探头探测到，然后就要进行左转为例
//直走直到车身中间靠左的探头碰到黑线，(hl碰到左边黑线之前,一直要往前直走)
readBackData();while(hl==0){readBackData();motor(40,40);}
//然后继续直走直到车身中间靠左的探头离开黑线，这就是我们测量出转弯的最佳地方(hl没越过黑线，也要一直往前直走)
while(hl==1){readBackData();motor(40,40);}
//进行左转直到左边探头碰到黑线
readFrontData(); while(ll == 0){readFrontData();motor(-40, 40);}
//进行左转直到中间探头碰到黑线这样车子就能转过来了
readFrontData(); while(m  == 0){readFrontData();motor(-40, 40);}
break;//小车成功转弯到正轨道上跳出循环进入下一个路口循环
}
}

  while(true)//第二个路口
{
lookUpLine();//基本寻线，因为两个马达的速度不一
if(rr){
readBackData();while(rl==0){readBackData();motor(40,40);}
while(rl==1){readBackData();motor(40,40);}
readFrontData(); while(rr == 0){readFrontData();motor(40,- 40);}
readFrontData(); while(m  == 0){readFrontData();motor(40, -40);}
break;
}
}
/* ... */
}

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];
[程序中没有data[5],data[6]的声明！]}

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];
}

查看程序我们对每个路口都进行了处理，保证了小车的出错率能够降到最低，上面是我演示的直角左转和右转，在遇到了如菱角、米字、不规则路口我们要根据实践情况进行转，思考怎么才能使出错率降到最低。

下面是我对下面轨道，部分轨道写的程序

技术分享图片

#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);
    }
}

在需要实现蔽障的实验中就要运用超声波。

以上是关于第四章小车寻路的那些骚操作的主要内容，如果未能解决你的问题，请参考以下文章

游戏中的人物是如何寻路的？

Esri推出用于设施寻路的室内定位系统

训练寻路

基于全志D1-H哪吒的自动寻路小车-附源码

AI自动寻路

基于深度优先搜索的寻路算法及其进一步的探究

第四章 小车寻路的那些骚操作

测量光度值

自动寻轨

复杂路口处理

超声波

第四章小车寻路的那些骚操作