1. 功能說(shuō)明
在小型雙輪差速底盤(pán)樣機(jī)前方安裝3個(gè) 灰度傳感器 ,實(shí)現(xiàn)機(jī)器人沿下圖所指定的跑道路線(xiàn)進(jìn)行運(yùn)動(dòng)的效果。
2. 使用樣機(jī)
本實(shí)驗(yàn)使用的樣機(jī)為R023樣機(jī)。
3. 功能實(shí)現(xiàn)
3.1 電子硬件
在這個(gè)示例中,我們采用了以下硬件,請(qǐng)大家參考:
主控板 | Basra(兼容Arduino Uno) |
擴(kuò)展板 | Bigfish2.1擴(kuò)展板 |
傳感器 | 灰度傳感器 |
電池 | 7.4V鋰電池 |
電路連接說(shuō)明:
② 右輪直流電機(jī)連在D5,D6接口上;
③ 3個(gè)灰度傳感器從左至右連接在A0,A4,A3端口上。
3.2 編程框架
本實(shí)驗(yàn)的編程框架用到了有限狀態(tài)機(jī)。有限狀態(tài)機(jī)(Finite-state machine)簡(jiǎn)稱(chēng)FSM,表示有限個(gè)狀態(tài)以及在這些狀態(tài)之間的轉(zhuǎn)移和動(dòng)作等行為的數(shù)學(xué)模型。它把復(fù)雜的控制邏輯分解成有限個(gè)穩(wěn)定狀態(tài),在每個(gè)狀態(tài)上判斷事件。由于有限狀態(tài)機(jī)有有限個(gè)狀態(tài),因此可以在實(shí)際中實(shí)現(xiàn)。有限狀態(tài)機(jī)可以廣泛的應(yīng)用于機(jī)器人多個(gè)傳感器觸發(fā)組合狀態(tài)的判斷,大大提高檢測(cè)效率。
狀態(tài)表
機(jī)器人的傳感器觸發(fā)一般用條件判斷來(lái)做。
這時(shí)機(jī)器人程序的一般思路是:
如果 機(jī)器人的某幾個(gè)傳感器觸發(fā)了; 機(jī)器人的某幾個(gè)電機(jī)做個(gè)什么事; 做多久; 如果 機(jī)器人的另外某幾個(gè)傳感器觸發(fā)了; 機(jī)器人的某幾個(gè)電機(jī)做個(gè)什么事; 做多久; |
所以我們總是要用到大量的 if 語(yǔ)句,比如雙輪小車(chē)的某個(gè)功能:
如果 機(jī)器人的1號(hào)傳感器觸發(fā)了; 機(jī)器人的左側(cè)電機(jī)順時(shí)針轉(zhuǎn); 機(jī)器人的右側(cè)電機(jī)逆時(shí)針轉(zhuǎn); 持續(xù)5秒; 如果 機(jī)器人的2號(hào)傳感器觸發(fā)了; 機(jī)器人的左側(cè)電機(jī)逆時(shí)針轉(zhuǎn); 機(jī)器人的右側(cè)電機(jī)順時(shí)針轉(zhuǎn); 持續(xù)5秒; 否則 都不轉(zhuǎn) |
用偽碼寫(xiě)出來(lái)就是:
if { Sensor(端口a,觸發(fā));//傳感器觸發(fā)時(shí)此句為真,否則為假 } { Motor(L,順); Motor(R,逆); Delay 5; } if { Sensor(端口b,觸發(fā)); } { Motor(L,逆); Motor(R,順); Delay 5; } else { Motor(L,停); Motor(R,停); }
在只有一個(gè)傳感器的情況下,我們假設(shè)這是個(gè)開(kāi)關(guān)量傳感器。那么我們可以得到一個(gè)狀態(tài)表格:
狀態(tài)序號(hào) | 傳感器1 |
1 | 1 |
2 | 0 |
這個(gè)傳感器有兩個(gè)狀態(tài)。
而當(dāng)有兩個(gè)傳感器時(shí),則有四個(gè)狀態(tài)。
狀態(tài)序號(hào) | 傳感器1 | 傳感器2 |
1 | 1 | 1 |
2 | 1 | 0 |
3 | 0 | 1 |
4 | 0 | 0 |
如果我們用 if 語(yǔ)句寫(xiě)這四個(gè)狀態(tài),就顯得比較長(zhǎng)。
狀態(tài)序號(hào) | 傳感器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 …… |
在編程的時(shí)候,狀態(tài)羅列的越全,機(jī)器人的bug就越少。但是隨著傳感器的增多,狀態(tài)數(shù)量按2的N次冪增加,大量的if語(yǔ)句使執(zhí)行效率變得很低,經(jīng)常出現(xiàn)識(shí)別不靈的情況。我們需要換一種高效寫(xiě)法。
多個(gè)確定數(shù)量的傳感器的觸發(fā)組合,符合有限狀態(tài)機(jī)的概念,有限狀態(tài)機(jī)一般是用Switch語(yǔ)句來(lái)實(shí)現(xiàn)。如:
switch(s) { case 1 : {動(dòng)作1;}break; case 2 : {動(dòng)作2;}break; case 3 : {動(dòng)作3;}break; case 4 : Act_Stop();break; default:;break; }
不難發(fā)現(xiàn),這段語(yǔ)句實(shí)現(xiàn)的關(guān)鍵,就是識(shí)別出上頁(yè)表中的1、2、3、4,四個(gè)狀態(tài)序號(hào)。
那么問(wèn)題就來(lái)了:我們?nèi)绾巫寵C(jī)器人知道自己傳感器的觸發(fā)組合對(duì)應(yīng)于1、2、3、4的哪個(gè)序號(hào)呢?
二進(jìn)制狀態(tài)表
下面,我們把每組傳感器返回值看成一個(gè)二進(jìn)制數(shù)值。
結(jié)果我們發(fā)現(xiàn)了一種新的、可計(jì)算的編碼方式:
新序號(hào) | 傳感器1 | 傳感器2 |
0 | 0 | 0 |
1 | 0 | 1 |
2 | 1 | 0 |
3 | 1 | 1 |
于是,只要我們知道了傳感器們的觸發(fā)狀態(tài),也就知道了序號(hào);知道了序號(hào),也就知道了傳感器們的觸發(fā)狀態(tài)。用這個(gè)序號(hào)去寫(xiě)switch語(yǔ)句,再合適不過(guò)了。下面我們要做的是,用一種算法,讓機(jī)器人能夠返回自己接收到的傳感器組合值的二進(jìn)制數(shù)據(jù)。
算法精解
我們可以使用以下算法來(lái)實(shí)現(xiàn):
首先設(shè)置一個(gè)變量s,這個(gè)s,將存儲(chǔ)傳感器組的二進(jìn)制狀態(tài)序號(hào)。
我們還需要用到一個(gè)重要的運(yùn)算符“<<”,這個(gè)運(yùn)算符的意義是:左移
如:1<
如:1<<1,結(jié)果就是10;1<<2,結(jié)果就是100;101<<1,結(jié)果就是1010
只要讓機(jī)器人依次返回各個(gè)傳感器的狀態(tài)數(shù)值,最早獲取的,移到最左;第二獲得的,移到“倒數(shù)第二左”,……,以此類(lèi)推。即可獲得。
如兩個(gè)傳感器均觸發(fā):
先獲得1號(hào)的數(shù)值(真)并左移0位,得
0 | 1 |
再獲得2號(hào)的數(shù)值(真)并左移1位,得
1 | 0 |
兩數(shù)值取“或”,即可得11
數(shù)學(xué)問(wèn)題解決了,很容易就可以轉(zhuǎn)化為程序語(yǔ)句:
s=0; for(i=0;i<2;i++) //因?yàn)榇死杏?個(gè)傳感器,i取2 { s=s|(Servo(i+1,觸發(fā)判斷)<
于是switch語(yǔ)句可以寫(xiě)為:
switch(s) { case 0x00 : {動(dòng)作0;}break; //序號(hào)也可以寫(xiě)作16進(jìn)制數(shù)值 case 0x01 : {動(dòng)作1;}break; case 0x02 : {動(dòng)作2;}break; case 0x03 : {動(dòng)作3;}break; default:;break; }
策略表
下面我們以本實(shí)驗(yàn)中的“小型雙輪差速底盤(pán)-3灰度循跡”程序?yàn)槔賮?lái)推導(dǎo)一遍。
傳感器觸發(fā)情況、小車(chē)行駛狀態(tài)、對(duì)應(yīng)行為策略表如下:
傳感器1 | 傳感器2 | 傳感器3 | 序號(hào) | 小車(chē)狀態(tài) | 動(dòng)作 |
0 | 0 | 0 | 0 | 都沒(méi)觸發(fā),可能是跑偏了 | 后退,轉(zhuǎn)向 |
0 | 0 | 1 | 1 | 小車(chē)左偏 | 左輪逆時(shí)針轉(zhuǎn),向右調(diào)整 |
0 | 1 | 0 | 2 | 小車(chē)正中 | 左輪逆時(shí)針轉(zhuǎn),右輪順時(shí)針轉(zhuǎn),前進(jìn) |
0 | 1 | 1 | 3 | 在這個(gè)行進(jìn)方向上不可能 | 無(wú) |
1 | 0 | 0 | 4 | 小車(chē)右偏 | 右輪順時(shí)針轉(zhuǎn),向左調(diào)整 |
1 | 0 | 1 | 5 | 在此跑道上不可能 | 無(wú) |
1 | 1 | 0 | 6 | 遇到轉(zhuǎn)角 | 右輪順時(shí)針轉(zhuǎn),左轉(zhuǎn) |
1 | 1 | 1 | 7 | 在此跑道上不可能 | 無(wú) |
偽碼如下:
s=0; for(i=0;i<3;i++) { s=s|(Input(i+1,1)<
這段代碼中的動(dòng)作,完全由策略表分析獲得,因此,當(dāng)狀態(tài)比較多時(shí),用戶(hù)要學(xué)會(huì)利用策略表進(jìn)行分析,從而確定機(jī)器人的動(dòng)作策略,而不是憑空想象。
3.3 編寫(xiě)程序
編程環(huán)境:Arduino 1.8.19
編寫(xiě)并燒錄以下程序(Track_Car.ino),該程序?qū)?shí)現(xiàn)演示視頻中的動(dòng)作【詳細(xì)例程源代碼下載請(qǐng)見(jiàn) https://www.robotway.com/h-col-113.html】
/*------------------------------------------------------------------------------------ 版權(quán)說(shuō)明: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 機(jī)器譜 2023-02-09 https://www.robotway.com/ ------------------------------------------------------------------------------------*/ int pin[3] = {A0, A3, A4}; //按車(chē)頭前進(jìn)方向,從右至左定義,后面經(jīng)過(guò)公式計(jì)算,會(huì)轉(zhuǎn)化為從左至右的順序 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++) //循環(huán)獲取三個(gè)傳感器的值 { s|= (!digitalRead(pin[i]) << i); //經(jīng)過(guò)左移運(yùn)算和或運(yùn)算后,按照A0、A3、A4的順序產(chǎn)生一個(gè)三位2進(jìn)制數(shù)值,表示3個(gè)傳感器的組合觸發(fā)狀態(tài) } switch (s) { case 0x00: //三個(gè)均未觸發(fā) back(); Left(); break; case 0x01: //右側(cè)傳感器觸發(fā),直線(xiàn)上擺動(dòng)或遇到右轉(zhuǎn)彎 Right(); break; case 0x02: //中間傳感器觸發(fā),直線(xiàn)上直行 Forwards(); break; case 0x04: //左側(cè)傳感器觸發(fā),直線(xiàn)上擺動(dòng)或遇到左轉(zhuǎn)彎 Left(); break; case 0x06: //左側(cè)兩個(gè)觸發(fā),遇到左轉(zhuǎn)彎 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 ); }
審核編輯黃宇
-
傳感器
+關(guān)注
關(guān)注
2552文章
51237瀏覽量
754785 -
機(jī)器人
+關(guān)注
關(guān)注
211文章
28524瀏覽量
207552
發(fā)布評(píng)論請(qǐng)先 登錄
相關(guān)推薦
評(píng)論