走行パターン生成

　カメラから画像データを入力し、センサーデータを生成後
　走行パターンを作ります。

　無計画にセンサーデータを判断しても、スパゲッティコードに
　なるので、次の３状態に分割して、動作を解析します。

NORMAL
CRANK
LANE

　考えやすくするために、センサーデータを１個とします。

　NORMAL
　　ラインに沿いながらの移動です。
　　常にラインがあるので、直線、ワインディングコースを走行します。

　CRANK
　　クランク指示の白線を検出し、クランクで９０度曲がり
　　再びラインに沿って走行するまでの状態です。

　LANE
　　車線変更指示の片側白線を検出し、ラインなし路面を移動し
　　再びラインに沿って走行するまでの状態です。

　この３状態の中で、どう走るのかを考えます。

　状態を切り替えるには、条件が必要です。
　その条件を考えて、状態を切り替えると下図になります。



　大雑把ですが、この状態遷移を基に、ファームウエアを作成すると
　考えやすくなります。

　３状態を、さらに細かく分けてみます。


NORMAL
　NORMALは、ラインに沿いながら移動します。

　ラインを忠実に追って、走行するが基本です。

　はじめに、NORMALから他の状態にうつる条件を考えます。

　NORMAL→CRANK
　　路面全体に広がる白線(all white)を検出したなら、CRANKに状態を変えます。



　NORMAL→LANE
　　路面中央から左に引いた白線(left white)か右に引いた白線(right white)を
　　検出したなら、LANEに状態を変えます。



　状態遷移図のままでは、コードにできないので、シーケンスを書きます。

センサーデータ取得
センサーデータがall whiteなら、CRANKに遷移
センサーデータがleft whiteなら、LANEに遷移
センサーデータがright whiteなら、LANEに遷移
NORMAL処理
１に戻る

　NORMALでは、直線、ワインディングコース走行です。

　直線と直線でない場合、どのように動くか考えます。

　センサーデータが、どんな状態を示すかリストします。

中央に白線(center)
中央から少し左に白線(little left)
中央から左に白線(left)
中央から大きく左に白線(big left)
中央から少し右に白線(little right)
中央から右に白線(right)
中央から大きく右に白線(big right)

　コース上を滑らかに移動する場合は、もう少し状態を増やさ
　なければなりませんが、逸脱してコースから転落しなければ
　よいと考えて、７つの状態にします。

　それぞれの状態で、どのように制御するか決めます。

　center検出
　　左右モータの回転数を上げて移動
　　１秒経過後、もとの回転数に戻す

　little left検出
　　右モータの回転数を５％下げて移動
　　１秒経過後、もとの回転数に戻す

　left検出
　　右モータの回転数を１０％下げて移動
　　１秒経過後、もとの回転数に戻す

　big left検出
　　右モータの回転数を２０％下げて移動
　　１秒経過後、もとの回転数に戻す

　little right検出
　　左モータの回転数を５％下げて移動
　　１秒経過後、もとの回転数に戻す

　right検出
　　左モータの回転数を１０％下げて移動
　　１秒経過後、もとの回転数に戻す

　big right検出
　　左モータの回転数を２０％下げて移動
　　１秒経過後、もとの回転数に戻す

　まとめると、以下となります。



　処理内容から、次の関数が必要になります。


センサーデータ取得
時間指定遅延
左右モータの回転数設定

