目次

ライントレーサ制御

 ライントレーサは、床面に引かれたライン(線)に
 沿って移動するロボット。

 AmForthの基板に、次の路面センサーを接続し
 ライントレーサを動かしてみます。



 モータは、同じ形状のベースをスペーサを入れて
 電池ホルダーがはいるように組み上げます。




 下のベースには、TAMIYAのツインギアボックスを
 取り付け、天板におく基板に接続。

 天板に、マイコンと電子回路を載せた基板を配置。



 ラインセンサーは、ポートCに接続します。

 ポートCは、6ビット構成なので、ラインセンサーの
 出力8ビット中のMSB、LSBは接続しません。

 10ピンケーブルに、次のように信号が配置されています。

 1 Vcc
 2 sensor_7(MSB)
 3 sensor_6
 4 sensor_5
 5 sensor_4
 6 sensor_3
 7 sensor_2
 8 sensor_1
 9 sensor_0(LSB)
10 GND

 MSBとLSBを除いて、ポートCに接続。

 1 Vcc             : Vcc
 2 sensor_7(MSB)
 3 sensor_6        : PORTC.B5
 4 sensor_5        : PORTC.B4
 5 sensor_4        : PORTC.B3
 6 sensor_3        : PORTC.B2
 7 sensor_2        : PORTC.B1
 8 sensor_1        : PORTC.B0
 9 sensor_0(LSB)
10 GND             : GND

 モータを動かす制御回路は、以下。



 ATmega328のポートBに、モータドライブ回路を
 接続するとし、10ピンケーブルの信号配置を確認。

 1 Vcc
 2 red LED
 3 green LED
 4 switch (select speed)
 5 switch (start trigger)
 6 
 7 left driver in
 8 
 9 right driver in
10 GND

 ポートBとの接続は、以下とします。

 1 Vcc
 2 red LED                : PORTB.B4
 3 green LED              : PORTB.B3
 4 switch (select speed)
 5 switch (start trigger) : PORTB.B0
 6 
 7 left driver in         : PORTB.B2
 8 
 9 right driver in        : PORTB.B1
10 GND

 ビット1、2からは、PWM波形を出力できるので
 それを利用できるようにしておきます。

 制御基板の動作テストは、終わっているので
 定義したワードを確認。

\ initialize port b
: init.control
 $38 PORTB c!
 $fe DDRB c!
;

\ initialize timer1
: init.timer1
  $06 DDRB c@ or DDRB c!
  19999 ICR1 !
  %10100010 TCCR1A c! \ both pin impress low on compare matched
  %10 3 lshift %11 or TCCR1B c! \ /64 = 250kHz
;

\ set duty ratio type A
: set.dutya 400 * OCR1A ! ;

\ set duty ratio type B
: set.dutyb 400 * OCR1B ! ;

\ show switch state
: get_sw PINB c@ 1 and . ;

\ turn on LED #A
: leda.set $08 PORTB c@ or PORTB c! ;

\ turn off LED #A
: leda.clr $f7 PORTB c@ and PORTB c! ;

\ turn on LED #B
: ledb.set $10 PORTB c@ or PORTB c! ;

\ turn off LED #B
: ledb.clr $ef PORTB c@ and PORTB c! ;

 ポートB、Cの初期化は、まとめておいて、ひとつの
 ワードにした方が簡単。

: init.robot ( -- )
 $38 PORTB c! $fe DDRB c!
 $00 DDRC c!
;

 DUTY比を指定するワードは、まとめた方がわかりやすいので
 新たにワードを定義しておきましょう。

: set.duty ( left right -- )
 set.dutyb set.dutya
;

 センサーから床面情報を入力し、その値でモータに与える
 DUTY比を指定すれば、ライントレースできます。

 センサーからの床面情報を、変数Roadに格納するワードを
 定義しておきましょう。

variable Road{enter}

: get.road ( -- )
 PINC c@ Road c!
;

 変数Roadに6ビットの情報が格納されるので、左右の
 モータに設定するDUTY比を考えていきます。

 6ビットの情報は、床面パターンにすると64通り。
 ただし、ラインがある場合に、ありえないパターン
 には、パラメータを変えないという方式を採用。

 床面が白で、黒のラインが入れられている場合の
 パターンを考えていきます。

 ライントレースのコースの例は、以下。



 コースを見て考えていくと、次の結論に至るでしょう。

 床面の情報を端で見て、白を検出したとき、反対方向に
 動くように制御すれば、ラインに沿って移動できる。

 床面の情報を2進数でリストして、端に白を検出するパターンを抽出。

0 0 0 0 0 1 : mark
0 0 0 0 1 1 : mark
0 0 0 1 0 1
0 0 0 1 1 1 : mark
0 0 1 0 0 1
0 0 1 0 1 1
0 0 1 1 0 1
0 0 1 1 1 1
0 1 0 0 0 1
0 1 0 0 1 1
0 1 0 1 0 1
0 1 0 1 1 1
0 1 1 0 0 1
0 1 1 0 1 1
0 1 1 1 0 1
0 1 1 1 1 1
1 0 0 0 0 0 : mark
1 0 0 0 0 1
1 0 0 0 1 0
1 0 0 0 1 1
1 0 0 1 0 0
1 0 0 1 0 1
1 0 0 1 1 0
1 0 0 1 1 1
1 0 1 0 0 0
1 0 1 0 0 1
1 0 1 0 1 0
1 0 1 0 1 1
1 0 1 1 0 0
1 0 1 1 0 1
1 0 1 1 1 0
1 0 1 1 1 1
1 1 0 0 0 0 : mark
1 1 0 0 0 1
1 1 0 0 1 0
1 1 0 0 1 1
1 1 0 1 0 0
1 1 0 1 0 1
1 1 0 1 1 0
1 1 0 1 1 1
1 1 1 0 0 0 : mark
1 1 1 0 0 1
1 1 1 0 1 0
1 1 1 0 1 1
1 1 1 1 0 0
1 1 1 1 0 1
1 1 1 1 1 0
1 1 1 1 1 1

 2進数を10進数で表現し、DUTY比を表にします。

 反対方向の移動には、白を検出した側のモータの回転数を
 下げるようにして実現します。

