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

http://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 ─ 循跡車。