双轮差速底盘三灰度循迹功能的实现

1. 功能说明

在小型双轮差速底盘样机前方安装3个 灰度传感器 ，实现机器人沿下图所指定的跑道路线进行运动的效果。

2. 使用样机

本实验使用的样机为R023样机。

3. 功能实现

3.1 电子硬件

在这个示例中，我们采用了以下硬件，请大家参考：

主控板	Basra（兼容Arduino Uno）
扩展板	Bigfish2.1扩展板
传感器	灰度传感器
电池	7.4V锂电池

电路连接说明：

① 左轮直流电机连在D9，D10接口上；

② 右轮直流电机连在D5，D6接口上；

③ 3个灰度传感器从左至右连接在A0，A4，A3端口上。

3.2 编程框架

本实验的编程框架用到了有限状态机。有限状态机（Finite-state machine）简称FSM，表示有限个状态以及在这些状态之间的转移和动作等行为的数学模型。它把复杂的控制逻辑分解成有限个稳定状态，在每个状态上判断事件。由于有限状态机有有限个状态，因此可以在实际中实现。有限状态机可以广泛的应用于机器人多个传感器触发组合状态的判断，大大提高检测效率。

状态表

机器人的传感器触发一般用条件判断来做。

这时机器人程序的一般思路是：

如果 机器人的某几个传感器触发了； 机器人的某几个电机做个什么事； 做多久； 如果 机器人的另外某几个传感器触发了； 机器人的某几个电机做个什么事； 做多久；

所以我们总是要用到大量的 if 语句，比如双轮小车的某个功能：

如果 机器人的1号传感器触发了； 机器人的左侧电机顺时针转； 机器人的右侧电机逆时针转； 持续5秒； 如果 机器人的2号传感器触发了； 机器人的左侧电机逆时针转； 机器人的右侧电机顺时针转； 持续5秒； 否则 都不转

用伪码写出来就是：

