目次 前 次

---

移動アルゴリズム

　メカを動かせるようになったので、移動アルゴリズムを
　考えていきます。



　Arduinoのloopの中に記述するコードを考えていきます。

　動作は、次のシーケンスになります。

路面センサーの情報取得
路面センサーの情報から左右のモータのDUTY比決定
左右のモータにDUTY比を出力
１にもどる

　平面を走行する場合、このシーケンスで充分ですが
　MCRのコースは、坂道や立体交差もあるので、その
　情報を加味してDUTY比を決定します。

路面センサーと傾斜センサーの情報取得
路面センサーの情報から左右のモータのDUTY比決定
傾斜センサーの情報により、DUTY比に係数をかける
左右のモータにDUTY比を出力
１にもどる

　路面センサーの情報からDUTY比を決定するためには
　どういう情報が来るのかを出していきます。

　８ビットのデータは、０から２５５までの２５６通り
　ありますが、これらすべてに対応しなくてもよいと
　わかっています。

　外せない４パターンは、以下。

ALL_BLACK   0000_0000(0x00)
ALL_WHITE   1111_1111(0xff)
LEFT_WHITE  1111_0000(0xf0)
LEFT_WHITE  1111_1000(0xf8)
RIGHT_WHITE 0000_1111(0x0f)
RIGHT_WHITE 0001_1111(0x1f)

　クランク、レーンチェンジを考えなければ
　白線がセンターにくるように、制御します。

　センターに白線がある場合のパターンは以下。

CENTER   0001_1000(0x18)
CENTER   0001_1100(0x1c)
CENTER   0011_1000(0x38)

　左あるいは右に寄っているときのパターンは以下。

LIMIT_RIGHT 0000_0011(0x03)
RIGHT       0000_0110(0x06)
BIT_RIGHT   0000_1100(0x0c)
BIT_LEFT    0011_0000(0x30)
LEFT        0110_0000(0x60)
LIMIT_LEFT  1100_0000(0xc0)

　走行状態によっては、両端に白線が見える
　こともあります。

BOTH_WHITE  1100_0011(0xc3)
BOTH_WHITE  1100_0001(0xc1)
BOTH_WHITE  1000_0011(0x83)
BOTH_WHITE  1000_0001(0x81)

　ここまでに挙げたパターン以外は、無視する
　仕様にして、スケッチを考えます。

　生の路面センサーから、文字列に変換した
　パターンでDUTY比を決めることに。
　文字列は、以下。

#define ALL_BLACK    0
#define ALL_WHITE    1
#define LEFT_WHITE   2
#define RIGHT_WHITE  3
#define CENTER       4
#define LIMIT_RIGHT  5
#define RIGHT        6
#define BIT_RIGHT    7
#define BIT_LEFT     8
#define LEFT         9
#define LIMIT_LEFT  10
#define BOTH_WHITE  11
#define ILLEAGAL    12

　生の路面センサーから変換する関数を定義します。

UBYTE decode(UBYTE x)
{
  UBYTE result ;
  /* judge */
  switch ( x ) {
    case 0x00 : result = ALL_BLACK   ; break ;
    case 0xff : result = ALL_WHITE   ; break ;
    case 0xf0 : result = LEFT_WHITE  ; break ;
    case 0xf8 : result = LEFT_WHITE  ; break ;
    case 0x0f : result = RIGHT_WHITE ; break ;
    case 0x1f : result = RIGHT_WHITE ; break ;
    case 0x18 : result = CENTER      ; break ;
    case 0x1c : result = CENTER      ; break ;
    case 0x38 : result = CENTER      ; break ;
    case 0x03 : result = LIMIT_RIGHT ; break ;
    case 0x06 : result = RIGHT       ; break ;
    case 0x0c : result = BIT_RIGHT   ; break ;
    case 0x30 : result = BIT_LEFT    ; break ;
    case 0x60 : result = LEFT        ; break ;
    case 0xc0 : result = LIMIT_LEFT  ; break ;
    case 0xc3 : result = BOTH_WHITE  ; break ;
    case 0xc1 : result = BOTH_WHITE  ; break ;
    case 0x83 : result = BOTH_WHITE  ; break ;
    case 0x81 : result = BOTH_WHITE  ; break ;
    default   : result = ILLEAGAL    ; break ;
  }

  return result ;
}

