目次
前
次
ライントレーサ制御
ライントレーサは、床面に引かれたライン(線)に
沿って移動するロボット。
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ならばDUTY比を(45,50)に設定
- 3ならばDUTY比を(40,50)に設定
- 7ならばDUTY比を(35,50)に設定
- 32ならばDUTY比を(50,45)に設定
- 48ならばDUTY比を(50,40)に設定
- 56ならばDUTY比を(50,35)に設定
- 上記以外ならばDUTY比を(50,50)に設定
- 1秒待ち
- ステート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通りのデータが得られたときに
モータの動かし方をまとめておきます。
- 0(%000) on line -> left.run right.run
- 1(%001) tiny right -> left.stp right.run
- 2(%010) *illeagal -> left.run right.run
- 3(%011) right -> left.stp right.run
- 4(%100) tiny left -> left.run right.stp
- 5(%101) *illeagal -> left.run right.run
- 6(%110) left -> left.run right.stp
- 7(%111) *illeagal -> left.run right.run
走行するときのパターンは、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!
;
センサー入力の仕様変更があっても、ワードで
定義している限り、そのワードの再定義で対応
できるのは、関数型言語と同じです。
目次
前
次