机上デバッグ
MCR-VCマシンの試走は、コースと場所が必要で
気軽に走らせるようにはなっていない。
自分の場合、物置の２Ｆが使えるので２５ｍ程度の
コースは常設である。しかし、北海道では冬期間に
物置の２Ｆで費やせる時間は長くない。
暖房を入れれば問題解決するが、節電や省資源の点
からストーブは焚きたくない。灯油を燃料にするコンロ
があるが、場所路をとるのでコースが短くなってしまう。
戸外は氷点下の気温でも、物置の２Ｆは室温が１０℃
前後なので、３０分程度ならばテスト走行やデバッグ
はできる。
そこで、走行だけに常設コースを使うことにして
なるべく自室でテスト、デバッグするように工夫
してみました。
マシンを載せる台を、健康食品の入っていた箱の
上にのせます。タイヤは自由に回転できるように
なっています。
タイヤの回転数がわかると、直進、右回転、左回転
を判断できるので、２チャネルの周波数カウンタを
用意しました。
左右のタイヤの回転数は、フォトインタラプタを
利用してＯＮ／ＯＦＦ信号に変換後、カウンタに
接続します。
周波数カウンタは、１秒あたりのパルス数をカウント
しているので、１秒あたりのタイヤ回転数を見れば
マシンのおおよその動きを判断できます。
MCR-VCマシンは、路面センサーからもたらされる情報を
活用して移動するので、センサー情報を出力する装置が
必要になります。
CPLDのCoolRunnerIIを利用し、１秒ごとに８ビットの
路面センサー情報を出力することにしました。
利用したVHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity gensen is
generic (
TOPX : integer := 10 ;
RMAX : integer := 921 ;
TOPY : integer := 10 ;
YMAX : integer := 500 --;
) ;
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- trigger input
SETRG : in std_logic ; -- start or exit flag
-- OUTPUT
SMON : out std_logic_vector(7 downto 0) ;
SOUT : out std_logic_vector(7 downto 0) ;
GLED : out std_logic ;
RLED : out std_logic ;
LED : out std_logic --;
);
end gensen;
architecture behavioral of gensen is
-- clock generator
component clkgenx is
generic (
TOPX : integer ;
RMAX : integer --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- output
CLKOUT : out std_logic -- ;
);
end component ;
-- sensor information generator
component senx is
port (
-- input
DIN : in std_logic_vector(3 downto 0) ;
-- output
JOUT : out std_logic_vector(7 downto 0) --;
);
end component ;
-- clock
signal iMCLK : std_logic ;
signal iPCLK : std_logic ;
-- parent sequencer
signal iPSTATE : std_logic_vector(1 downto 0) ;
signal iENABLE : std_logic ;
-- trigger
signal iSETRG : std_logic ;
signal iSETRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG_R : std_logic ;
-- counter
signal iCNT : std_logic_vector(3 downto 0) ;
signal iJOUT : std_logic_vector(7 downto 0) ;
begin
-- clock
CLKX : clkgenx generic map (TOPX,RMAX) port map (nRESET,CLOCK,iMCLK) ;
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,iMCLK,iPCLK) ;
-- judge
SENOUT : senx port map (iCNT,iJOUT) ;
-- input
iSETRG <= not SETRG ;
-- output
SOUT <= iJOUT ;
SMON <= not iJOUT ;
LED <= iPCLK ;
GLED <= iPCLK ;
RLED <= not iPCLK ;
-- trigger sequencer
process (nRESET,iMCLK)
begin
if ( nRESET = '0' ) then
iSETRG_SFT <= "000" ;
elsif rising_edge(iMCLK) then
iSETRG_SFT <= iSETRG_SFT(1 downto 0) & iSETRG ;
end if ;
end process ;
iTRG_R <= '1' when ( iSETRG_SFT = "011" or iSETRG_SFT = "001" ) else '0' ;
-- parent sequencer
process (nRESET,iMCLK)
begin
if ( nRESET = '0' ) then
iPSTATE <= "00" ;
elsif rising_edge(iMCLK) then
case conv_integer(iPSTATE) is
-- wait RFLAG rising edge
when 0 => if ( iTRG_R = '1' ) then
iPSTATE <= "01" ;
else
iPSTATE <= "00" ;
end if ;
-- send blind run trigger
when 1 => iPSTATE <= "11" ;
-- wait blind run flag
when 3 => if ( iTRG_R= '1' ) then
iPSTATE <= "10" ;
else
iPSTATE <= "11" ;
end if ;
-- return first state
when 2 => iPSTATE <= "00" ;
-- default
when others =>
iPSTATE <= "00" ;
end case ;
end if ;
end process ;
iENABLE <= iPSTATE(0) ;
-- trigger sequencer
process (nRESET,iPCLK)
begin
if ( nRESET = '0' ) then
iCNT <= "0000" ;
elsif rising_edge(iPCLK) then
if ( iENABLE = '1' ) then
iCNT <= iCNT + '1' ;
end if ;
end if ;
end process ;
end behavioral;
内容は単純で、トリガーを受けたら、４ビットの
バイナリカウンタをインクリメントします。
バイナリカウンタの出力（０〜１５）を入力すると
対応した８ビットのデータを出力します。
８ビットデータ出力は、別ファイルで定義しています。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity senx is
port (
-- input
DIN : in std_logic_vector(3 downto 0) ;
-- output
JOUT : out std_logic_vector(7 downto 0) --;
);
end senx;
architecture Behavioral of senx is
signal iJOUT : std_logic_vector(7 downto 0);
begin
-- output
JOUT <= iJOUT ;
-- convert
iJOUT <= X"18" when ( DIN = X"0" ) else -- center
X"38" when ( DIN = X"1" ) else -- center
X"1C" when ( DIN = X"2" ) else -- center
X"0C" when ( DIN = X"3" ) else -- tiny_right
X"0C" when ( DIN = X"4" ) else -- tiny_right
X"18" when ( DIN = X"5" ) else -- center
X"30" when ( DIN = X"6" ) else -- tiny_left
X"30" when ( DIN = X"7" ) else -- tiny_left
X"60" when ( DIN = X"8" ) else -- left
X"18" when ( DIN = X"9" ) else -- center
X"06" when ( DIN = X"A" ) else -- right
X"1C" when ( DIN = X"B" ) else -- center
X"60" when ( DIN = X"C" ) else -- left
X"38" when ( DIN = X"D" ) else -- center
X"18" when ( DIN = X"E" ) else -- center
X"18" ;
end Behavioral;
出力パターンを変更したい場合、上のVHDLコードで
記述しているデコードテーブルを変えます。
16パターンで足りないときには、バイナリカウンタを
５ビット、６ビット、７ビットとし、デコード内容を
増やします。
メインとなるVHDLコードには、出力しているセンサー情報を
LEDに出力して、目視できるようにしてあります。
JICA研修で利用したMCR制御基板を使い、CPLDのVHDLコード
が設計通りに動いているのかを確認します。
CPLDが出力するセンサー情報を10ピンケーブルでFPGAに
接続します。
MCR-VCマシンの移動には、FPGA内部のシーケンサで
センサー情報を判断し、左右のモータ回転数を制御
します。
このシーケンサが、CPLDが出力する情報を利用する
ので、情報を確実に受取ったかをモニタします。
もう１枚のMCR制御基板を使い、入力したセンサー情報を
表示します。センサー情報を正しく受けとって、はじめて
コース上を移動できるので、センサー情報をモニタする
ことは、デバッグの基本となります。
センサー情報モニタを実現するのは、簡単で次のVHDLコード
を入れるだけです。
-- input
iJSENSOR <= SENSOR ;
-- output
SOUT <= not iJSENSOR ;
正論理で判断できるように、出力を反転しています。
MCR制御基板のLEDにはインバータがないので、この
操作を入れます。
PLDでは、必要なデジタル回路の着脱はデバイスを
半田付けする必要がないので、楽です。
MCR制御基板上に、スタート／ゴールプッシュスイッチを
実装しているので、ゲートセンサーが反応しない場合には
このスイッチを押すと、マシンが動き出します。
FPGA内部のシーケンサは、以下の４モードのいずれかに
なるようにしてあります。
- BLIND_RUN
- NORMAL
- CRANK
- LANE_CHANGE
スタート／ゴールプッシュスイッチを押してから
どのモードにいるのかを確認できるよう、16個の
LEDを利用して、状態を表示させます。
FPGAから、モードと状態の情報をMCR制御基板上の
LEDに出力します。
モードは、以下のように状態に分割しています。
- BLIND_RUN 1 -> 9
- NORMAL 10 -> 13
- CRANK 20 -> 33
- LANE_CHANGE 40 -> 54
モードと状態の組合せで、どのモードのどんな処理を
実行しているのかがわかるようにします。
処理は、８ビットで表現しました。
STATUS <= not iSTATUS ;
iSTATUS <= "10000000" when ( iSTATE = 1 ) else -- blind run set duty 10%
"10000000" when ( iSTATE = 2 ) else -- blind run delay duty 10%
"01000000" when ( iSTATE = 3 ) else -- blind run set duty 25%
"01000000" when ( iSTATE = 4 ) else -- blind run delay duty 25%
"00100000" when ( iSTATE = 5 ) else -- blind run set duty 35%
"00100000" when ( iSTATE = 6 ) else -- blind run delay duty 35%
"00010000" when ( iSTATE = 7 ) else -- blind run set duty 50%
"00010000" when ( iSTATE = 8 ) else -- blind run delay duty 50%
"00000001" when ( iSTATE = 10 ) else -- NORMAL get sensor data
"00000010" when ( iSTATE = 11 ) else -- NORMAL judge
"00000100" when ( iSTATE = 12 ) else -- NORMAL delay
"00000001" when ( iSTATE = 13 ) else -- NORMAL return first state
"00000001" when ( iSTATE = 20 ) else -- CRANK slow move
"00000001" when ( iSTATE = 21 ) else -- CRANK slow move (delay)
"00000010" when ( iSTATE = 22 ) else -- CRANK get road states
"00000100" when ( iSTATE = 23 ) else -- CRANK judge
"00001000" when ( iSTATE = 24 ) else -- CRANK turn
"00010000" when ( iSTATE = 25 ) else -- CRANK turn
"00010000" when ( iSTATE = 26 ) else -- CRANK turn (delay)
"00100000" when ( iSTATE = 27 ) else -- CRANK judge center
"00100000" when ( iSTATE = 28 ) else -- CRANK judge center
"01000000" when ( iSTATE = 29 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 30 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 31 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 32 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 33 ) else -- CRANK complete turn
"00000001" when ( iSTATE = 40 ) else -- LANE slow move
"00000001" when ( iSTATE = 41 ) else -- LANE slow move (delay)
"00000010" when ( iSTATE = 42 ) else -- LANE get road state
"00000010" when ( iSTATE = 43 ) else -- LANE get road state (delay)
"00000100" when ( iSTATE = 44 ) else -- LANE turn
"00000100" when ( iSTATE = 45 ) else -- LANE turn (delay)
"00001000" when ( iSTATE = 46 ) else -- LANE center move
"00001000" when ( iSTATE = 47 ) else -- LANE center move (delay)
"00010000" when ( iSTATE = 48 ) else -- LANE get road state
"00010000" when ( iSTATE = 49 ) else -- LANE get road state (delay)
"00100000" when ( iSTATE = 50 ) else -- LANE turn
"00100000" when ( iSTATE = 51 ) else -- LANE turn (delay)
"01000000" when ( iSTATE = 52 ) else -- LANE center move
"01000000" when ( iSTATE = 53 ) else -- LANE center move (delay)
"10000000" when ( iSTATE = 54 ) else -- LANE return NORMAL
"00000000" ;
モードは、４ビットで表示します。
MODEX <= not iMODEX ;
iMODE <= 0 when ( iSTATE < 10 ) else -- BLIND_RUN
1 when ( 9 < iSTATE and iSTATE < 14 ) else -- NORMAL
2 when ( 19 < iSTATE and iSTATE < 40 ) else -- CRANK
3 ;
iMODEX <= "0001" when ( iMODE = 0 ) else -- BLIND_RUN
"0010" when ( iMODE = 1 ) else -- NORMAL
"0100" when ( iMODE = 2 ) else -- CRANK
"1000" when ( iMODE = 3 ) else -- LANE_CHANGE
"0000" ;
モードと状態の組合せをLEDに出力する処理は、実走行には
関係ありません。テスト、デバッグのときだけに必要になり
ます。テスト、デバッグが必要なくなったときは、10ピン
ケーブルをFPGA基板から外すだけです。
マイコン利用では、プログラム実行のため、テスト、デバッグ
用コードは、処理を遅くすることになりますが、PLDを使うと
並列動作なので、処理が遅くなることはありません。
この机上デバッグで、次の３つのバグを発見できました。
机上デバッグが終わった時点のFPGAのVHDLコードは
次のようになりました。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity mcrvcx0 is
generic (
TOPX : integer := 2 ;
RMAX : integer := 3 ;
TOPY : integer := 3 ;
YMAX : integer := 4 ;
TOPZ : integer := 9 ;
ZMAX : integer := 500 --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- sensor input
SENSOR : in std_logic_vector(7 downto 0) ;
-- sensor output
SOUT : out std_logic_vector(7 downto 0) ;
-- PWM , trigger and direciton
POUTR : out std_logic ; -- right motor duty ratio
POUTL : out std_logic ; -- left motor duty ratio
SGTRG : in std_logic ; -- start goal trigger
GLED : out std_logic ; -- run
RLED : out std_logic ; -- cross white line
-- output
MODEX : out std_logic_vector(3 downto 0) ;
LDIR : out std_logic ;
RDIR : out std_logic ;
STATUS : out std_logic_vector(7 downto 0) --;
);
end mcrvcx0;
architecture Behavioral of mcrvcx0 is
-- component clock generator
component clkgenx is
generic (
TOPX : integer ;
RMAX : integer --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- output
CLKOUT : out std_logic -- ;
);
end component ;
-- judge
component judge is
port (
-- input
DIN : in std_logic_vector(7 downto 0) ;
-- output
JOUT : out std_logic_vector(3 downto 0) --;
);
end component ;
-- component PWM
component pwmx is
port(
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
PRATER : in std_logic_vector(5 downto 0) ;
PRATEL : in std_logic_vector(5 downto 0) ;
-- output
POUTR : out std_logic ;
POUTL : out std_logic --;
);
end component ;
-- clock
signal iMCLK : std_logic ;
signal iPCLK : std_logic ;
signal iPWCLK : std_logic ;
-- divider
signal iDCLK : std_logic ;
signal iDCNT : integer range 0 to 50 ;
-- trigger
signal iSGTRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG_R : std_logic ;
-- sensor
signal iSENSOR : std_logic_vector(7 downto 0) ;
-- master sequencer
signal iSTATE : integer range 0 to 54;
signal iDIRECTION : std_logic_vector(1 downto 0) ;
-- judge run sequencer
signal iJSENSOR : std_logic_vector(7 downto 0) ;
signal iJOUT : std_logic_vector(3 downto 0) ;
signal iMODE : integer range 0 to 3 ;
signal iMODEX : std_logic_vector(3 downto 0) ;
signal iJCNT : integer range 0 to 60 ;
--
signal iSTATUS : std_logic_vector(7 downto 0) ;
-- PWM
signal iPRATER : std_logic_vector(5 downto 0) ;
signal iPRATEL : std_logic_vector(5 downto 0) ;
signal iPOUTR : std_logic ;
signal iPOUTL : std_logic ;
signal iGLED : std_logic ;
signal iRLED : std_logic ;
begin
-- component clock generator (48MHz/6 = 8MHz)
CLKX : clkgenx generic map (TOPX,RMAX) port map (nRESET,CLOCK,iMCLK);
-- component clock generator ( 8MHz/8 = 1MHz)
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,iMCLK,iPCLK);
-- component clock generator ( 1MHz/1000 = 1kHz)
CLKZ : clkgenx generic map (TOPZ,ZMAX) port map (nRESET,iPCLK,iPWCLK);
-- judge
JUDGEX : judge port map (iSENSOR,iJOUT) ;
-- component PWM
PWM : pwmx port map (nRESET,iPWCLK,iPRATER,iPRATEL,iPOUTR,iPOUTL);
-- input
iJSENSOR <= SENSOR ;
-- output
SOUT <= not iJSENSOR ;
POUTR <= iPOUTR ;
POUTL <= iPOUTL ;
GLED <= not iGLED ;
RLED <= not iRLED ;
MODEX <= not iMODEX ;
LDIR <= not iDIRECTION(1) ;
RDIR <= not iDIRECTION(0) ;
-- monitor
STATUS <= not iSTATUS ;
iGLED <= '1' when ( iSTATE > 0 ) else '0' ;
iRLED <= '1' when ( iJSENSOR = X"FF" ) else '0' ;
-- internal status
iSTATUS <= "10000000" when ( iSTATE = 1 ) else -- blind run set duty 10%
"10000000" when ( iSTATE = 2 ) else -- blind run delay duty 10%
"01000000" when ( iSTATE = 3 ) else -- blind run set duty 25%
"01000000" when ( iSTATE = 4 ) else -- blind run delay duty 25%
"00100000" when ( iSTATE = 5 ) else -- blind run set duty 35%
"00100000" when ( iSTATE = 6 ) else -- blind run delay duty 35%
"00010000" when ( iSTATE = 7 ) else -- blind run set duty 50%
"00010000" when ( iSTATE = 8 ) else -- blind run delay duty 50%
"00000001" when ( iSTATE = 10 ) else -- NORMAL get sensor data
"00000010" when ( iSTATE = 11 ) else -- NORMAL judge
"00000100" when ( iSTATE = 12 ) else -- NORMAL delay
"00000001" when ( iSTATE = 13 ) else -- NORMAL return first state
"00000001" when ( iSTATE = 20 ) else -- CRANK slow move
"00000001" when ( iSTATE = 21 ) else -- CRANK slow move (delay)
"00000010" when ( iSTATE = 22 ) else -- CRANK get road states
"00000100" when ( iSTATE = 23 ) else -- CRANK judge
"00001000" when ( iSTATE = 24 ) else -- CRANK turn
"00010000" when ( iSTATE = 25 ) else -- CRANK turn
"00010000" when ( iSTATE = 26 ) else -- CRANK turn (delay)
"00100000" when ( iSTATE = 27 ) else -- CRANK judge center
"00100000" when ( iSTATE = 28 ) else -- CRANK judge center
"01000000" when ( iSTATE = 29 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 30 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 31 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 32 ) else -- CRANK complete turn
"10000000" when ( iSTATE = 33 ) else -- CRANK complete turn
"00000001" when ( iSTATE = 40 ) else -- LANE slow move
"00000001" when ( iSTATE = 41 ) else -- LANE slow move (delay)
"00000010" when ( iSTATE = 42 ) else -- LANE get road state
"00000010" when ( iSTATE = 43 ) else -- LANE get road state (delay)
"00000100" when ( iSTATE = 44 ) else -- LANE turn
"00000100" when ( iSTATE = 45 ) else -- LANE turn (delay)
"00001000" when ( iSTATE = 46 ) else -- LANE center move
"00001000" when ( iSTATE = 47 ) else -- LANE center move (delay)
"00010000" when ( iSTATE = 48 ) else -- LANE get road state
"00010000" when ( iSTATE = 49 ) else -- LANE get road state (delay)
"00100000" when ( iSTATE = 50 ) else -- LANE turn
"00100000" when ( iSTATE = 51 ) else -- LANE turn (delay)
"01000000" when ( iSTATE = 52 ) else -- LANE center move
"01000000" when ( iSTATE = 53 ) else -- LANE center move (delay)
"10000000" when ( iSTATE = 54 ) else -- LANE return NORMAL
"00000000" ;
-- divider 1kHz / 100 = 10Hz
process (nRESET,iPWCLK)
begin
if ( nRESET = '0' ) then
iDCNT <= 0 ;
iDCLK <= '0' ;
elsif rising_edge(iPWCLK) then
if ( iDCNT = 50 ) then
iDCNT <= 0 ;
iDCLK <= not iDCLK ;
else
iDCNT <= iDCNT + 1 ;
end if ;
end if ;
end process ;
-- trigger
process (nRESET,iDCLK)
begin
if ( nRESET = '0' ) then
iSGTRG_SFT <= "000" ;
elsif rising_edge(iDCLK) then
iSGTRG_SFT <= iSGTRG_SFT(1 downto 0) & (not SGTRG) ;
end if ;
end process ;
iTRG_R <= '1' when ( iSGTRG_SFT = "011" or iSGTRG_SFT = "001" ) else '0' ;
-- master sequencer
process (nRESET,iDCLK)
begin
if ( nRESET = '0' ) then
iSTATE <= 0 ;
iDIRECTION <= "00" ;
elsif rising_edge(iDCLK) then
case conv_integer(iSTATE) is
-- wait RFLAG rising edge
when 0 => if ( iTRG_R = '1' ) then
iSTATE <= 1 ;
end if ;
-- send blind run trigger (10%)
when 1 => iSTATE <= 2 ;
iPRATER <= conv_std_logic_vector(10,6) ;
iPRATEL <= conv_std_logic_vector(10,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
-- delay blind run (10%)
when 2 => if ( iJCNT = 0 ) then
iSTATE <= 3 ;
else
iJCNT <= iJCNT - 1 ;
iSTATE <= 2 ;
end if ;
-- send blind run trigger (25%)
when 3 => iSTATE <= 4 ;
iPRATER <= conv_std_logic_vector(25,6) ;
iPRATEL <= conv_std_logic_vector(25,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
-- delay blind run (25%)
when 4 => if ( iJCNT = 0 ) then
iSTATE <= 5 ;
else
iJCNT <= iJCNT - 1 ;
iSTATE <= 4 ;
end if ;
-- send blind run trigger (35%)
when 5 => iSTATE <= 6 ;
iPRATER <= conv_std_logic_vector(35,6) ;
iPRATEL <= conv_std_logic_vector(35,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
-- delay blind run (35%)
when 6 => if ( iJCNT = 0 ) then
iSTATE <= 7 ;
else
iJCNT <= iJCNT - 1 ;
iSTATE <= 6 ;
end if ;
-- send blind run trigger (50%)
when 7 => iSTATE <= 8 ;
iPRATER <= conv_std_logic_vector(50,6) ;
iPRATEL <= conv_std_logic_vector(50,6) ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
-- delay blind run (50%)
when 8 => if ( iJCNT = 0 ) then
iSTATE <= 9 ;
else
iJCNT <= iJCNT - 1 ;
iSTATE <= 8 ;
end if ;
-- branch NORMAL run
when 9 => iSTATE <= 10 ;
-- NORMAL (get sensor data)
when 10 => iSENSOR <= iJSENSOR ;
iSTATE <= 11 ;
-- NORMAL (judge)
when 11 => if ( iJCNT = 10 ) then -- all_white
iSTATE <= 20 ;
elsif ( iJCNT = 8 ) then -- left_white
iDIRECTION <= "10" ; -- left
iSTATE <= 40 ;
elsif ( iJCNT = 9 ) then -- right_white
iDIRECTION <= "01" ; -- right
iSTATE <= 40 ;
elsif ( iJCNT = 4 ) then -- center
iPRATER <= conv_std_logic_vector(35,6) ;
iPRATEL <= conv_std_logic_vector(35,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
iSTATE <= 12 ;
elsif ( iJCNT = 1 ) then -- tiny_right
iPRATER <= conv_std_logic_vector(35,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
iJCNT <= 5 ; -- 0.5sec 10Hz = 100ms
iSTATE <= 12 ;
elsif ( iJCNT = 2 ) then -- right
iPRATER <= conv_std_logic_vector(35,6) ;
iPRATEL <= conv_std_logic_vector( 5,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
iSTATE <= 12 ;
elsif ( iJCNT = 3 ) then -- big_right
iPRATER <= conv_std_logic_vector(45,6) ;
iPRATEL <= conv_std_logic_vector( 5,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
iSTATE <= 12 ;
elsif ( iJCNT = 5 ) then -- tiny_left
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(35,6) ;
iJCNT <= 5 ; -- 0.5sec 10Hz = 100ms
iSTATE <= 12 ;
elsif ( iJCNT = 6 ) then -- left
iPRATER <= conv_std_logic_vector( 5,6) ;
iPRATEL <= conv_std_logic_vector(35,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
iSTATE <= 12 ;
elsif ( iJCNT = 7 ) then -- big_left
iPRATER <= conv_std_logic_vector( 5,6) ;
iPRATEL <= conv_std_logic_vector(45,6) ;
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
iSTATE <= 12 ;
else
iDIRECTION <= "00" ; -- center
iJCNT <= 10 ; -- 1sec 10Hz = 100ms
iSTATE <= 12 ;
end if ;
-- NORMAL (delay)
when 12 => if ( iJCNT = 0 ) then
iSTATE <= 13 ;
else
iJCNT <= iJCNT - 1 ;
iSTATE <= 12 ;
end if ;
-- NORMAL (return first state)
when 13 => if ( iTRG_R = '1' ) then
iSTATE <= 0 ;
else
iSTATE <= 10 ;
end if ;
-- CRANK (slow move)
when 20 => iSTATE <= 21 ;
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
iJCNT <= 30 ; -- 3sec 10Hz = 100ms
-- CRANK (delay)
when 21 => if ( iJCNT = 0 ) then
iSTATE <= 22 ;
else
iJCNT <= iJCNT - 1 ;
iSTATE <= 21 ;
end if ;
-- CRANK (get road information)
when 22 => iSENSOR <= iJSENSOR ;
iDIRECTION <= "00" ; -- center
iSTATE <= 23 ;
-- CRANK (judge)
when 23 => -- right_white
if ( iJOUT = 8 ) then
iSTATE <= 25 ;
iDIRECTION <= "01" ; -- right
-- left_white
elsif ( iJOUT = 9 ) then
iSTATE <= 25 ;
iDIRECTION <= "10" ; -- left
else
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
iJCNT <= 1 ; -- 1sec 10Hz = 100ms
iSTATE <= 24 ;
end if ;
-- CRANK (delay)
when 24 => if ( iJCNT = 0 ) then
iSTATE <= 22 ;
else
iSTATE <= 24 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- CRANK (turn)
when 25 => iSTATE <= 26 ;
iJCNT <= 2 ; -- 2sec 10Hz = 100ms
-- right_turn
if ( iDIRECTION = "01" ) then
iPRATER <= conv_std_logic_vector( 0,6) ;
iPRATEL <= conv_std_logic_vector(30,6) ;
-- left_white
elsif ( iDIRECTION = "10" ) then
iPRATER <= conv_std_logic_vector(30,6) ;
iPRATEL <= conv_std_logic_vector( 0,6) ;
end if ;
-- CRANK (turn delay)
when 26 => if ( iJCNT = 0 ) then
iSTATE <= 27 ;
else
iSTATE <= 26 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- CRANK (get road information)
when 27 => iSENSOR <= iJSENSOR ;
iSTATE <= 28 ;
-- CRANK (judge center)
when 28 => -- center
if ( iJOUT = 4 ) then
iSTATE <= 29 ;
else
iJCNT <= 1 ; -- 1sec 10Hz = 100ms
iSTATE <= 28 ;
end if ;
-- CRANK (turn delay center)
when 29 => if ( iJCNT = 0 ) then
iSTATE <= 30 ;
else
iSTATE <= 29 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- CRANK (complete turn)
when 30 =>
iSTATE <= 31 ;
iJCNT <= 2 ; -- 2sec 10Hz = 100ms
-- right_turn
if ( iDIRECTION = "01" ) then
iPRATER <= conv_std_logic_vector(30,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
-- left_white
elsif ( iDIRECTION = "10" ) then
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(30,6) ;
end if ;
-- CRANK (delay exit turn)
when 31 => if ( iJCNT = 0 ) then
iSTATE <= 32 ;
else
iSTATE <= 31 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- CRANK (center move)
when 32 => iSTATE <= 33 ;
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
-- CRANK (delay center move)
when 33 => if ( iJCNT = 0 ) then
iSTATE <= 34 ;
else
iSTATE <= 33 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- return NORMAL run
when 34 => iSTATE <= 10 ;
-- LANE (blind run)
when 40 => iSTATE <= 41 ;
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
-- LANE (blind run delay)
when 41 => if ( iJCNT = 0 ) then
iSTATE <= 42 ;
else
iSTATE <= 41 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- LANE (get sensor data)
when 42 => iSENSOR <= iJSENSOR ;
iSTATE <= 43 ;
-- LANE (judge all black)
when 43 => if ( iJOUT = 0 ) then
iSTATE <= 44 ;
else
iSTATE <= 42 ;
end if ;
-- LANE (turn)
when 44 => iSTATE <= 45 ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
-- right_turn
if ( iDIRECTION = "01" ) then
iPRATER <= conv_std_logic_vector( 0,6) ;
iPRATEL <= conv_std_logic_vector(30,6) ;
-- left_white
elsif ( iDIRECTION = "10" ) then
iPRATER <= conv_std_logic_vector(30,6) ;
iPRATEL <= conv_std_logic_vector( 0,6) ;
end if ;
-- LANE (turn delay)
when 45 => if ( iJCNT = 0 ) then
iSTATE <= 46 ;
else
iSTATE <= 45 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- LANE (center move)
when 46 => iSTATE <= 47 ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
-- LANE (turn delay)
when 47 => if ( iJCNT = 0 ) then
iSTATE <= 48 ;
else
iSTATE <= 47 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- LANE (get sensor data)
when 48 => iSENSOR <= iJSENSOR ;
iSTATE <= 49 ;
-- LANE (judge center)
when 49 => if ( iJOUT = 4 ) then
iSTATE <= 50 ;
else
iSTATE <= 48 ;
end if ;
-- LANE (turn)
when 50 => iSTATE <= 51 ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
-- right_turn
if ( iDIRECTION = 1 ) then
iPRATER <= conv_std_logic_vector(30,6) ;
iPRATEL <= conv_std_logic_vector( 0,6) ;
-- left_white
elsif ( iDIRECTION = 2 ) then
iPRATER <= conv_std_logic_vector( 0,6) ;
iPRATEL <= conv_std_logic_vector(30,6) ;
end if ;
-- LANE (turn delay)
when 51 => if ( iJCNT = 0 ) then
iSTATE <= 52 ;
else
iSTATE <= 51 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- LANE (center move)
when 52 => iSTATE <= 53 ;
iJCNT <= 20 ; -- 2sec 10Hz = 100ms
iPRATER <= conv_std_logic_vector(15,6) ;
iPRATEL <= conv_std_logic_vector(15,6) ;
-- LANE (turn delay)
when 53 => if ( iJCNT = 0 ) then
iSTATE <= 54 ;
else
iSTATE <= 53 ;
iJCNT <= iJCNT - 1 ;
end if ;
-- return NORMAL run
when 54 => iSTATE <= 10 ;
iDIRECTION <= "00" ; -- center
-- default
when others =>
iSTATE <= 0 ;
end case ;
end if ;
end process ;
iMODE <= 0 when ( iSTATE < 10 ) else -- BLIND_RUN
1 when ( 9 < iSTATE and iSTATE < 14 ) else -- NORMAL
2 when ( 19 < iSTATE and iSTATE < 40 ) else -- CRANK
3 ;
iMODEX <= "0001" when ( iMODE = 0 ) else -- BLIND_RUN
"0010" when ( iMODE = 1 ) else -- NORMAL
"0100" when ( iMODE = 2 ) else -- CRANK
"1000" when ( iMODE = 3 ) else -- LANE_CHANGE
"0000" ;
end Behavioral;
コース情報を、実際の路面から拾い出すのは面倒
なので、乱数を利用し、大まかな内容を生成する
スクリプトを記述しました。
for {set i 0} {$i < 32} {incr i} {
# generate
set ans [lindex [split [expr rand()*17] "."] 0]
# 2 columns
set ans [format "%2d" $ans]
# generate strings
set tmp ""
set sdat "0x00"
if { $ans == 0 } {
set tmp "all_black"
}
if { $ans == 1 } {
set tmp "tiny_right"
set sdat "0x0C"
}
if { $ans == 2 } {
set tmp "right"
set sdat "0x06"
}
if { $ans == 3 } {
set tmp "big_right"
set sdat "0x03"
}
if { $ans == 4 } {
set tmp "center0"
set sdat "0x18"
}
if { $ans == 5 } {
set tmp "center1"
set sdat "0x1C"
}
if { $ans == 6 } {
set tmp "center2"
set sdat "0x38"
}
if { $ans == 7 } {
set tmp "tiny_left"
set sdat "0x30"
}
if { $ans == 8 } {
set tmp "left"
set sdat "0x60"
}
if { $ans == 9 } {
set tmp "big_left"
set sdat "0xC0"
}
if { $ans == 10 } {
set tmp "left_white0"
set sdat "0xE0"
}
if { $ans == 11 } {
set tmp "left_white1"
set sdat "0xF0"
}
if { $ans == 12 } {
set tmp "left_white2"
set sdat "0xF8"
}
if { $ans == 13 } {
set tmp "right_white0"
set sdat "0x07"
}
if { $ans == 14 } {
set tmp "right_white1"
set sdat "0x0F"
}
if { $ans == 15 } {
set tmp "right_white2"
set sdat "0x1F"
}
if { $ans == 16 } {
set tmp "all_white"
set sdat "0xFF"
}
puts "$ans $sdat $tmp"
}
センサー情報は、８ビットデータで生成するので
乱数により０〜１６の整数、文字列、16進数の
データに変換します。
テキストファイルに出力すると、次のようになります。
9 0xC0 big_left
3 0x03 big_right
10 0xE0 left_white0
9 0xC0 big_left
12 0xF8 left_white2
15 0x1F right_white2
3 0x03 big_right
8 0x60 left
3 0x03 big_right
12 0xF8 left_white2
15 0x1F right_white2
14 0x0F right_white1
6 0x38 center2
2 0x06 right
16 0xFF all_white
14 0x0F right_white1
12 0xF8 left_white2
16 0xFF all_white
13 0x07 right_white0
10 0xE0 left_white0
10 0xE0 left_white0
3 0x03 big_right
2 0x06 right
6 0x38 center2
8 0x60 left
0 0x00 all_black
3 0x03 big_right
1 0x0C tiny_right
15 0x1F right_white2
5 0x1C center1
9 0xC0 big_left
11 0xF0 left_white1
この内容を、実際のコースに合わせて、矛盾の
ない並びに変換して、VHDLコードの中に埋込み
デバッグに利用します。
目次
前
次