2015年10月3日 星期六

ROSA 系統開發 7 ─ mBot 避碰模式

https://4rdp.blogspot.com/2015/10/rosa-7-mbot.html

前面章節已經介紹 ROSA 應用於 mBot 的紅外線操控車及循跡車,現在繼續加入避碰車功能。

// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)


void PROCESS_100ms(void){
  IR_CHECK();
  WA_CHECK();
}

// pin 17,16   UltraSound   A3,A2
void WA_CHECK(void){
  if (line_state != LINE_WA) return;
  if (GET_DISTANCE(17, 16) < 15.0)    // cm
    MOTOR(MOVE_RIGHT, car_speed);
  else
    MOTOR(MOVE_FORWARD, car_speed);
}

float GET_DISTANCE(unsigned char trig, unsigned char echo){
  D_OUT(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  pinMode(echo, INPUT);
//               pin   read     us
  return pulseIn(echo, HIGH, 30000) / 58.0;   // cm
}

首先於 100 ms 系統時間內加入 WA_CHECK 副程式,mBot 建議超音波偵測使用 port 3  A3,A2 腳,然後避碰規則設定為靠近 15 cm 以內就右轉閃避,否則直行前進。

超音波偵測是呼叫 GET_DISTANCE 副程式,A3 腳為 trig 控制偵測電路,此腳 HIGH level 時超音波發射電路啟動,請注意,平時 GET_DISTANCE 就開啟超音波發射,這是為了提早偵測障礙物,如果偵測時才開啟,當車速快或斜斜往牆面行進,因無法立即測到障礙物,將來不及轉彎而撞牆。A2 腳為 echo 讀取超音波訊號,這裡利用系統函數 pulseIn 來偵測,它讀取 echo HIGH  level 訊號長度,最長 30 ms 時間。

第六節文章已加入,紅外線遙控按鍵 B 會執行避碰模式, 從第一節到這第七節程式整合在一起,就是模仿 mBot 出廠時的功能,這裡不會提供整合好的程式,主要想訓練讀者程式整合的能力,只要把重複的程式碼去掉,並調整一下程式碼次序,即可正常運作,希望程式初學者能自己做一次,這樣才能提升程式除錯能力。

沒有留言:

張貼留言