　走行を次の３状態に分け、より細かくDUTY比を
　設定できるようにします。

NORMAL
CRANK
LANE

　実際コースを見て、走行パターンを考えます。
　レーンチェンジから見ていきます。



　動作シーケンスは、以下とします。

路面センサー情報取得
情報を文字列変換
LEFT_WHITEかRIGHT_WHITEを検出したならLANEモードにする
dlane[0]、dlane[1]に文字列保存
最低速度でALL_BLACKを検出するまで直進
ALL_BLACKを検出したなら、dlane[0]が示す方向に移動
最低速度でCENTERを検出するまで直進
CENTERを検出したなら、dlane[1]が示す方向に移動
NORMALモードにして、ぬける

　クランクを見ます。



　動作シーケンスは、以下とします。

路面センサー情報取得
情報を文字列変換
ALL_WHITEを検出したならCRANKモードにする
最低速度でLEFT_WHITEかRIGHT_WHITEを検出するまで移動
LEFT_WHITEかRIGHT_WHITEを検出したならdcrankに文字列保存
最低速度でALL_BLACKを検出するまで直進
ALL_BLACKを検出したなら、dcrankが示す方向に移動
最低速度でCENTERを検出するまで直進
CENTERを検出したなら、dcrankが示す方向と逆に移動
NORMALモードにして、ぬける

　NORMALモードのシーケンスは、後で記述するとして
　途中までコードに変換します。

  sensor = get_sensor();
  sensorx = decode(sensor);
  /* judge */
  if ( sensorx == ALL_WHITE ) {
    if ( mode == NORMAL ) {
      /* change mode */
      mode = CRANK ;
      /* slow move */
    }
  }
  if ( sensorx == LEFT_WHITE || 
       sensorx == RIGHT_WHITE   ) {
    if ( mode == NORMAL ) {
      /* change mode */
      mode = LANE ; 
      /* slow move */
      /* */
      dlane[0] = sensorx ;
      if ( sensorx == LEFT_WHITE ) {
        dlane[1] = RIGHT_WHITE ;
      } else {
        dlane[1] = LEFT_WHITE ;
      }
      /* clear */
      turn_cnt = 0 ;
    }
    if ( mode == CRANK ) {
      dcrank = sensorx ;
      /* */
    }
  }
  if ( sensorx == ALL_BLACK ) {
    if ( mode == CRANK ) { 
    }
    if ( mode == LANE  ) {
      if ( turn_cnt == 0 ) {
        /* LEFT move */
        if ( dlane[turn_cnt] == LEFT_WHITE ) {
          /* set duty */
        }
        /* RIGHT move */
        if ( dlane[turn_cnt] == RIGHT_WHITE ) {
          /* set duty */
        }
        /* increment */
        turn_cnt++ ;
      } else {
        /* LEFT move */
        if ( dlane[turn_cnt] == LEFT_WHITE ) {
          /* set duty */
        }
        /* RIGHT move */
        if ( dlane[turn_cnt] == RIGHT_WHITE ) {
          /* set duty */
        }
        /* clear */
        turn_cnt = 0 ;
      }
    }
  }
  if ( sensorx == CENTER ) {

  }
  if ( sensorx == LIMIT_RIGHT ) {

  }
  if ( sensorx == RIGHT ) {

  }
  if ( sensorx == BIT_RIGHT ) {

  }
  if ( sensorx == BIT_LEFT ) {

  }
  if ( sensorx == LEFT ) {

  }
  if ( sensorx == LIMIT_LEFT ) {

  }
  if ( sensorx == BOTH_WHITE ) {

  }

　CRANK、LANEでは、旋回、車線変更が入るので
　時間待ちが必要になります。

　時間待ちは、Arduinoの組込み関数delayを利用します。

目次 前 次