1. 功能說明

在小型雙輪差速底盤樣機前方安裝3個 灰度傳感器 ，實現機器人沿下圖所指定的跑道路線進行運動的效果。

2. 使用樣機

本實驗使用的樣機為R023樣機。

3. 功能實現

3.1 電子硬件

在這個示例中，我們采用了以下硬件，請大家參考：

電路連接說明：

① 左輪直流電機連在D9，D10接口上；

② 右輪直流電機連在D5，D6接口上；

③ 3個灰度傳感器從左至右連接在A0，A4，A3端口上。

3.2 編程框架

本實驗的編程框架用到了有限狀態機。有限狀態機（Finite-state machine）簡稱FSM，表示有限個狀態以及在這些狀態之間的轉移和動作等行為的數學模型。它把復雜的控制邏輯分解成有限個穩定狀態，在每個狀態上判斷事件。由于有限狀態機有有限個狀態，因此可以在實際中實現。有限狀態機可以廣泛的應用于機器人多個傳感器觸發組合狀態的判斷，大大提高檢測效率。

狀態表

機器人的傳感器觸發一般用條件判斷來做。

這時機器人程序的一般思路是：

所以我們總是要用到大量的 if 語句，比如雙輪小車的某個功能：

用偽碼寫出來就是：

if {   Sensor(端口a,觸發);//傳感器觸發時此句為真，否則為假 }

{

    Motor(L,順);

    Motor(R,逆);

    Delay 5;

}

if {   Sensor(端口b,觸發); }

{

    Motor(L,逆);

    Motor(R,順);

    Delay 5;

}

else

{

    Motor(L,停);

    Motor(R,停);

}

在只有一個傳感器的情況下，我們假設這是個開關量傳感器。那么我們可以得到一個狀態表格：

這個傳感器有兩個狀態。

而當有兩個傳感器時，則有四個狀態。

如果我們用 if 語句寫這四個狀態，就顯得比較長。

在編程的時候，狀態羅列的越全，機器人的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的哪個序號呢？

二進制狀態表

下面，我們把每組傳感器返回值看成一個二進制數值。

結果我們發現了一種新的、可計算的編碼方式：

于是，只要我們知道了傳感器們的觸發狀態，也就知道了序號；知道了序號，也就知道了傳感器們的觸發狀態。用這個序號去寫switch語句，再合適不過了。下面我們要做的是，用一種算法，讓機器人能夠返回自己接收到的傳感器組合值的二進制數據。

算法精解

我們可以使用以下算法來實現：

首先設置一個變量s，這個s，將存儲傳感器組的二進制狀態序號。

我們還需要用到一個重要的運算符“<<”，這個運算符的意義是：左移

如：1<

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

只要讓機器人依次返回各個傳感器的狀態數值，最早獲取的，移到最左；第二獲得的，移到“倒數第二左”，……，以此類推。即可獲得。

如兩個傳感器均觸發：

先獲得1號的數值（真）并左移0位，得

再獲得2號的數值（真）并左移1位，得

兩數值取“或”，即可得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灰度循跡”程序為例，再來推導一遍。

傳感器觸發情況、小車行駛狀態、對應行為策略表如下：

偽碼如下：

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

}

審核編輯黃宇