if {   Sensor(端口a,触发);//传感器触发时此句为真，否则为假 }

{

    Motor(L,顺);

    Motor(R,逆);

    Delay 5;

}

if {   Sensor(端口b,触发); }

{

    Motor(L,逆);

    Motor(R,顺);

    Delay 5;

}

else

{

    Motor(L,停);

    Motor(R,停);

}

在只有一个传感器的情况下，我们假设这是个开关量传感器。那么我们可以得到一个状态表格：

状态序号	传感器1
1	1
2	0

这个传感器有两个状态。

而当有两个传感器时，则有四个状态。

状态序号	传感器1	传感器2
1	1	1
2	1	0
3	0	1
4	0	0

如果我们用 if 语句写这四个状态，就显得比较长。

状态序号	传感器1	传感器2	伪码
1	1	1	if { Sensor(1,1); Sensor(2,1); …… }
2	1	0	if { Sensor(1,1); Sensor(2,0); …… }
3	0	1	if { Sensor(1,0); Sensor(2,1); …… }
4	0	0	else ……

在编程的时候，状态罗列的越全，机器人的bug就越少。但是随着传感器的增多，状态数量按2的N次幂增加，大量的if语句使执行效率变得很低，经常出现识别不灵的情况。我们需要换一种高效写法。

多个确定数量的传感器的触发组合，符合有限状态机的概念，有限状态机一般是用Switch语句来实现。如：

switch(s)

{

case 1 : {动作1;}break;

case 2 : {动作2;}break;

case 3 : {动作3;}break;

case 4 : Act_Stop();break;

default:;break;

}

不难发现，这段语句实现的关键，就是识别出上页表中的1、2、3、4，四个状态序号。

那么问题就来了：我们如何让机器人知道自己传感器的触发组合对应于1、2、3、4的哪个序号呢？

二进制状态表

下面，我们把每组传感器返回值看成一个二进制数值。

结果我们发现了一种新的、可计算的编码方式：

新序号	传感器1	传感器2
0	0	0
1	0	1
2	1	0
3	1	1

于是，只要我们知道了传感器们的触发状态，也就知道了序号；知道了序号，也就知道了传感器们的触发状态。用这个序号去写switch语句，再合适不过了。下面我们要做的是，用一种算法，让机器人能够返回自己接收到的传感器组合值的二进制数据。

算法精解

我们可以使用以下算法来实现：

首先设置一个变量s，这个s，将存储传感器组的二进制状态序号。

我们还需要用到一个重要的运算符“<<”，这个运算符的意义是：左移

如：1<

如：1<<1，结果就是10；1<<2，结果就是100；101<<1，结果就是1010

只要让机器人依次返回各个传感器的状态数值，最早获取的，移到最左；第二获得的，移到“倒数第二左”，……，以此类推。即可获得。

如两个传感器均触发：

先获得1号的数值（真）并左移0位，得

0

1

再获得2号的数值（真）并左移1位，得

1

0

两数值取“或”，即可得11

数学问题解决了，很容易就可以转化为程序语句：

s=0;

for(i=0;i<2;i++) //因为此例中有2个传感器，i取2

{

s=s|(Servo(i+1，触发判断)<

于是switch语句可以写为：

switch(s)

{

case 0x00 : {动作0;}break; //序号也可以写作16进制数值

case 0x01 : {动作1;}break;

case 0x02 : {动作2;}break;

case 0x03 : {动作3;}break;

default:;break;

}

策略表
下面我们以本实验中的“小型双轮差速底盘-3灰度循迹”程序为例，再来推导一遍。

传感器触发情况、小车行驶状态、对应行为策略表如下：

传感器1	传感器2	传感器3	序号	小车状态	动作
0	0	0	0	都没触发，可能是跑偏了	后退，转向
0	0	1	1	小车左偏	左轮逆时针转，向右调整
0	1	0	2	小车正中	左轮逆时针转，右轮顺时针转，前进
0	1	1	3	在这个行进方向上不可能	无
1	0	0	4	小车右偏	右轮顺时针转，向左调整
1	0	1	5	在此跑道上不可能	无
1	1	0	6	遇到转角	右轮顺时针转，左转
1	1	1	7	在此跑道上不可能	无

伪码如下：

s=0;

for(i=0;i<3;i++)

{

s=s|(Input(i+1,1)<

这段代码中的动作，完全由策略表分析获得，因此，当状态比较多时，用户要学会利用策略表进行分析，从而确定机器人的动作策略，而不是凭空想象。

3.3 编写程序

编程环境：Arduino 1.8.19

编写并烧录以下程序（Track_Car.ino），该程序将实现演示视频中的动作【详细例程源代码下载请见 https://www.robotway.com/h-col-113.html】

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-09 https://www.robotway.com/                                   

------------------------------------------------------------------------------------*/

int pin[3] = {A0, A3, A4};           //按车头前进方向，从右至左定义，后面经过公式计算，会转化为从左至右的顺序

int s;

void setup()

{

  pinMode( 5 , OUTPUT);

  pinMode( 6 , OUTPUT);

  pinMode( 9 , OUTPUT);

  pinMode( 10 , OUTPUT);

}



void loop()

{

   s = 0;

        for(int i=0; i<3; i++)                 //循环获取三个传感器的值

        {

            s|= (!digitalRead(pin[i]) << i);   //经过左移运算和或运算后，按照A0、A3、A4的顺序产生一个三位2进制数值，表示3个传感器的组合触发状态

        }

        switch (s)

        {

          case 0x00: //三个均未触发

          back();

          Left();

          break;

          case 0x01: //右侧传感器触发，直线上摆动或遇到右转弯

          Right();

          break;

          case 0x02: //中间传感器触发，直线上直行

          Forwards();

          break;

          case 0x04: //左侧传感器触发，直线上摆动或遇到左转弯

          Left();

          break;

          case 0x06: //左侧两个触发，遇到左转弯

          Left();

          break;

          default:;break;

        }

  }

void Left()

{

digitalWrite( 5 , LOW );

digitalWrite( 6 , HIGH);

digitalWrite( 9 , HIGH );

digitalWrite( 10 , LOW );

}



void Right()

{

digitalWrite( 5 , HIGH );

digitalWrite( 6 , LOW );

digitalWrite( 9 , LOW );

digitalWrite( 10 , HIGH );

}



void Forwards()

{

digitalWrite( 5 , HIGH );

digitalWrite( 6 , LOW );

digitalWrite( 9 , HIGH );

digitalWrite( 10 , LOW );

}



void back()

{

digitalWrite( 5 , LOW );

digitalWrite( 6 , HIGH );

digitalWrite( 9 , LOW );

digitalWrite( 10 , HIGH );

}

审核编辑黄宇