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

 

在这一章,我来带领大家学习机器人巡线。首先请大家按照要求拼装好小车。我们巡线采用的是安装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个探头的光值。我们需要按两下12分别测量前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];
}

 

复杂路口处理

硬件上

     首先,为了提高在复杂路口转弯的精确性,我们放弃了传统的五探头寻线,我们自己研制出了一套“七眼”寻线的方法,并对前五个探头的位置进行了适当调整,而且在小车底部的中间增加了两个探头。我的探头布置如下:(如图用字母表示探头)

 

用前五个探头中间的RML探头进行基本寻轨,前面的RRLL个探头和底部中间的HRHL探头进行复杂路口细节处理。考虑到和传统的五探头寻线相比机器效率会有所降低,开销上会有些上升。所以我们设计了相应的程序算法来提高效率,减少开销。

程序上

   我们在程序上做了大量改进和优化。研制了一套适用于这种小车寻轨的智能算法。

在对“七眼”寻轨提升效率和减小开销上,在基本寻轨时用RML三个探头判断,旁边的RRLL用于对路口的判断。在处理复杂或特殊路口时,我们才会用到HRHL探头。

整体我们采用分治的思想,将每一个复杂的路口拆分成若干个相对简单小步骤来处理。我通过大量的实验和思考,设计出了能处理绝大部分复杂路口的小步骤的代码和使用框架,并用宏定义了这些小步骤,采用这些用宏定义的小步骤,进行创造性的组合能够精准的通过复杂路口。

我们在处理这些小步骤的,我们利用RRLLMHRHL探头进行“卡位”。下面给出一个复杂路口的示例:

 

路口示例图

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

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

转载于:https://www.cnblogs.com/StringSir/p/8443340.html

  • 2
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值