平面地圖自動探測、繪製機

Objective

用Nitrogen6X控制Boe-Bot Robot使其能夠自動探索限定範圍內的區域，搭配其上的距離感測，讓自走車避開障礙並記錄障礙位置，同時繪製成地圖在host端顯示。

方法概念

地圖資料記錄

宣告plane[10][10]作為地圖資料(先假設地圖大小不超過10乘10，而每一格代表實際地圖上一個20cm乘20cm的方格，一開始data全為0，每次移動前將車目前座標data改為1，並偵測左方、前方、右方有沒有障礙物；若有，則將該方向之隔壁座標設為2。

0 ----代表空地

1 ----代表領地(走過的路)

2 ----代表障礙

Key code

%偵測周圍

servo_angle(STANDARD_MOTOR, 1800);
pause(1000);
cmDist1 = ping_cm(17);`
servo_angle(STANDARD_MOTOR, 900);
pause(500);
cmDist2 = ping_cm(17);
servo_angle(STANDARD_MOTOR, 0);
pause(500);`
cmDist3 = ping_cm(17);`

%判別障礙

if((way%4) == 0){`
if(cmDist1 < UNIDIS) plane[posx_now][posy_now-1]=2;`
if(cmDist2 < UNIDIS) plane[posx_now+1][posy_now]=2;`
if(cmDist3 < UNIDIS) plane[posx_now][posy_now+1]=2;`
}
%way代表的是車子面對的方向，藉以判別當時的三個方向鄰近點與車子目前座標的位置關係，這裡指擷取其中一種。`

Boe-Bot Robot移動決策

將車子一開始的座標設在plane[1][1]，故每次探索地圖前須將車子放在想探索區域的左下角，在做完該地點的地圖資料記錄後，根據目前車子的左方、前方、右方鄰近座標data判定下一步要做什麼，並且更新車子所在座標，可能的選擇有：

前進20cm----根據way(見地圖資料記錄)決定如何更新車子所在座標

左轉90度----way = way-1

右轉90度----way = way+1

右轉180度----way = way+2

由於code方面太複雜及冗長，這部分不附上，大致的策略是讓Boe-Bot Robot沿著當前左方的牆壁或是領地，由外向內將地圖做地毯式的探索，而車子的決策方式如下：

額外功能1 – 亮度感測

因在未知的地圖上可能會遇到山洞、陰暗的地區，故在車子上加裝了亮度感測器，並宣告光之地圖light[10][10]，座標和plane一起更新，並在感測到當前位置亮度不佳時將該座標data改為10。

Key code

int t = rc_time(5, 1); // Measure decay time on P5
// print("t = %d\n", t); // Display decay time
if(t > 500) light[posx_now][posy_now] = 10;

額外功能2—生物驅趕

因為探索地圖的時候有可能會遇到小動物如老鼠等的干擾，為避免其影響車子的判斷甚至直接碰撞車子使其改向或位移，加裝了蜂鳴器作為生物驅趕裝置，而因為距離感測裝置無法判定偵測到的物體是動物還是無生物障礙，且正常行進下車子應會避免撞上偵測物，故加裝了蟑螂腳作為生物驅趕裝置的啟動機制，主要驅趕已經貼近到能碰觸車子距離的生物。

Key code

void SOUD(){
while(1){
int wL = input(7); // Left whisker -> wL variable
int wR = input(8); // Right whisker -> wR variable
// print("%c", HOME); // Terminal cursor home (top-left)
// print("wL = %d wR = %d", wL, wR); // Display whisker variables
if(wL == 0 || wR == 0){
freqout(4, 2000, 3030);
}
// pause(50);
}
}
(因為此機制必須與其他功能同步啟動，故宣告了SOUD function並在code中以cog_run(SOUD,128)呼叫其與main中的主要while function並行。)

Boe-Bot Robot回收機制

用和Boe-Bot Robot移動決策中相同的判斷方法，但這次的終止條件設為車子當前座標回到plane[1][1]。

Data回傳

利用兩層for loop將判定結果(by plane、light)一一傳回Nitrogen6X，並在接收時同樣利用兩層for loop一一顯示出回傳的data，繪製出簡單的地圖資訊。

D – 表陰暗地

O – 表明亮空地

X – 表有障礙物或牆壁之地

? – 表未知領域或地圖邊界之外區域

Key code

for (i = 9 ; i >= 0; i--){
for (k = 0 ; k < 10; k++){
// plane[i][k] = plane[i][k] + light[i][k];//if(plane[i][k]==0){
if(light[i][k] == 10) ch = 'D';
else{
if(plane[i][k] == 0){
ch = '?';
}
if(plane[i][k] == 1){
ch = 'O';
}
if(plane[i][k] == 2){
ch = 'X';
}
}
fdserial_txChar(xbee, ch);
fdserial_txFlush(xbee);
plane[i][k] = 0;
}
}

Host端

修改lab11用過的XBee_test.c，將最下面的while改成下方的code，便不會顯傳送了什麼指令，而是顯示回傳回來的地圖資料。

Key code

for (i = 9 ; i >= 0; i--){
for (k = 0 ; k < 10; k++){
	recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message
    	tcdrain(serial_d);
    	printf("%s\n\n", recv_buf);
}
printf("\n");
}

如此便能繪製出地圖了！

使用的器材及工具

Nitrogen6x board

Boe-Bot Robot(車子)

C語言

Simpleide

XBee

電路

蜂鳴器電路

光敏電阻電路

遇到的問題

為了讓車子能夠更自由地探索大範圍區域，需使用安插於其上的電池供電，但是電池的蓄電力似乎不太夠，常常走到後來會開始慢慢地偏移正常軌道或是每次移動的距離越來越短，甚至無法完成回收車子的動作就停止了，因此無法探索太大的區域，demo實用的4乘4地圖已經接近極限。

Demo結果

(影片中由於電池已經快要沒電，故轉彎和前進距離稍微有些誤差，所以有用手稍微輔助一下xD) https://www.youtube.com/watch?v=6djhIwZISPs