2015年9月26日 星期六

ROSA 系統開發 6 ─ mBot 循跡自走

https://4rdp.blogspot.com/2015/09/rosa-6-mbot.html

mBot 的循跡功能是藉由尋線感應器 (Line Sensor) 感知黑色與白色狀態,只要檢知不是黑色的情形,尋線感應器就會亮燈,並回傳檢知狀態到控制板,檢知黑色時,數位輸入訊號為 low (0 V),反之為 high (5 V)。

mBot 的循線感應器有兩個感知端點,以車體俯視角度來看,兩個檢知燈都亮時,表示車子不在線上,兩個燈都滅時,表示車子壓在線上,亮右燈表示車子偏右邊,亮左邊則偏左邊。

根據上述說明,先加入前面章節程式碼,下面為循跡自走範例:


// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)
// pin 9,10  line
enum {
LINE_STOP,   // MANUAL CONTROL
LINE_WA,     // WALL AVOIDANCE
LINE_IN,     // LINE START
LINE_LEFT,
LINE_RIGHT,
LINE_OUT
};
enum {
LEFT_IN,
LEFT_LEFT,
LEFT_RIGHT,
LEFT_OUT,

RIGHT_IN,
RIGHT_LEFT,
RIGHT_RIGHT,
RIGHT_OUT,
};

byte line_alteration;
byte line_state;
byte car_speed = 255;

void LINE_CHECK(void) {
byte i;
if (line_state >= LINE_IN) {
if (D_IN(9) == LOW)
i = (D_IN(10) == LOW) ? LINE_IN : LINE_RIGHT;    // LH  @ line right side
else
i = (D_IN(10) == LOW) ? LINE_LEFT : LINE_OUT;     // HL  @ line left side
line_alteration = (line_state - LINE_LEFT) * 4 + i - LINE_IN;
if (i == LINE_LEFT || i == LINE_RIGHT)
line_state = i;
LINE_RULE();
}
}
void LINE_RULE(void){
switch (line_alteration){
case LEFT_IN:
MOTOR(MOVE_FORWARD, car_speed);
break;
case LEFT_LEFT:
MOTOR(MOVE_FORWARD, car_speed);
break;
case LEFT_RIGHT:
MOTOR(MOVE_FORWARD, car_speed);
break;
case LEFT_OUT:
MOTOR(MOVE_RIGHT, car_speed);
break;
case RIGHT_IN:
MOTOR(MOVE_FORWARD, car_speed);
break;
case RIGHT_LEFT:
MOTOR(MOVE_FORWARD, car_speed);
break;
  case RIGHT_RIGHT:
MOTOR(MOVE_FORWARD, car_speed);
break;
case RIGHT_OUT:
MOTOR(MOVE_LEFT, car_speed);
break;
default:
MOTOR(MOVE_FORWARD, car_speed);
break;
}
}

// pin 2,3   IR
#include "MeIR.h"
MeIR ir;

void IR_CHECK() {   // for mBot car
byte i;
switch (ir.key()) {
case 64: // mBot-up
MOTOR(MOVE_FORWARD, car_speed);
line_state = LINE_STOP;
break;
case 25: // mBot-down
MOTOR(MOVE_BACK, car_speed);
line_state = LINE_STOP;
break;
case 7:  // mBot-left
MOTOR(MOVE_LEFT, car_speed);
line_state = LINE_STOP;
break;
case 9:  // mBot-right
MOTOR(MOVE_RIGHT, car_speed);
line_state = LINE_STOP;
break;
case 69: // mBot-A      MANUAL CONTROL
MOTOR(MOVE_STOP, 0);
line_state = LINE_STOP;
break;
case 70: // mBot-B      WALL AVOIDANCE
MOTOR(MOVE_STOP, 0);
line_state = LINE_WA;
break;
case 71: // mBot-C      LINE START
MOTOR(MOVE_STOP, 0);
line_state = LINE_IN;
break;
    case 68: // mBot-D
              break;
    case 67: // mBot-E
              break;
    case 13: // mBot-F
              break;
    case 22: // mBot-0
              break;
    case 12: // mBot-1
      car_speed = 50;
      break;
    case 24: // mBot-2
      car_speed = 75;
              break;
    case 94: // mBot-3
      car_speed = 100;
              break;
    case  8: // mBot-4
      car_speed = 125;
              break;
    case 28: // mBot-5
      car_speed = 150;
              break;
    case 90: // mBot-6
      car_speed = 175;
              break;
    case 66: // mBot-7
      car_speed = 200;
              break;
    case 82: // mBot-8
      car_speed = 225;
              break;
    case 74: // mBot-9
      car_speed = 255;
              break;
    case 21: // mBot-setup
              break;
default:
break;
}
ir.loop();
}

void PROCESS_100ms(void){
  IR_CHECK();

}

void PROCESS_10ms(void){
  LINE_CHECK();
}


系統每 10 ms 執行 LINE_CHECK 檢查循跡狀況,每 100 ms 執行 IR_CHECK 檢查紅外線按鍵操作。

首先改寫 IR_CHECK 副程式,將遙控器按鍵加入幾個功能:
按鍵 A 為手動控制模式,按它 mBot 立即停止,
按鍵 B 為避碰車模式,
按鍵 C 為循跡車模式。
上下左右方向鍵為手動控制時,方向控制。
按鍵 1 ~ 9 調整車速,car_speed 數值越高,車速越快。

另外以 line_state 紀錄 mBot 的狀態,這裡定義常數 LINE_STOP 停止,也表示手動控制,LINE_WA 為避碰模式,LINE_IN 為進入循線模式,LINE_LEFT、LINE_RIGHT 分別是在線的左邊及右邊,LINE_OUT 為出線情形。

關於 LINE_CHECK 副程式,原本循線狀態應該有 16 種情形,個人將它簡化成 8 種,也就是壓線在左右兩邊時,就記錄線的位置,然後 LINE_RULE 副程式檢查 mBot 超出線時,就左右轉修正,例如車子前一刻在線的右邊,而下一刻卻超出線,這表示車子在線的更右邊,應該向左修正,直到接觸到線為止。
前一位置>目前位置
在線左
在線中
在線右
超出線
在線左
紀錄位置
繼續直行

繼續直行
紀錄位置
繼續直行

向右修正
在線右
紀錄位置
繼續直行

繼續直行
紀錄位置
繼續直行

向左修正
如果你有更好的控制方法,可以修正 LINE_RULE 副程式。

其它未列出的副程式,請自行參考前文加入。影片可以參考演算法訓練 9 ─ 循跡車

2 則留言:

  1. Bridan,您試拍一段循跡車的video~ 應該是比樂高的color sensor循跡車更好

    回覆刪除
    回覆
    1. 薛老師,

      中秋及教師節快樂,謝謝你的稱讚,這是我第一次寫循跡車程式,寫程式時就在想,如何讓我的程式跟別人不一樣,這裡的控制規則就是要簡單易懂,並且還要預留ROSA架構,好方便未來擴充功能。

      mBot 的八字圖紙反光很厲害,這部分樂高的 color sensor + PID 比較適合處理這種問題。

      刪除