目次
前
次
画像処理
FIFOなしカメラからデータを入力し、5ライン分の
センサーデータを生成する回路をFPGA内部に入れる
ことにしました。
ブロック図は、以下です。
ブロック図から、FPGAに入れる内容は
以下としました。
- カメラインタフェース
- 画像データ保存
- センサーデータ生成
- バスインタフェース
順を追って、処理内容を説明します。
カメラインタフェース
カメラインタフェースは、SCCBにより
動作パラメータを送ります。
動作パラメータ変更の度に、VHDLコードを
コンパイルするのは、面倒なのでマイコン
から出力する信号をそのままトスします。
VHDLコードは、単純に記述しました。
signal iSCK : std_logic ;
signal iSDA : std_logic ;
-- input
iSCK <= ARM_SCCB_SCK ;
iSDA <= ARM_SCCB_SDA ;
-- output
SCK <= iSCK ;
SDA <= iSDA ;
利用するカメラは、RGB形式、YUV形式で画像
データを出力できるので、輝度の情報だけを
出力するように設定します。
画像データ保存
画像データは、マシン移動に必要となる
5ライン分を保存します。
カメラからQQVGA(160x120)のカラー画像を取得しますが
マシンを動かすためには、すべてのラインデータは不要
なので、上の60ラインから5ラインを選び保存します。
マイコンからの指示で、内部シーケンサを動かし
デュアルポートメモリに160ピクセルx5ラインを
保存します。
動作シーケンスは、以下のようにしました。
- マイコンからのトリガー待ち
- VSYNCのraising_edge待ち
- HREF=HのときPCKに同期して、Y(輝度)の8ビットをバッファに保存
- HREF=HのときPCKに同期して、出力される8ビットをスルー
- 指定ラインならば、輝度データをSRAMに保存
- 3、4、5を160回繰返す
- 指定の5ラインを保存したら1に戻る
今回利用するFPGAには、DualPortのSRAMが
用意されていますが、他FPGAでも利用可能
とするため、SRAMを定義します。
type WORD is array (0 to 159) of std_logic_vector(7 downto 0) ;
signal iMEMORY : WORD ;
signal iMEMORYX : WORD ;
カメラから出力される画像データを
iMEMORYで受取り、次のiMEMORYXに
転送します。
目的ラインでないときに、iMEMORYXに
格納されている1ライン分データから
センサーデータを生成します。
この方式では、ターゲットラインでない
ときに、別の回路でデータ処理するので
無駄がありません。
画像データを保存するVHDLコードは、以下としました。
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iCSTATE <= "00000" ;
iLCNT <= 0 ;
iPCNT <= 0 ;
iYDAT <= "00000000" ;
for i in 0 to 159 loop
iMEMORY(i) <= "00000000" ;
end loop ;
elsif rising_edge(CLOCK) then
case conv_integer(iCSTATE) is
-- wait trigger from micom
when 0 => if ( iCTRG = '1' ) then
iCSTATE <= "00001" ; -- state : 1
else
iCSTATE <= "00000" ; -- state : 0
end if ;
-- wait VSYNC trigger
when 1 => if ( iVSTRG = '1' ) then
iCSTATE <= "00011" ; -- state : 3
iLCNT <= 0 ;
iYDAT <= "00000000" ;
else
iCSTATE <= "00001" ; -- state : 1
end if ;
-- judge line counter
when 3 => if ( iLCNT = 60 ) then
iCSTATE <= "10000" ; -- state : 16
else
iCSTATE <= "00111" ; -- state : 7
iPCNT <= 0 ;
end if ;
-- judge pixel counter
when 7 => if ( iPCNT = 160 ) then
iCSTATE <= "11000" ; -- state : 24
iPCNT <= 0 ;
iLCNT <= iLCNT + 1 ;
else
iCSTATE <= "01111" ; -- state : 15
end if ;
-- get upper byte (Y)
when 15 => if ( iPTRG = '1' ) then
iCSTATE <= "11111" ; -- state : 31
iYDAT <= CDAT ;
else
iCSTATE <= "01111" ; -- state : 15
end if ;
-- get lower byte but through (U or V) and store
when 31 => if ( iPTRG = '1' ) then
iCSTATE <= "11110" ; -- state : 30
-- store
if ( iSAVEFLG = '1' ) then
iMEMORY(iPCNT) <= iYDAT ;
end if ;
else
iCSTATE <= "11111" ; -- state : 31
end if ;
-- pointer increment
when 30 => iCSTATE <= "11100" ; -- state : 28
iPCNT <= iPCNT + 1 ;
-- return camera data getting
when 28 => iCSTATE <= "00111" ; -- state : 7
-- ? compelete line handling and put trigger
when 24 => iCSTATE <= "00011" ; -- state : 3
-- return first state
when 16 => iCSTATE <= "00000" ; -- state : 0
-- default
when others =>
iCSTATE <= "00000" ;
end case ;
end if ;
end process ;
輝度データは、PCLKに同期して出力されるので
HREFとの論理積で、iPCLKを生成します。
iPCLKをFPGA内部シーケンサと同期化するため
シフトレジスタを利用して、rasing_edgeを
トリガー同期のiPTRGに変換します。
iPCLK <= HREF and PCLK ;
process (nRESET,iMCLK)
begin
if ( nRESET = '0' ) then
iPCLK_SFT <= "000" ;
elsif rising_edge(iMCLK) then
iPCLK_SFT <= iPCLK_SFT(1 downto 0) & iPCLK ;
end if ;
end process ;
iPTRG <= '1' when ( iPCLK_SFT = "011" or iPCLK_SFT = "001" ) else '0' ;
保存するラインを後で変更できるように
フラグをセットして使っています。
iSAVEFLG <= '1' when ( iLCNT = 10 ) else
'1' when ( iLCNT = 20 ) else
'1' when ( iLCNT = 30 ) else
'1' when ( iLCNT = 40 ) else
'1' when ( iLCNT = 50 ) else
'0' ;
目的ラインの画像データからセンサーデータ
生成のトリガーは、次のように指定します。
iBTRG <= '1' when ( iLCNT = 10 and conv_integer(iCSTATE) = 28 ) else
'1' when ( iLCNT = 20 and conv_integer(iCSTATE) = 28 ) else
'1' when ( iLCNT = 30 and conv_integer(iCSTATE) = 28 ) else
'1' when ( iLCNT = 40 and conv_integer(iCSTATE) = 28 ) else
'1' when ( iLCNT = 50 and conv_integer(iCSTATE) = 28 ) else
'0' ;
画像データ保存のシーケンサは
メモリ処理とセンサーデータ生成
へのトリガーを与えるのが仕事と
なります。
センサーデータ生成
輝度変換した画像データからセンサーデータを
生成するには、以下のアルゴリズムを使います。
- 中央の白線の真ん中の位置を見つける
- 中央の白線の真ん中の位置からセンサーデータ生成
中央の白線の真ん中の位置を見つけるには、隣合うピクセルの
差分をとるアルゴリズムを利用します。
VHDLコードを作成する前に、Tcl/Tkを利用して
差分を取ることで、中央の白線の真ん中を検出
できるかをシミュレーションしました。
Tcl/Tkのコードは、以下です。
#!/usr/local/bin/wish
# procedures
# initialize
proc xinit {x} {
global dmax dmin loc_dmax loc_dmin idx
# min max
set dmax $x
set dmin $x
# location
set loc_dmax [expr 255 - $x]
set loc_dmin [expr 255 - $x]
# index
set idx -1
}
# generate data
proc xmake {x} {
global idat
# pixel size
set countmax [lindex $x 0]
# L -> H location
set first [lindex $x 1]
# H -> L location
set last [lindex $x 2]
#
set idat ""
for {set i 0} {$i < $countmax} {incr i} {
if { $i >= $first && $i <= $last } {
set idat "$idat 128"
} else {
set idat "$idat 0"
}
}
}
#
xinit 0
puts "dmax = $dmax dmin = $dmin"
puts "loc_dmax = $loc_dmax loc_dmin = $loc_dmin index = $idx"
puts ""
#
xmake "120 80 100"
puts "data => "
puts $idat
#
set ibuf [lindex $idat 0]
incr idx
set idat [lrange $idat 1 159]
puts ""
foreach n $idat {
set itmp [expr $n - $ibuf]
set ibuf $n
incr idx
if { $itmp >= $dmax } {
set dmax $itmp
set loc_dmax $idx
}
if { $itmp <= $dmin } {
set dmin $itmp
set loc_dmin $idx
}
}
puts "dmax = $dmax dmin = $dmin loc_dmax = $loc_dmax loc_dmin = $loc_dmin"
puts "center = [expr ($loc_dmax+$loc_dmin) / 2]"
このスクリプトを動かして、どんな結果に
なるのかをCUIでテストしました。
差分の最大値、最小値を求めて、位置を算出し
白線の中央を求めると、確かに生成したデータ
から正しい値を叩き出しています。
黒から白、白から黒に変化する位置を
見つけるVHDLコードは、以下のように
定義しました。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity getlocx is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- trigger
CMD_RST : in std_logic ;
CMD_BUF : in std_logic ;
CMD_DTRG : in std_logic ;
-- status
BUSY : out std_logic ;
-- data bus
Din : in std_logic_vector(7 downto 0) ;
LMAX : out std_logic_vector(7 downto 0) ;
LMIN : out std_logic_vector(7 downto 0) ;
MAX : out std_logic_vector(7 downto 0) ;
MIN : out std_logic_vector(7 downto 0) --;
);
end getlocx ;
architecture Behavioral of getlocx is
-- trigger
signal iCMD_RST : std_logic ;
signal iCMD_RST_SFT : std_logic_vector(2 downto 0) ;
signal iCMD_BUF : std_logic ;
signal iCMD_BUF_SFT : std_logic_vector(2 downto 0) ;
signal iCMD_DTRG : std_logic ;
signal iCMD_DTRG_SFT : std_logic_vector(2 downto 0) ;
-- min max
signal iMAX : std_logic_vector(7 downto 0) ;
signal iMIN : std_logic_vector(7 downto 0) ;
-- location
signal iLOC_MAX : std_logic_vector(7 downto 0) ;
signal iLOC_MIN : std_logic_vector(7 downto 0) ;
-- index
signal iPTR : std_logic_vector(7 downto 0) ;
-- sequencer
signal iSTATE : std_logic_vector(1 downto 0) ;
-- BUS buffer
signal iBUFX : std_logic_vector(7 downto 0) ;
signal iTMP : std_logic_vector(7 downto 0) ;
begin
-- output
LMAX <= iLOC_MAX ;
LMIN <= iLOC_MIN ;
MAX <= iMAX ;
MIN <= iMIN ;
-- status output
BUSY <= iSTATE(0) ;
-- trigger
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iCMD_RST_SFT <= "000" ;
iCMD_BUF_SFT <= "000" ;
iCMD_DTRG_SFT <= "000" ;
elsif falling_edge( CLOCK ) then
iCMD_RST_SFT <= iCMD_RST_SFT(1 downto 0) & CMD_RST ;
iCMD_BUF_SFT <= iCMD_BUF_SFT(1 downto 0) & CMD_BUF ;
iCMD_DTRG_SFT <= iCMD_DTRG_SFT(1 downto 0) & CMD_DTRG ;
end if ;
end process ;
iCMD_RST <= '1' when ( iCMD_RST_SFT = "011" or iCMD_RST_SFT = "001" ) else '0' ;
iCMD_BUF <= '1' when ( iCMD_BUF_SFT = "011" or iCMD_BUF_SFT = "001" ) else '0' ;
iCMD_DTRG <= '1' when ( iCMD_DTRG_SFT = "011" or iCMD_DTRG_SFT = "001" ) else '0' ;
-- get data
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iSTATE <= "00" ;
iMAX <= (others => '0') ;
iMIN <= (others => '0') ;
iLOC_MAX <= (others => '1') ;
iLOC_MIN <= (others => '1') ;
iPTR <= (others => '1') ;
iBUFX <= (others => '0') ;
iTMP <= (others => '0') ;
elsif rising_edge( CLOCK ) then
case conv_integer(iSTATE) is
-- wait trigger
when 0 => if ( iCMD_RST = '1' ) then
iSTATE <= "10" ;
iMAX <= (others => '0') ;
iMIN <= (others => '0') ;
iLOC_MAX <= (others => '1') ;
iLOC_MIN <= (others => '1') ;
iPTR <= (others => '1') ;
iBUFX <= (others => '0') ;
iTMP <= (others => '0') ;
elsif ( iCMD_BUF = '1' ) then
iSTATE <= "10" ;
iBUFX <= Din ;
iPTR <= iPTR + '1' ;
elsif ( iCMD_DTRG = '1' ) then
iSTATE <= "01" ;
iTMP <= Din ;
iPTR <= iPTR + '1' ;
else
iSTATE <= "00" ;
end if ;
-- calculate
when 1 => iSTATE <= "11" ;
iTMP <= iTMP - iBUFX ;
-- compare
when 3 => iSTATE <= "10" ;
iBUFX <= iTMP ;
if ( iTMP <= iMAX ) then
iMAX <= iTMP ;
iLOC_MAX <= iPTR ;
end if ;
if ( iTMP <= iMAX ) then
iMIN <= iTMP ;
iLOC_MAX <= iPTR ;
end if ;
-- return first state
when 2 => iSTATE <= "00" ;
-- default
when others =>
iSTATE <= "00" ;
end case ;
end if ;
end process ;
end Behavioral;
変化位置がわかったなら、相加平均を求めます。
これを、センターラインの中央位置とします。
変化位置は、0〜159のどれかになりますが
その相加平均も0〜159になります。
相加平均が、範囲外になった場合は、別の判定
処理が必要になります。
センターラインの中央位置を求めるだけでは
コースの状態を正しく認識できません。
変化位置の差を、ラインの幅として使います。
ライン幅がわかれば、片側白線、白線を判定
できます。
幅を計算して、0〜159になるのでセンターラインの
中央位置との組合せで、コースの状態を判断します。
バスインタフェース
マイコンから、内部レジスタ番号を指定すると
バスに8ビットでデータを出力します。
バス制御は、GP2の8ビットを以下のように
割り当てます。
- GP2.7 camera take photo trigger
- GP2.6 data latch trigger
- GP2.5 motor rotate direction selector bit1
- GP2.4 motor rotate direction selector bit1
- GP2.3 output enable
- GP2.2 internal register number bit2
- GP2.1 internal register number bit1
- GP2.0 internal register number bit0
バスデータは、GP3を使い入力あるいは出力します。
通常は、入力でGP2.3にHを与えると出力に
切り替えます。
内部レジスタは、次のようにセンサーデータ
その他のパラメータを保存しているとします。
- 0 sensor_data_0
- 1 sensor_data_1
- 2 sensor_data_2
- 3 sensor_data_3
- 4 sensor_data_4
- 5 front motor duty ratio
- 6 rear motor duty ratio
- 7 rotary encoder count
データを出力する場合は、GP2.3で制御します。
GP3 <= iDAT when ( GP2(3) = '1' ) else (others => 'Z');
レジスタ番号は、GP2.2〜GP2.0で指定します。
iDAT <= iSENSOR1 when ( GP2(2 downto 0) = "001" ) else -- line_1
iSENSOR2 when ( GP2(2 downto 0) = "010" ) else -- line_2
iSENSOR3 when ( GP2(2 downto 0) = "011" ) else -- line_3
iSENSOR4 when ( GP2(2 downto 0) = "100" ) else -- line_4
('0' & iDUTYF) when ( GP2(2 downto 0) = "101" ) else -- front duty
('0' & iDUTYR) when ( GP2(2 downto 0) = "110" ) else -- rear duty
"00000000" when ( GP2(2 downto 0) = "111" ) else -- rotary encoder
iSENSOR0 ; -- line_0
モータのDUTY比を設定する場合、シーケンサを
利用します。
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iDSTATE <= "00" ;
iDIRF <= "00" ;
iDIRR <= "00" ;
iDIR <= "00" ;
iDUTY <= (others => '0') ;
iDUTYF <= (others => '0') ;
iDUTYR <= (others => '0') ;
elsif rising_edge( CLOCK ) then
case conv_integer(iDSTATE) is
-- wait trigger
when 0 => if ( iTRG = '1' ) then
iDSTATE <= "01" ;
else
iDSTATE <= "00" ;
end if ;
-- latch
when 1 => iDSTATE <= "11" ;
iDIR <= GP2(5 downto 4) ;
iSEL <= GP3(7) ;
iDUTY <= GP3(6 downto 0) ;
-- deliver
when 3 => iDSTATE <= "10" ;
if ( iSEL = '1' ) then
iDIRR <= iDIR ;
iDUTYR <= iDUTY ;
else
iDIRF <= iDIR ;
iDUTYF <= iDUTY ;
end if ;
-- return first state
when 2 => iDSTATE <= "00" ;
-- default
when others =>
iDSTATE <= "00" ;
end case ;
end if ;
end process ;
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iTRG_SFT <= "000" ;
elsif rising_edge(CLOCK) then
iTRG_SFT <= iTRG_SFT(1 downto 0) & GP2(6) ;
end if ;
end process ;
iTRG <= '1' when ( iTRG_SFT = "011" or iTRG_SFT = "001" ) else '0' ;
モータを回すためのパラメータであるDUTY比は
次のように一定時間ごとに再設定します。
process (iMCLK)
begin
if rising_edge(iMCLK) then
iDIR_FRONT <= iDIRF ;
iDIR_REAR <= iDIRR ;
iDUTY_FRONT <= iDUTYF ;
iDUTY_REAR <= iDUTYR ;
end if ;
end process ;
目次
前
次