0 0 0 0 0 1 :  1 => left 45 right 50
0 0 0 0 1 1 :  3 => left 40 right 50
0 0 0 1 1 1 :  7 => left 35 right 50
1 0 0 0 0 0 : 32 => left 50 right 45
1 1 0 0 0 0 : 48 => left 50 right 40
1 1 1 0 0 0 : 56 => left 50 right 35

 移動するために時間が必要なので、DUTY比を指定して1秒後に
 元のDUTY比に戻しておきます。

 動作シーケンスを作成。
  1. 路面情報取得
  2. 1ならばDUTY比を(45,50)に設定
  3. 3ならばDUTY比を(40,50)に設定
  4. 7ならばDUTY比を(35,50)に設定
  5. 32ならばDUTY比を(50,45)に設定
  6. 48ならばDUTY比を(50,40)に設定
  7. 56ならばDUTY比を(50,35)に設定
  8. 上記以外ならばDUTY比を(50,50)に設定
  9. 1秒待ち
  10. ステート1に戻る
 コードにまとめます。 begin 1 while get_road Road c@ 1 = if 45 50 set.duty else Road c@ 3 = if 40 50 set.duty else Road c@ 7 = if 35 50 set.duty else Road c@ 32 = if 50 45 set.duty else Road c@ 48 = if 50 40 set.duty else Road c@ 56 = if 50 35 set.duty else 50 50 set.duty then then then then then then 1000 MS repeat  変数定義と初期化を含めると、全体では次のようになります。 variable Road : run.robot init.robot begin 1 while get_road Road c@ 1 = if 45 50 set.duty else Road c@ 3 = if 40 50 set.duty else Road c@ 7 = if 35 50 set.duty else Road c@ 32 = if 50 45 set.duty else Road c@ 48 = if 50 40 set.duty else Road c@ 56 = if 50 35 set.duty else 50 50 set.duty then then then then then then 1000 MS repeat ;  路面センサーをシャーシにつけ、制御基板にArduino基板を  載せて、動かせるようにします。  IchigoJamと同じハードウエアを使うなら、LPC1114を  ATmega328に換装しての対応が可能。  IchigoJamの入出力ピンを次のようにしたとき  どんなワードに置換するか考えてみます。 IN_1 left sensor IN_2 right sensor IN_4 center sensor OUT_1 left motor OUT_2 right motor  ATmega328との接続を、以下とします。 PORTC.B0 IN_2 right sensor PORTC.B1 IN_4 center sensor PORTC.B2 IN_1 left sensor PORTB.B0 OUT_2 right motor PORTB.B1 OUT_1 left motor  ポートB、Cを出力、入力に設定。 : init.jamcar $00 PORTB c! $ff DDRB c! $00 DDRC c! ;  センサーから路面情報を入力して  変数に転送するワードを定義。 variable SIJC : get.sensor PINC c@ 7 and SIJC c! ;  左右のモータは、論理値の'1'を与えると  回転するとすれば、回転と停止をそれぞれ  ワードで定義して、わかりやすくできます。 : set.portb 1 swap lshift PORTB c@ or PORTB c! ; : clr.portb 1 swap lshift $ff xor PORTB c@ and PORTB c! ; : left.run 1 set.portb ; : left.stp 1 clr.portb ; : right.run 0 set.portb ; : right.stp 0 clr.portb ;  使うワードを定義したなら、動かし方を考えます。  センサーが出力してくる情報は、8通り。  この8通りのデータが得られたときに  モータの動かし方をまとめておきます。  走行するときのパターンは、4通りなるので  ワードで定義。 : move.stop left.stp right.stp ; : left.turn left.stp right.run ; : right.turn left.run right.stp ; : move.straight left.run right.run ;  センサーから情報を取得して、それに  応じたモータの回転をさせるとして  ライントレースをまとめると以下。 : line.trace init.jamcar move.stop begin 1 while get.sensor SIJC c@ 1 = if left.turn else SIJC c@ 3 = if left.turn else SIJC c@ 4 = if right.turn else SIJC c@ 6 = if right.turn else move.straight then then then then 1000 MS repeat ;  ワードごとに動作テスト後、基板上のForthインタプリタ  プロセッサに、渡せば完成です。  IchigoJamで動かすライントレーサのセンサーは  黒で'1'、白で'0'を出力すると連絡が来ました。  センサー入力のワードを変更して対応。 : get.sensor PINC c@ 7 and 7 xor SIJC c! ;  センサー入力の仕様変更があっても、ワードで  定義している限り、そのワードの再定義で対応  できるのは、関数型言語と同じです。

目次

inserted by FC2 system