目次
前
次
新マルチプロセッサ構成
過年度のマシンの制御は、次の３モードに分けていました。
これまでのテストから、FPGA内部に５個程度の
マイクロコントローラを入れられるとわかって
います。
モードごとに専用プロセッサを用意し、センサー
情報を個別に処理し、マスタープロセッサが各々
の出力パラメータを、モータ制御回路に渡す方式
が最も簡単と判断しました。
この方式を採用すると、必要なプロセッサは５個
程度でよくなります。
- MASTER PROCESSOR
- SENSOR HANDLER
- NORMAL PROCESSOR
- CRANK PROCESSOR
- LANE PROCESSOR
マルチプロセッサは、人間の大脳、小脳、延髄に
区別された部位と似た構成になります。
MCR_VCマシンは、小規模なロボットなので
各部位を人間に似せて構成するのが、最も
自然だと判断しました。
Subsamption理論と同じ考え方です。
各プロセッサが担当する機能を定義します。
MASTER PROCESSOR
スタートトリガー、スタートセンサーを利用し
他のプロセッサが動作できるようにクロックを
イネーブルにします。
マイクロコンピュータに内蔵されるモジュール
（PWM波形ジェネレータやA/D変換器）を、使う
使わないは、対応レジスタで電源電圧を与える
かどうかを指定します。プロセッサはクロック
を与えて動かすので、クロックを制御すること
で、該当プロセッサの動作を制御します。
またSENSOR HANDLERが出力した情報から、NORMAL、CRANK、LANE
のどのプロセッサを動かすのかを決めます。
SENSOR HANDLER
Bar Code ScannerあるいはGame Boy Cameraからの情報を
入力し、センサーデータに変換します。
センサーデータは、次のコードに対応させます。
ALL_BLACK 0
ALL_WHITE 1
LEFT_WHITE 2
RIGHT_WHITE 3
CENTER 4
TINY_RIGHT 5
RIGHT 6
BIG_RIGHT 7
TINY_LEFT 8
LEFT 9
BIG_LEFT 10
BOTH_WHITE 11
ILLEAGAL 12
センサーデータを、コードに対応させると
４ビットまで情報圧縮できるので、移動に
関係する判定を少なくできます。
NORMAL PROCESSOR
センサーデータを入力して、直線、曲線コースの移動で
モータに与えるPWM波形のDUTY比を求めます。
センサーデータは、13通りなので左右にテーブルを
個別に割り当てても、26通りにしかならないです。
実際に動かしてDUTY比を確定するので、テーブルを
使わずに、センサーデータ対応の処理を記述します。
;++++++++++++++++++++++++
; sensor code
;++++++++++++++++++++++++
CONSTANT ALL_BLACK, 00
CONSTANT ALL_WHITE, 01
CONSTANT LEFT_WHITE, 02
CONSTANT RIGHT_WHITE,03
CONSTANT CENTER, 04
CONSTANT TINY_RIGHT, 05
CONSTANT RIGHT, 06
CONSTANT BIG_RIGHT, 07
CONSTANT TINY_LEFT, 08
CONSTANT LEFT, 09
CONSTANT BIG_LEFT, 0A
CONSTANT BOTH_WHITE, 0B
CONSTANT ILLEAGAL, 0C
;++++++++++++++++++++++++
; I/O address
;++++++++++++++++++++++++
CONSTANT SENCODE, 00
CONSTANT PWMRIGHT,01
CONSTANT PWMLEFT, 02
;++++++++++++++++++++++++
; entry address
;++++++++++++++++++++++++
ADDRESS 000
;++++++++++++++++++++++++
; initialize
;++++++++++++++++++++++++
INIT:
; clear
XOR s0,s0
LOAD sD,s0
; PWM duty ratio
LOAD sA,20
LOAD sB,20
;++++++++++++++++++++++++
; main loop
;++++++++++++++++++++++++
MAIN:
CALL GETS
; judge
COMPARE s0,CENTER
JUMP NZ,MAIN0
; set duty
LOAD sA,20
LOAD sB,20
JUMP MAINX
; big left
MAIN0:
COMPARE s0,BIG_LEFT
JUMP NZ,MAIN1
; set duty
LOAD sA,05
LOAD sB,25
JUMP MAINX
; left
MAIN1:
COMPARE s0,LEFT
JUMP NZ,MAIN2
; set duty
LOAD sA,15
LOAD sB,25
JUMP MAINX
; tiny left
MAIN2:
COMPARE s0,LEFT
JUMP NZ,MAIN3
; set duty
LOAD sA,20
LOAD sB,25
JUMP MAINX
; big right
MAIN3:
COMPARE s0,BIG_RIGHT
JUMP NZ,MAIN4
; set duty
LOAD sA,25
LOAD sB,05
JUMP MAINX
; right
MAIN4:
COMPARE s0,BIG_RIGHT
JUMP NZ,MAIN5
; set duty
LOAD sA,25
LOAD sB,15
JUMP MAINX
; tiny right
MAIN5:
COMPARE s0,BIG_RIGHT
JUMP NZ,MAIN6
; set duty
LOAD sA,25
LOAD sB,20
JUMP MAINX
; all white
MAIN6:
COMPARE s0,ALL_WHITE
JUMP NZ,MAIN7
; set duty
LOAD sA,15
LOAD sB,15
JUMP MAINX
; left white
MAIN7:
COMPARE s0,LEFT_WHITE
JUMP NZ,MAIN8
; set duty
LOAD sA,25
LOAD sB,05
JUMP MAINX
; right white
MAIN8:
COMPARE s0,RIGHT_WHITE
JUMP NZ,MAIN9
; set duty
LOAD sA,05
LOAD sB,25
JUMP MAINX
;
MAIN9:
;
MAINX:
CALL PUTS
CALL DELAY
;
JUMP MAIN
;+++++++++++++++++++
; store sensor code
;+++++++++++++++++++
GETS:
INPUT s0,SENCODE
RETURN
;++++++++++++++++++++
; impress duty ratio
;++++++++++++++++++++
PUTS:
OUTPUT sA,PWMRIGHT
OUTPUT sB,PWMLEFT
RETURN
;++++++++++++++++++++
; impress duty ratio
;++++++++++++++++++++
DELAY:
LOAD sC,10
; loop
DELAY0:
COMPARE sC,00
JUMP Z,DELAY1
;
SUB sC,01
JUMP DELAY0
;
DELAY1:
;
RETURN
CRANK PROCESSOR
CRANK MARKERの白線を検出後、低速で右か左の直角ラインに
到達したなら旋回させます。
センターラインを検出したなら、モータへの制御信号出力を
を停止します。
３分割した処理で記述します。
- ENT_STATE
- GO_BLACK
- EXE_TURN
ENT_STATEは、白線検出の処理で、NORMALモードからCRANKに
切替わったとして、オーバーラップ時の動作を確保します。
GO_BLACKは、ラインに沿ってスロー走行し、黒を検出する
までの移動を担当します。途中で、左か右の片側白線情報
を取得します。
EXE_TURNは、旋回制御を担当します。
;++++++++++++++++++++++++
; sensor code
;++++++++++++++++++++++++
CONSTANT ALL_BLACK, 00
CONSTANT ALL_WHITE, 01
CONSTANT LEFT_WHITE, 02
CONSTANT RIGHT_WHITE,03
CONSTANT CENTER, 04
CONSTANT TINY_RIGHT, 05
CONSTANT RIGHT, 06
CONSTANT BIG_RIGHT, 07
CONSTANT TINY_LEFT, 08
CONSTANT LEFT, 09
CONSTANT BIG_LEFT, 0A
CONSTANT BOTH_WHITE, 0B
CONSTANT ILLEAGAL, 0C
;++++++++++++++++++++++++
; I/O address
;++++++++++++++++++++++++
CONSTANT SENCODE, 00
CONSTANT PWMRIGHT,01
CONSTANT PWMLEFT, 02
;++++++++++++++++++++++++
; entry address
;++++++++++++++++++++++++
ADDRESS 000
;++++++++++++++++++++++++
; initialize
;++++++++++++++++++++++++
INIT:
; clear
XOR s0,s0
LOAD s1,s0
LOAD sE,s0
; PWM duty ratio
LOAD sA,20
LOAD sB,20
;++++++++++++++++++++++++
; main loop
;++++++++++++++++++++++++
MAIN:
CALL ENT_STATE
; judge
COMPARE s1,01
JUMP NZ,MAINX
; slow
CALL PUTS
; slow move to ALL_BLACK
CALL GO_BLACK
; turn
CALL EXE_TURN
;
MAINX:
;
JUMP MAIN
;+++++++++++++++++++
; store sensor code
;+++++++++++++++++++
GETS:
INPUT s0,SENCODE
RETURN
;++++++++++++++++++++
; impress duty ratio
;++++++++++++++++++++
PUTS:
OUTPUT sA,PWMRIGHT
OUTPUT sB,PWMLEFT
RETURN
;++++++++++++++++++++
; impress duty ratio
;++++++++++++++++++++
DELAY:
LOAD sC,10
; loop
DELAY0:
COMPARE sC,00
JUMP Z,DELAY1
;
SUB sC,01
JUMP DELAY0
;
DELAY1:
;
RETURN
;++++++++++++++++++++
; slow move
;++++++++++++++++++++
ENT_STATE:
; get sensor code
CALL GETS
;
ENT_STATE0:
; judge
COMPARE s0,ALL_WHITE
JUMP Z,ENT_STATE1
; clear flag
LOAD s1,00
CALL DELAY
CALL GETS
JUMP ENT_STATE0
; set duty ratio
ENT_STATE1:
LOAD sA,10
LOAD sB,10
CALL PUTS
CALL DELAY
; set flag
LOAD s1,01
;
RETURN
;++++++++++++++++++++++++
; slow move to ALL_BLACK
;++++++++++++++++++++++++
GO_BLACK:
; set duty ratio
LOAD sA,15
LOAD sB,15
CALL PUTS
CALL DELAY
; loop
GO_BLACK0:
CALL GETS
; jude pass line area
COMPARE s0,ALL_BLACK
JUMP Z,GO_BLACKX
; judge
COMPARE s0,CENTER
JUMP NZ,GO_BLACK1
; center
LOAD sA,15
LOAD sB,15
JUMP GO_BLACK9
; big left
GO_BLACK1:
COMPARE s0,BIG_LEFT
JUMP NZ,GO_BLACK2
; set duty
LOAD sA,05
LOAD sB,15
JUMP GO_BLACK9
; left
GO_BLACK2:
COMPARE s0,LEFT
JUMP NZ,GO_BLACK3
; set duty
LOAD sA,10
LOAD sB,15
JUMP GO_BLACK9
; tiny left
GO_BLACK3:
COMPARE s0,TINY_LEFT
JUMP NZ,GO_BLACK4
; set duty
LOAD sA,12
LOAD sB,15
JUMP GO_BLACK9
; big right
GO_BLACK4:
COMPARE s0,BIG_RIGHT
JUMP NZ,GO_BLACK5
; set duty
LOAD sA,15
LOAD sB,05
JUMP GO_BLACK9
; right
GO_BLACK5:
COMPARE s0,RIGHT
JUMP NZ,GO_BLACK6
; set duty
LOAD sA,15
LOAD sB,10
JUMP GO_BLACK9
; tiny right
GO_BLACK6:
COMPARE s0,TINY_RIGHT
JUMP NZ,GO_BLACK9
; set duty
LOAD sA,15
LOAD sB,12
JUMP GO_BLACK9
; right edge
GO_BLACK7:
COMPARE s0,RIGHT_WHITE
JUMP NZ,GO_BLACK8
; set direction
LOAD sE,01
JUMP GO_BLACK9
; left edge
GO_BLACK8:
COMPARE s0,LEFT_WHITE
JUMP NZ,GO_BLACK9
; set direction
LOAD sE,02
;
GO_BLACK9:
CALL PUTS
CALL DELAY
JUMP GO_BLACK0
;
GO_BLACKX:
RETURN
;+++++++
; turn
;+++++++
EXE_TURN:
; ? RIGHT
COMPARE sE,01
JUMP Z,EXE_TURN1
; ? LEFT
JUMP EXE_TURN2
; right
EXE_TURN1:
LOAD sA,10
LOAD sB,25
JUMP EXE_TURN3
; left
EXE_TURN2:
LOAD sA,25
LOAD sB,10
;
EXE_TURN3:
CALL PUTS
; loop
EXE_TURN4:
CALL GETS
; judge
COMPARE s0,CENTER
JUMP NZ,EXE_TURN4
;
LOAD sA,10
LOAD sB,10
CALL PUTS
CALL DELAY
;
RETURN
LANE PROCESSOR
LANE CHANGE MARKERの片側白線を検出後、低速で移動し
ラインが切れたなら、コースを変更します。
センターラインを検出したなら、モータへの制御信号出力を
停止します。
６分割した処理で記述します。
- ENT_STATE
- GO_BLACK
- FIRST_TURN
- GO_CENTER
- SECOND_TURN
- GO_CENTER
ENT_STATEは、白線検出の処理で、NORMALモードからLANEに
切替わったとして、オーバーラップ時の動作を確保します。
GO_BLACKは、ラインに沿ってスロー走行し、黒を検出する
までの移動を担当します。
FIRST_TURNは、片側白線で指定されたレーン変更を担当します。
GO_CENTERは、すべて黒のエリアを走行し、センターラインを
検出するまでの移動を担当します。
SECOND_TURNは、FIRST_TURNと反対方向の移動を担当します。
;++++++++++++++++++++++++
; sensor code
;++++++++++++++++++++++++
CONSTANT ALL_BLACK, 00
CONSTANT ALL_WHITE, 01
CONSTANT LEFT_WHITE, 02
CONSTANT RIGHT_WHITE,03
CONSTANT CENTER, 04
CONSTANT TINY_RIGHT, 05
CONSTANT RIGHT, 06
CONSTANT BIG_RIGHT, 07
CONSTANT TINY_LEFT, 08
CONSTANT LEFT, 09
CONSTANT BIG_LEFT, 0A
CONSTANT BOTH_WHITE, 0B
CONSTANT ILLEAGAL, 0C
;++++++++++++++++++++++++
; I/O address
;++++++++++++++++++++++++
CONSTANT SENCODE, 00
CONSTANT PWMRIGHT,01
CONSTANT PWMLEFT, 02
;++++++++++++++++++++++++
; entry address
;++++++++++++++++++++++++
ADDRESS 000
;++++++++++++++++++++++++
; initialize
;++++++++++++++++++++++++
INIT:
; clear
XOR s0,s0
LOAD s1,s0
LOAD sE,s0
LOAD sF,sE
; PWM duty ratio
LOAD sA,20
LOAD sB,20
;++++++++++++++++++++++++
; main loop
;++++++++++++++++++++++++
MAIN:
CALL ENT_STATE
; judge
COMPARE s1,01
JUMP NZ,MAINX
; slow
CALL PUTS
; slow move to ALL_BLACK
CALL GO_BLACK
; turn
CALL FIRST_TURN
; slow move to CENTER
CALL GO_CENTER
; turn
CALL SECOND_TURN
; slow move to CENTER
CALL GO_CENTER
;
MAINX:
;
JUMP MAIN
;+++++++++++++++++++
; store sensor code
;+++++++++++++++++++
GETS:
INPUT s0,SENCODE
RETURN
;++++++++++++++++++++
; impress duty ratio
;++++++++++++++++++++
PUTS:
OUTPUT sA,PWMRIGHT
OUTPUT sB,PWMLEFT
RETURN
;++++++++++++++++++++
; slow move
;++++++++++++++++++++
ENT_STATE:
; get sensor code
CALL GETS
; judge
COMPARE s0,LEFT_WHITE
JUMP Z,ENT_STATE0
;
COMPARE s0,RIGHT_WHITE
JUMP Z,ENT_STATE1
; clear flag
LOAD s1,00
; exit
JUMP ENT_STATE2
; LEFT_WHITE set directions
ENT_STATE0:
LOAD sE,02
LOAD sF,01
; set flag
LOAD s1,01
JUMP ENT_STATE2
; RIGHT_WHITE set directions
ENT_STATE1:
LOAD sE,01
LOAD sF,02
; set flag
LOAD s1,01
ENT_STATE2:
;
RETURN
;++++++++++++++++++++++++
; slow move to ALL_BLACK
;++++++++++++++++++++++++
GO_BLACK:
; set duty ratio
LOAD sA,15
LOAD sB,15
CALL PUTS
; loop
GO_BLACK0:
CALL GETS
; jude pass line area
COMPARE s0,ALL_BLACK
JUMP Z,GO_BLACKX
; judge
COMPARE s0,CENTER
JUMP NZ,GO_BLACK1
; center
LOAD sA,15
LOAD sB,15
JUMP GO_BLACK7
; big left
GO_BLACK1:
COMPARE s0,BIG_LEFT
JUMP NZ,GO_BLACK2
; set duty
LOAD sA,05
LOAD sB,15
JUMP GO_BLACK7
; left
GO_BLACK2:
COMPARE s0,LEFT
JUMP NZ,GO_BLACK3
; set duty
LOAD sA,10
LOAD sB,15
JUMP GO_BLACK7
; tiny left
GO_BLACK3:
COMPARE s0,TINY_LEFT
JUMP NZ,GO_BLACK4
; set duty
LOAD sA,12
LOAD sB,15
JUMP GO_BLACK7
; big right
GO_BLACK4:
COMPARE s0,BIG_RIGHT
JUMP NZ,GO_BLACK5
; set duty
LOAD sA,15
LOAD sB,05
JUMP GO_BLACK7
; right
GO_BLACK5:
COMPARE s0,RIGHT
JUMP NZ,GO_BLACK6
; set duty
LOAD sA,15
LOAD sB,10
JUMP GO_BLACK7
; tiny right
GO_BLACK6:
COMPARE s0,TINY_RIGHT
JUMP NZ,GO_BLACK7
; set duty
LOAD sA,15
LOAD sB,12
;
GO_BLACK7:
CALL PUTS
JUMP GO_BLACK0
;
GO_BLACKX:
RETURN
;+++++++++++++++++++
; turn at ALL_BLACK
;+++++++++++++++++++
FIRST_TURN:
; ? RIGHT
COMPARE sE,01
JUMP Z,FIRST_TURN1
; ? LEFT
JUMP FIRST_TURN2
; set right
FIRST_TURN1:
LOAD sA,10
LOAD sB,15
JUMP FIRST_TURN3
; set left
FIRST_TURN2:
LOAD sA,15
LOAD sB,10
; impress
FIRST_TURN3:
CALL PUTS
; set counter
LOAD sC,10
; loop
FIRST_TURN4:
COMPARE sC,00
JUMP FIRST_TURNX
;
SUB sC,01
JUMP FIRST_TURN4
;
FIRST_TURNX:
RETURN
;++++++++++++++++++++
; move on BLACK area
;++++++++++++++++++++
GO_CENTER:
; set duty ratio
LOAD sA,15
LOAD sB,15
CALL PUTS
; loop
GO_CENTER0:
CALL GETS
; judge
COMPARE s0,CENTER
JUMP NZ,GO_CENTER0
; set duty ratio
LOAD sA,10
LOAD sB,10
CALL PUTS
;
RETURN
;++++++++++++++++
; turn at CENTER
;++++++++++++++++
SECOND_TURN:
; ? RIGHT
COMPARE sF,01
JUMP Z,SECOND_TURN1
; ? LEFT
JUMP SECOND_TURN2
; set right
SECOND_TURN1:
LOAD sA,10
LOAD sB,15
JUMP SECOND_TURN3
; set left
SECOND_TURN2:
LOAD sA,15
LOAD sB,10
; impress
SECOND_TURN3:
CALL PUTS
; set counter
LOAD sC,10
; loop
SECOND_TURN4:
COMPARE sC,00
JUMP SECOND_TURNX
;
SUB sC,01
JUMP SECOND_TURN4
;
SECOND_TURNX:
; judge direction
RETURN
MCR_VCの大会規定で、スタートから120秒以内にゴールに
到達していることが要求されています。
スタート後120秒をカウントし、システムをリセットする
マイクロシーケンサを定義します。
スタートトリガーは、MASTER PROCESSORに接続しているので
このトリガーを使い、120秒カウンタを動かします。
120秒カウンタのカウントアップフラグをチェックして
フラグセットになったなら、NORMAL、CRANK、LANEの各
プロセッサのクロックを停止し、リセットします。
MASTER PROCESSORは、常時動作し、システム全体の動きを
管理させます。
目次
前
次