　ぞれぞれの内容をまとめて、switchで記述します。

  dtim = 1000 ;
  state = 0 ;
  while (1) {
    /* get sensor data */
    sensor_data = get_sensor();
    /* state machine */
    switch ( state ) {
      case 0 : /* judge */
        state = 0 ;
        if ( sensor_data == ALL_WHITE    ) { state = 100 ; }
        if ( sensor_data == LEFT_WHITE   ) { state = 200 ; }
        if ( sensor_data == RIGHT_WHITE  ) { state = 200 ; }
        if ( sensor_data == CENTER       ) { state =  10 ; }
        if ( sensor_data == LITTLE_LEFT  ) { state =  20 ; }
        if ( sensor_data == LEFT         ) { state =  30 ; }
        if ( sensor_data == BIG_LEFT     ) { state =  40 ; }
        if ( sensor_data == LITTLE_RIGHT ) { state =  50 ; }
        if ( sensor_data == RIGHT        ) { state =  60 ; }
        if ( sensor_data == BIG_RIGHT    ) { state =  70 ; }
        break ;
      /* CENTER */
      case 10 :
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left  <<= 3 ; dc_left  /= 10 ;
        dc_right <<= 3 ; dc_right /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
     /* LITTLE_LEFT (move center) */
     case 20 :
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left  *= 95 ; dc_left /= 100 ;
        update_motor(dc_left,dc_left);
        break ;
      /* LEFT (move center) */
      case 30 : 
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        do_right *= 9 ; do_right /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      /* BIG_LEFT (move center) */
      case 40 :
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        do_right <<= 3 ; do_right /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      /* LITTLE_RIGHT (move center) */
      case 50 :
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left *= 95 ; dc_left /= 100 ;
        update_motor(dc_left,dc_left);
        break ;
      /* RIGHT (move center) */
      case 60 :
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left *= 9 ; dc_left /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      /* BIG_RIGHT (move center) */
      case 70 :
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        do_left <<= 3 ; do_left /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      /* delay */
      case 80 :
        state = 90 ;
        delay_ms( dtim ) ;
        break ;
      /* return */
      case 90 :
        state = 0 ;
        dc_left  = do_left  ;
        dc_right = do_right ;
        break ;
      /* CRANK */
      case 100 :
        state = 0 ;
        break ;
      /* LANE */
      case 200 :
        state = 0 ;
        break ;
    }


CRANK
　CRANK状態での走行を考えます。

　右クランクの移動シーケンスは、次のようにします。

右クランク(crank_right)を検出するまで、低速直線走行
右に６０度回転
中央白線(center)を検出するまで、右に回転
１秒低速直線走行
NORMALに戻る

　右クランク走行と同様に、左クランクの移動シーケンスを考えます。

左クランク(crank_left)を検出するまで、低速直線走行
左に６０度回転
中央白線(center)を検出するまで、左に回転
１秒低速直線走行
NORMALに戻る

　クランクまでは、低速で移動し、クランクを見つけたなら、右か左に
　大きく回転します。
　その後、centerを見つけるまで、少しずつ回転します。



　移動シーケンスから、回転が必要になります。

　回転をrotateで表現すると、次のステートマシンを実行すれば
　CRANK状態の移動ができます。

      /* slow move */
      case 100 :
        state = 100 ;
        update_motor(20,20);
        delay_ms(1000);
        if ( sensor_data == CRANK_RIGHT ) {
          state = 110  ; angle0 = -60 ; angle1 =  -5 ;
        }
        if ( sensor_data == CRANK_LEFT  ) {
          state  = 110 ; angle0 =  60 ; angle1 =   5 ;
        }
        break ;
      /* rotate */
      case 110 :
        state = 120 ;
        rotate(angle0);
        delay_ms(3000);
        break ;
      /* rotate (bit by bit) */
      case 120 :
        state = 120 ;
        if ( sensor_data == CENTER ) { state = 130 ; }
        else                         { rotate(angle1) ; delay_ms(100); }
        break ;
      /* slow move */
      case 130 :
        state = 140 ;
        update_motor(20,20);
        delay_ms(1000);
        break ;
      /* return NORMAL */
      case 150 :
        state = 0 ;
        break ;



LANE
　LANE状態での走行を考えます。

　右の車線変更移動シーケンスは、次のようにします。

黒路面(all_black)を検出するまで、低速直線走行
右に３０度回転
中央白線(center)を検出するまで、中速直線走行
左に３０度回転
１秒低速直線走行
NORMALに戻る

　右車線変更と同様に、左車線変更の移動シーケンスを考えます。

黒路面(all_black)を検出するまで、低速直線走行
左に６０度回転
中央白線(center)を検出するまで、中速直線走行
右に３０度回転
１秒低速直線走行
NORMALに戻る

　all_blackまでは、低速で移動し、all_blackを見つけたなら、右か左に
　小さく回転します。
　回転後、centerを見つけるまで、中速で直線走行します。

  NORMALからLANEに状態遷移するとき、どちらに白線があったのかを
　記憶しておきます。



　次のステートマシンを実行すると、LANE状態の移動ができます。

      case 200 : /* slow move */
        state = 200 ;
        update_motor(20,20);
        delay_ms(1000);
        if ( sensor_data == ALL_BLACK ) {
          state = 210 ; 
          angle0 = -30 ; angle1 =  30 ;
          if ( lane_direction == LANE_LEFT ) { angle0 *= -1 ; angle1 *= -1 ; }
        }
        break ;
      case 210 : /* rotate */
        state = 220 ;
        rotate(angle0);
        delay_ms(1000);
        break ;
      case 220 : /* move */
        state = 220 ;
        if ( sensor_data == CENTER ) { state = 230 ; }
        else {
          update_motor(40,40);
          delay_ms(1000);
        }
        break ;
      case 230 : /* roate (reverse) */
        state = 240 ;
        rotate(angle1);
        delay_ms(1000);
        break ;
      case 240 : /* slow move */
        state = 250 ;
        update_motor(20,20);
        delay_ms(1000);
        break ;
      case 250 : /* return NORMAL */
        state = 0 ;
        break ;



ソースコード
　まとめると、走行パターン生成は、以下となります。

  UWORD state ;
  UBYTE sensor_data ;
  UWORD dtim ;
  UBYTE dc_left,dc_right;
  UBYTE do_left,do_right;
  SBYTE angle0,angle1;
  UBYTE lane_direction ;

  dc_left  = 30 ;
  dc_right = 30 ;
  angle0 = 0 ;
  angle1 = 0 ;
  state = 0 ;
  dtim = 1000 ;
  lane_direction = LANE_RIGHT ;
  while (1) {
    /* get sensor data */
    sensor_data = get_sensor();
    switch ( state ) {
      case 0 : /* judge */
        state = 0 ;
        if ( sensor_data == ALL_WHITE    ) { state = 100 ; }
        if ( sensor_data == LEFT_WHITE   ) {
          state = 200 ;
          lane_direction = LANE_LEFT ;
        }
        if ( sensor_data == RIGHT_WHITE  ) {
          state = 200 ;
          lane_direction = LANE_RIGHT ;
        }
        if ( sensor_data == CENTER       ) { state = 10 ; }
        if ( sensor_data == LITTLE_LEFT  ) { state = 20 ; }
        if ( sensor_data == LEFT         ) { state = 30 ; }
        if ( sensor_data == BIG_LEFT     ) { state = 40 ; }
        if ( sensor_data == LITTLE_RIGHT ) { state = 50 ; }
        if ( sensor_data == RIGHT        ) { state = 60 ; }
        if ( sensor_data == BIG_RIGHT    ) { state = 70 ; }
        break ;
      case 10 : /* CENTER */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left  <<= 3 ; dc_left  /= 10 ;
        dc_right <<= 3 ; dc_right /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      case 20 : /* LITTLE_LEFT (move center) */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_right *= 95 ; dc_right /= 100 ;
        update_motor(dc_left,dc_left);
        break ;
      case 30 : /* LEFT (move center) */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_right *= 9 ; dc_right /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      case 40 : /* BIG_LEFT (move center) */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_right <<= 3 ; dc_right /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      case 50 : /* LITTLE_RIGHT (move center) */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left *= 95 ; dc_left /= 100 ;
        update_motor(dc_left,dc_left);
        break ;
      case 60 : /* RIGHT (move center) */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left *= 9 ; dc_left /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      case 70 : /* BIG_RIGHT (move center) */
        state = 80 ;
        do_left  = dc_left  ;
        do_right = dc_right ;
        dc_left <<= 3 ; dc_left /= 10 ;
        update_motor(dc_left,dc_left);
        break ;
      case 80 : /* delay */
        state = 90 ;
        delay_ms( dtim ) ;
        break ;
      case 90 : /* return */
        state = 0 ;
        dc_left  = do_left  ;
        dc_right = do_right ;
        break ;

      case 100 : /* slow move */
        state = 100 ;
        update_motor(20,20);
        delay_ms(1000);
        if ( sensor_data == CRANK_RIGHT ) {
          state = 110  ; angle0 = -60 ; angle1 =  -5 ;
        }
        if ( sensor_data == CRANK_LEFT  ) {
          state  = 110 ; angle0 =  60 ; angle1 =   5 ;
        }
        break ;
      case 110 : /* rotate */
        state = 120 ;
        rotate(angle0);
        delay_ms(3000);
        break ;
      case 120 : /* rotate (bit by bit) */
        state = 120 ;
        if ( sensor_data == CENTER ) { state = 130 ; }
        else                         { rotate(angle1) ; delay_ms(100); }
        break ;
      case 130 : /* slow move */
        state = 140 ;
        update_motor(20,20);
        delay_ms(1000);
        break ;
      case 150 : /* return NORMAL */
        state = 0 ;
        break ;

      case 200 : /* slow move */
        state = 200 ;
        update_motor(20,20);
        delay_ms(1000);
        if ( sensor_data == ALL_BLACK ) {
          state = 210 ; 
          angle0 = -30 ; angle1 =  30 ;
          if ( lane_direction == LANE_LEFT ) { angle0 *= -1 ; angle1 *= -1 ; }
        }
        break ;
      case 210 : /* rotate */
        state = 220 ;
        rotate(angle0);
        delay_ms(1000);
        break ;
      case 220 : /* move */
        if ( sensor_data == CENTER ) { state = 230 ; }
        else {
          update_motor(40,40);
          delay_ms(1000);
        }
        break ;
      case 230 : /* roate (reverse) */
        state = 240 ;
        rotate(angle1);
        delay_ms(1000);
        break ;
      case 240 : /* slow move */
        state = 250 ;
        update_motor(20,20);
        delay_ms(1000);
        break ;
      case 250 : /* return NORMAL */
        state = 0 ;
        break ;
    }
  }

目次 前 次