目次
前
最終VHDLコード
大阪電気通信大学の特設コースを移動した、MCR-VCマシンは
次のVHDLコードを利用しています。
- mcrvcx2.vhd
- barsx.vhd
- bservo.vhd
- nservo.vhd
- cservo.vhd
- lservo.vhd
- clkgenx.vhd
- judge.vhd
- pwmx.vhd
- spwmx.vhd
各VHDLコードを簡単に説明します。
mcrvcx2.vhd
マスターシーケンサと下位ハンドラモジュールに与える
クロックを生成しています。
マスターシーケンサは、100Hz(10ms)で動作させます。
下位ハンドラに与えるクロックは、以下としました。
- DCモータハンドラ 100kHz
- サーボモータハンドラ 100kHz
- BarCodeScannerハンドラ 400kHz
クロック自体は、clkgenx.vhdが生成します。
シーケンサは、スタートトリガーを検出すると
BarCodeScannerが出力する路面情報を手がかり
に該当モードと照合し、DCモータ、サーボモータ
の設定パラメータを確定します。
シーケンサは、ステートマシンと呼ばれる、デジタル回路
ですが、マイコンよりも高速に逐次処理を実現できます。
全ソースコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
--
entity mcrvcx2 is
generic (
TOPX : integer := 6 ;
XMAX : integer := 47 ;
TOPY : integer := 8 ;
YMAX : integer := 239 ;
TOPZ : integer := 18 ;
ZMAX : integer := 239999 ;
TOPB : integer := 6 ;
BMAX : integer := 60 ;
SENSOR_ALL_BLACK : integer := 0 ;
SENSOR_EXSTRA_RIGHT : integer := 1 ;
SENSOR_BIG_RIGHT : integer := 2 ;
SENSOR_RIGHT : integer := 3 ;
SENSOR_TINY_RIGHT : integer := 4 ;
SENSOR_CENTER : integer := 5 ;
SENSOR_TINY_LEFT : integer := 6 ;
SENSOR_LEFT : integer := 7 ;
SENSOR_BIG_LEFT : integer := 8 ;
SENSOR_EXSTRA_LEFT : integer := 9 ;
SENSOR_RIGHT_WHITE : integer := 10 ;
SENSOR_LEFT_WHITE : integer := 11 ;
SENSOR_ALL_WHITE : integer := 12 -- ;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- sensor input
SH : in std_logic ;
CLKA : in std_logic ;
OS : in std_logic ;
MSH : in std_logic ;
-- sensor output
SOUT : out std_logic_vector(7 downto 0) ;
MSOUT : out std_logic_vector(7 downto 0) ;
-- front SERVO
SVOUT : out std_logic ; -- front SERVO control
-- rear PWM
POUT : out std_logic ; -- rear PWM wave
-- start goal trigger
SGTRG : in std_logic ;
GLED : out std_logic ; -- run
RLED : out std_logic ; -- cross white line
-- monitor
SHX : out std_logic ;
CLKAX : out std_logic ;
OSX : out std_logic ;
MSHX : out std_logic ;
MOUT : out std_logic_vector(1 downto 0) --;
);
end mcrvcx2 ;
architecture Behavioral of mcrvcx2 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
RATE : in std_logic_vector(6 downto 0) ;
-- output
POUT : out std_logic --;
);
end component ;
-- component SPWMX
component spwmx is
port(
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
RATE : in std_logic_vector(7 downto 0) ;
-- output
SVOUT : out std_logic --;
);
end component ;
-- BLIND_RUN mode servo and delay component
component bservo is
port (
-- input
SELIN : in std_logic_vector(1 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end component ;
-- NORMAL mode servo and delay component
component nservo is
port (
-- input
SELIN : in std_logic_vector(3 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end component ;
-- CRANK mode servo and delay component
component cservo is
port (
-- input
SELIN : in std_logic_vector(3 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end component ;
-- LANE_CHANGE mode servo and delay component
component lservo is
port (
-- input
SELIN : in std_logic_vector(3 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end component ;
-- BCS component
component barsx is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
SH : in std_logic ;
CLKA : in std_logic ;
OS : in std_logic ;
MSH : in std_logic ;
-- monitor
SHX : out std_logic ;
CLKAX : out std_logic ;
OSX : out std_logic ;
MSHX : out std_logic ;
-- output
MSOUT : out std_logic_vector(7 downto 0) ;
SOUT : out std_logic_vector(7 downto 0) --;
) ;
end component ;
-- clock
signal iPWCLK : std_logic ;
signal iSCLK : std_logic ;
signal iBCLK : std_logic ;
-- trigger
signal iSGTRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG : std_logic ;
-- sensor
signal iMSOUT : std_logic_vector(7 downto 0) ;
signal iSENSOR : std_logic_vector(7 downto 0) ;
-- master sequencer
signal iSTATE : integer range 0 to 31 ;
signal iDIR : std_logic_vector(1 downto 0) ;
signal iDIRX : std_logic_vector(1 downto 0) ;
signal iBSEL : std_logic_vector(1 downto 0) ;
-- judge run sequencer
signal iTCNT : integer range 0 to 255 ;
signal iTCNT_B : std_logic_vector(7 downto 0) ; -- BLIND_RUN
signal iTCNT_N : std_logic_vector(7 downto 0) ; -- NORMAL
signal iTCNT_C : std_logic_vector(7 downto 0) ; -- CRANK
signal iTCNT_L : std_logic_vector(7 downto 0) ; -- LANE_CHANGE
signal iJSENSOR : std_logic_vector(7 downto 0) ;
signal iJUDGE : std_logic_vector(3 downto 0) ;
signal iMODE : integer range 0 to 3 ;
-- SERVO
signal iSVRATE : std_logic_vector(7 downto 0) ;
signal iSVRATE_B : std_logic_vector(7 downto 0) ; -- BLIND_RUN
signal iSVRATE_N : std_logic_vector(7 downto 0) ; -- NORMAL
signal iSVRATE_C : std_logic_vector(7 downto 0) ; -- CRANK
signal iSVRATE_L : std_logic_vector(7 downto 0) ; -- LANE_CHANGE
signal iSVOUT : std_logic ;
-- PWM
signal iPWMRATE : std_logic_vector(6 downto 0) ;
signal iDUTY_B : std_logic_vector(6 downto 0) ;
signal iDUTY_N : std_logic_vector(6 downto 0) ;
signal iDUTY_C : std_logic_vector(6 downto 0) ;
signal iDUTY_L : std_logic_vector(6 downto 0) ;
signal iPWMOUT : std_logic ;
signal iGLED : std_logic ;
signal iRLED : std_logic ;
-- mode
signal iMSEL : std_logic_vector(1 downto 0) ;
begin
-- component clock generator (48MHz/480 = 100kHz)
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,CLOCK,iPWCLK);
-- component clock generator (48MHz/480000 = 100Hz)
CLKZ : clkgenx generic map (TOPZ,ZMAX) port map (nRESET,CLOCK,iSCLK);
-- component clock generator (48MHz/120 = 400kHz)
CLKB : clkgenx generic map (TOPB,BMAX) port map (nRESET,CLOCK,iBCLK);
-- judge
JUDGEX : judge port map (iJSENSOR,iJUDGE) ;
-- component PWM
PWM : pwmx port map (nRESET,iPWCLK,iPWMRATE,iPWMOUT);
-- component SPWMX
SPWM : spwmx port map (nRESET,iPWCLK,iSVRATE,iSVOUT);
-- NORMAL mode servo and delay component
BSD : bservo port map (iBSEL,iSVRATE_B,iDUTY_B,iTCNT_B);
-- component NORMAL servo and delay counter
NSD : nservo port map (iJUDGE,iSVRATE_N,iDUTY_N,iTCNT_N) ;
-- component CRANK servo and delay counter
CSD : cservo port map (iJUDGE,iSVRATE_C,iDUTY_C,iTCNT_C) ;
-- component LANE_CHANGE servo and delay counter
LSD : lservo port map (iJUDGE,iSVRATE_L,iDUTY_L,iTCNT_L) ;
-- BCS component (201kHz)
BCSX : barsx port map (nRESET,iBCLK,SH,CLKA,OS,MSH,SHX,CLKAX,OSX,MSHX,iMSOUT,iSENSOR) ;
-- output
SOUT <= not iSENSOR ;
MSOUT <= not iMSOUT ;
SVOUT <= iSVOUT ; -- front SERVO control
POUT <= iPWMOUT ; -- rear PWM wave
GLED <= not iGLED ;
RLED <= not iRLED ;
MOUT <= not iMSEL ;
-- monitor
iGLED <= '1' when ( iSTATE > 0 ) else '0' ;
iRLED <= '1' when ( iSENSOR = X"FF" ) else '0' ;
-- internal status
iMSEL <= "00" when ( iMODE = 0 ) else -- BLIND_RUN
"01" when ( iMODE = 2 ) else -- CRANK
"10" when ( iMODE = 3 ) else -- LANE_CHANGE
"11" ; -- NORMAL
-- trigger
process (nRESET,iSCLK)
begin
if ( nRESET = '0' ) then
iSGTRG_SFT <= "000" ;
elsif rising_edge(iSCLK) then
iSGTRG_SFT <= iSGTRG_SFT(1 downto 0) & (not SGTRG) ;
end if ;
end process ;
iTRG <= '1' when ( iSGTRG_SFT = "011" or iSGTRG_SFT = "001" ) else '0' ;
-- master sequencer
process (nRESET,iSCLK)
begin
if ( nRESET = '0' ) then
iSTATE <= 0 ;
iBSEL <= "00" ;
iDIR <= "00" ;
iDIRX <= "11" ;
iPWMRATE <= conv_std_logic_vector(0,7);
elsif rising_edge(iSCLK) then
case conv_integer(iSTATE) is
-- wait start trigger
when 0 => if ( iTRG = '1' ) then
iSTATE <= 1 ;
iBSEL <= "00" ;
else
iSTATE <= 0 ;
end if ;
-- send blind run trigger (15%)
when 1 => iSTATE <= 2 ;
iSVRATE <= iSVRATE_B ; -- center
iPWMRATE <= iDUTY_B ;
iTCNT <= conv_integer(iTCNT_B) ;
-- timer up (15%)
when 2 => if ( iTCNT = 0 ) then
iSTATE <= 3 ;
iBSEL <= "01" ;
else
iSTATE <= 2 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- send blind run trigger (30%)
when 3 => iSTATE <= 4 ;
iSVRATE <= iSVRATE_B ; -- center
iPWMRATE <= iDUTY_B ;
iTCNT <= conv_integer(iTCNT_B) ;
-- timer up (30%)
when 4 => if ( iTCNT = 0 ) then
iSTATE <= 5 ;
iBSEL <= "10" ;
else
iSTATE <= 4 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- send blind run trigger (45%)
when 5 => iSTATE <= 6 ;
iSVRATE <= iSVRATE_B ; -- center
iPWMRATE <= iDUTY_B ;
iTCNT <= conv_integer(iTCNT_B) ;
-- timer up (45%)
when 6 => if ( iTCNT = 0 ) then
iSTATE <= 7 ;
iBSEL <= "11" ;
else
iSTATE <= 6 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- send blind run trigger (60%)
when 7 => iSTATE <= 8 ;
iSVRATE <= iSVRATE_B ; -- center
iPWMRATE <= iDUTY_B ;
iTCNT <= conv_integer(iTCNT_B) ;
-- timer up (60%)
when 8 => if ( iTCNT = 0 ) then
iSTATE <= 9 ;
else
iSTATE <= 8 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- branch NORMAL
when 9 => iSTATE <= 10 ;
-- <NORMAL>
-- <NORMAL> get sensor data
when 10 => iJSENSOR <= iMSOUT ; -- iJSENSOR <= iSENSOR
iSTATE <= 11 ;
-- <NORMAL> (judge)
when 11 => if ( iJUDGE = SENSOR_ALL_WHITE ) then
iSTATE <= 14 ; -- branch CRANK
iSVRATE <= iSVRATE_N ;
iPWMRATE <= iDUTY_N ;
iTCNT <= conv_integer(iTCNT_N) ; -- delay
elsif ( iJUDGE = SENSOR_LEFT_WHITE ) then
iDIR <= "10" ; -- left
iSTATE <= 22 ; -- branch LANE_CHANGE
iSVRATE <= iSVRATE_N ;
iPWMRATE <= iDUTY_N ;
iTCNT <= conv_integer(iTCNT_N) ; -- delay
elsif ( iJUDGE = SENSOR_RIGHT_WHITE ) then
iDIR <= "01" ; -- right
iSTATE <= 22 ; -- branch LANE_CHANGE
iSVRATE <= iSVRATE_N ;
iPWMRATE <= iDUTY_N ;
iTCNT <= conv_integer(iTCNT_N) ; -- delay
else
iSTATE <= 12 ; -- branch NORMAL
iSVRATE <= iSVRATE_N ; -- servo counter
iPWMRATE <= iDUTY_N ; -- duty ratio
iTCNT <= conv_integer(iTCNT_N) ; -- delay
iDIR <= "00" ; -- center
end if ;
-- <NORMAL> time up
when 12 => if ( iTCNT = 0 ) then
iSTATE <= 13;
else
iSTATE <= 12 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <NORMAL> return first state
when 13 => iSTATE <= 10 ;
-- <CRANK>
-- <CRANK> delay
when 14 => if ( iTCNT = 0 ) then
iSTATE <= 15 ; -- branch
iSVRATE <= iSVRATE_C ; -- servo counter
else
iSTATE <= 14 ; -- stay
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> get road information
when 15 => iJSENSOR <= iMSOUT ;
iSTATE <= 16 ;
-- <CRANK> (judge)
when 16 => if ( iJUDGE = SENSOR_LEFT_WHITE ) then
iDIR <= "10" ; -- left
iSTATE <= 18 ; -- state
iSVRATE <= iSVRATE_C ; -- servo counter
iPWMRATE <= iDUTY_C ; -- duty ratio
iTCNT <= conv_integer(iTCNT_C) ; -- delay
elsif ( iJUDGE = SENSOR_RIGHT_WHITE ) then
iDIR <= "01" ; -- right
iSTATE <= 18 ; -- state
iSVRATE <= iSVRATE_C ; -- servo counter
iPWMRATE <= iDUTY_C ; -- duty ratio
iTCNT <= conv_integer(iTCNT_C) ; -- delay
else
iDIR <= "00" ; -- center
iSTATE <= 17 ; -- state
iSVRATE <= iSVRATE_C ; -- servo counter
iPWMRATE <= iDUTY_C ; -- duty ratio
iTCNT <= conv_integer(iTCNT_C) ; -- delay
end if ;
-- <CRANK> time up
when 17 => if ( iTCNT = 0 ) then
iSTATE <= 15 ; -- state
iSVRATE <= conv_std_logic_vector(150,8); -- stearing
else
iSTATE <= 17 ; -- stay
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> time up ROTATE
when 18 => if ( iTCNT = 0 ) then
iSTATE <= 19 ; -- next state
else
iSTATE <= 18 ; -- stay
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> get road information
when 19 => iJSENSOR <= iMSOUT ;
iSTATE <= 20 ;
-- <CRANK> (judge)
when 20 => if ( iJUDGE = SENSOR_CENTER ) then
iDIR <= "00" ; -- left
iSTATE <= 21 ; -- state
else
iSTATE <= 19 ; -- state
end if ;
-- <CRANK> transfer NORMAL
when 21 => iSTATE <= 10 ; -- state
-- <LANE_CHANGE>
-- <LANE_CHANGE> delay
when 22 => if ( iTCNT = 0 ) then
iSTATE <= 23 ; -- branch
iSVRATE <= iSVRATE_L ; -- servo counter
iDIRX <= iDIR xor "11" ; -- return direction
else
iSTATE <= 22 ; -- stay
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> get sensor information
when 23 => iJSENSOR <= iMSOUT ;
iSTATE <= 24 ;
-- <LANE_CHANGE> judge
when 24 => if ( iJUDGE = SENSOR_ALL_BLACK ) then
iSTATE <= 26 ; -- run black area
-- left side lane_change
if ( iDIR = "10" ) then
iSVRATE <= conv_std_logic_vector(165,8);
end if ;
-- right side lane_change
if ( iDIR = "01" ) then
iSVRATE <= conv_std_logic_vector(135,8);
end if ;
iPWMRATE <= iDUTY_L ;
iTCNT <= conv_integer(iTCNT_L) ;
else
iSTATE <= 25 ; -- state
iSVRATE <= iSVRATE_L ; -- servo counter
iPWMRATE <= iDUTY_L ; -- duty ratio
iTCNT <= conv_integer(iTCNT_L) ; -- delay
end if ;
-- <LANE_CHANGE> time up except ALL_BLACK
when 25 => if ( iTCNT = 0 ) then
iSTATE <= 23 ;
else
iSTATE <= 25 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> time up ALL_BLACK
when 26 => if ( iTCNT = 0 ) then
iSTATE <= 27 ;
iSVRATE <= conv_std_logic_vector(150,8); -- stearing
else
iSTATE <= 26 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> get road state
when 27 => iJSENSOR <= iMSOUT ;
iSTATE <= 28 ;
-- <LANE_CHANGE> judge
when 28 => if ( iJUDGE = SENSOR_CENTER ) then
iSTATE <= 29 ;
else
iSTATE <= 27 ;
end if ;
-- <LANE_CHANGE> reverse stearing
when 29 => iSTATE <= 30 ; -- state
-- left side lane_change
if ( iDIRX = "10" ) then
iSVRATE <= conv_std_logic_vector(165,8);
end if ;
-- right side lane_change
if ( iDIRX = "01" ) then
iSVRATE <= conv_std_logic_vector(135,8);
end if ;
iPWMRATE <= conv_std_logic_vector(32,7) ; -- DUTY
iTCNT <= 10 ;
-- <LANE_CHANGE> tiemr up
when 30 => if ( iTCNT = 0 ) then
iSTATE <= 31 ; -- state
iSVRATE <= conv_std_logic_vector(150,8); -- stearing
iDIR <= "00" ; -- center
else
iSTATE <= 30 ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> return NORMAL run
when 31 => iSTATE <= 10 ;
-- 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 < 15 ) else -- NORMAL
2 when ( 13 < iSTATE and iSTATE < 22 ) else -- CRANK
3 ;
end Behavioral;
barsx.vhd
BarCodeScannerからピクセルデータを入力し
8ビットの路面情報に変換します。
BarCodeScannerとのインタフェースは、FPGAが
3.3V動作をしているので、抵抗による分圧処理
だけを入れています。
BarCodeScannerのスキャナ基板のマイコンが出力する
SH、φ1(CLKA)を同期化後、信号OSをラッチします。
φ1は、100kHzクロックなので、FPGA内部では400kHzで
サンプリングし同期化しています。
同期化したφ1(CLKA)で、rising_edge、falling_edgeの
どちらかでデータを記憶し、さらに間引いて8ビットに
しています。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity barsx is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
SH : in std_logic ;
CLKA : in std_logic ;
OS : in std_logic ;
MSH : in std_logic ;
-- monitor
SHX : out std_logic ;
CLKAX : out std_logic ;
OSX : out std_logic ;
MSHX : out std_logic ;
-- output
MSOUT : out std_logic_vector(7 downto 0) ;
SOUT : out std_logic_vector(7 downto 0) --;
) ;
end barsx;
architecture Behavioral of barsx is
-- capture counter
signal iCNT : std_logic_vector(12 downto 0);
signal iSH : std_logic ;
signal iCLKA : std_logic ;
signal iCLKA_SFT : std_logic_vector(2 downto 0);
signal iCLKA_TRG : std_logic ;
signal iOS : std_logic ;
signal iMSH : std_logic ;
signal iREGX : std_logic_vector(7 downto 0);
signal iREG : std_logic_vector(7 downto 0);
-- sensor out
signal iSCNT : integer range 0 to 400 ;
signal iREGBUF : std_logic_vector(7 downto 0);
begin
-- input
iSH <= SH ;
iCLKA <= CLKA ;
iOS <= OS ;
iMSH <= MSH ;
-- output
MSOUT <= iREGBUF ;
SOUT <= iREG ;
-- monitor output
SHX <= not iSH ;
CLKAX <= not iCLKA_TRG ;
OSX <= not iOS ;
MSHX <= not iMSH ;
-- synchronizer
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iCLKA_SFT <= "000" ;
elsif rising_edge(CLOCK) then
iCLKA_SFT <= iCLKA_SFT(1 downto 0) & iCLKA ;
end if ;
end process ;
iCLKA_TRG <= '1' when (iCLKA_SFT = "011" or iCLKA_SFT = "001") else '0' ;
-- counter and OS capture
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iCNT <= (others => '0') ;
iREG <= (others => '0') ;
iREGX <= (others => '0') ;
elsif falling_edge(CLOCK) then
-- counter
if ( iSH = '1' ) then
iCNT <= (others => '0') ;
elsif ( iCLKA_TRG = '1' ) then
iCNT <= iCNT + '1' ;
end if ;
-- get OS
if ( conv_integer(iCNT) = 128 ) then
iREGX(7) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 256 ) then
iREGX(6) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 374 ) then
iREGX(5) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 512 ) then
iREGX(4) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 640 ) then
iREGX(3) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 768 ) then
iREGX(2) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 896 ) then
iREGX(1) <= iOS ;
end if ;
if ( conv_integer(iCNT) = 1024 ) then
iREGX(0) <= iOS ;
end if ;
-- latch
if ( conv_integer(iCNT) = 1030 ) then
iREG <= iREGX ;
end if ;
-- clear
if ( conv_integer(iCNT) = 1035 ) then
iREGX <= "00000000" ;
end if ;
end if ;
end process ;
-- buffering
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iREGBUF <= (others => '0') ;
iSCNT <= 0 ;
elsif rising_edge(CLOCK) then
-- icrement
iSCNT <= iSCNT + 1 ;
--
if ( conv_integer(iSCNT) = 400 ) then
iREGBUF <= iREG ;
iSCNT <= 0 ;
end if ;
end if ;
end process ;
end Behavioral;
bservo.vhd
BLIND_RUNモード時、DCモータのDUTY比、サーボ
モータの角度等価カウント値、継続時間を出力
します。
センサー情報を入力として、3パラメータを出力
します。
単純なデコーダで、実現しました。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity bservo is
port (
-- input
SELIN : in std_logic_vector(1 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end bservo;
architecture Behavioral of bservo is
-- servo motor counter
signal iSVCNT : std_logic_vector(7 downto 0);
-- delay counter
signal iDUTY : std_logic_vector(6 downto 0);
-- delay counter
signal iDCNT : std_logic_vector(7 downto 0);
begin
-- output
SVCNT <= iSVCNT ;
DUTY <= iDUTY ;
DCNT <= iDCNT ;
-- servo motor counter
iSVCNT <= conv_std_logic_vector(140,8) ; -- default
-- DUTY ratio
iDUTY <= conv_std_logic_vector(15,7) when ( SELIN = "00" ) else -- 15 %
conv_std_logic_vector(30,7) when ( SELIN = "01" ) else -- 30 %
conv_std_logic_vector(45,7) when ( SELIN = "10" ) else -- 45 %
conv_std_logic_vector(75,7) when ( SELIN = "11" ) else -- 60 %
conv_std_logic_vector(55,7) ; -- default
-- delay counter
iDCNT <= conv_std_logic_vector(100,8) when ( SELIN = "00" ) else -- 15 %
conv_std_logic_vector(100,8) when ( SELIN = "01" ) else -- 30 %
conv_std_logic_vector(200,8) when ( SELIN = "10" ) else -- 45 %
conv_std_logic_vector( 50,8) when ( SELIN = "11" ) else -- 60 %
conv_std_logic_vector( 0,8) ; -- default
end Behavioral;
nservo.vhd
NORMALモード時、DCモータのDUTY比、サーボ
モータの角度等価カウント値、継続時間を出力
します。
センサー情報を入力として、3パラメータを出力
します。
単純なデコーダで、実現しました。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity nservo is
port (
-- input
SELIN : in std_logic_vector(3 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end nservo;
architecture Behavioral of nservo is
-- servo motor counter
signal iSVCNT : std_logic_vector(7 downto 0);
-- delay counter
signal iDUTY : std_logic_vector(6 downto 0);
-- delay counter
signal iDCNT : std_logic_vector(7 downto 0);
begin
-- output
SVCNT <= iSVCNT ;
DUTY <= iDUTY ;
DCNT <= iDCNT ;
-- servo motor counter
iSVCNT <= conv_std_logic_vector(170,8) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector(165,8) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector(160,8) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector(155,8) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector(145,8) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector(140,8) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector(135,8) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(130,8) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector(150,8) ; -- center
-- DUTY ratio
iDUTY <= conv_std_logic_vector(45,7) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector(55,7) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector(55,7) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector(60,7) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector(60,7) when ( SELIN = X"5" ) else -- center
conv_std_logic_vector(60,7) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector(55,7) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector(55,7) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(45,7) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector(60,7) ; -- default
-- delay counter
iDCNT <= conv_std_logic_vector( 9,8) when ( SELIN = X"0" ) else -- all_black
conv_std_logic_vector(103,8) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector( 59,8) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector( 59,8) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector( 19,8) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector( 19,8) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector( 59,8) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector( 59,8) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(103,8) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector( 99,8) ; -- default
end Behavioral;
cservo.vhd
CRANKモード時、DCモータのDUTY比、サーボ
モータの角度等価カウント値、継続時間を出力
します。
センサー情報を入力として、3パラメータを出力
します。
単純なデコーダで、実現しました。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity cservo is
port (
-- input
SELIN : in std_logic_vector(3 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end cservo;
architecture Behavioral of cservo is
-- servo motor counter
signal iSVCNT : std_logic_vector(7 downto 0);
-- delay counter
signal iDUTY : std_logic_vector(6 downto 0);
-- delay counter
signal iDCNT : std_logic_vector(7 downto 0);
begin
-- output
SVCNT <= iSVCNT ;
DUTY <= iDUTY ;
DCNT <= iDCNT ;
-- servo motor counter
iSVCNT <= conv_std_logic_vector(170,8) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector(165,8) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector(160,8) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector(155,8) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector(145,8) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector(140,8) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector(135,8) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(130,8) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector(150,8) ; -- center
-- DUTY ratio
iDUTY <= conv_std_logic_vector(40,7) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector(45,7) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector(45,7) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector(50,7) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector(50,7) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector(45,7) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector(45,7) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(40,7) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector(40,7) when ( SELIN = X"A" ) else -- right_white
conv_std_logic_vector(40,7) when ( SELIN = X"B" ) else -- left_white
conv_std_logic_vector(50,7) ; -- default
-- delay counter
iDCNT <= conv_std_logic_vector( 9,8) when ( SELIN = X"0" ) else -- all_black
conv_std_logic_vector(103,8) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector( 59,8) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector( 59,8) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector( 19,8) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector( 19,8) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector( 59,8) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector( 59,8) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(103,8) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector( 49,6) when ( SELIN = X"A" ) else -- right_white
conv_std_logic_vector( 49,6) when ( SELIN = X"B" ) else -- left_white
conv_std_logic_vector( 19,8) ; -- default
end Behavioral;
lservo.vhd
LANE_CHANGEモード時、DCモータのDUTY比、サーボ
モータの角度等価カウント値、継続時間を出力
します。
センサー情報を入力として、3パラメータを出力
します。
単純なデコーダで、実現しました。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity lservo is
port (
-- input
SELIN : in std_logic_vector(3 downto 0) ;
-- servo motor counter
SVCNT : out std_logic_vector(7 downto 0) ;
-- duty ratio
DUTY : out std_logic_vector(6 downto 0) ;
-- delay counter
DCNT : out std_logic_vector(7 downto 0) --;
);
end lservo;
architecture Behavioral of lservo is
-- servo motor counter
signal iSVCNT : std_logic_vector(7 downto 0);
-- delay counter
signal iDUTY : std_logic_vector(6 downto 0);
-- delay counter
signal iDCNT : std_logic_vector(7 downto 0);
begin
-- output
SVCNT <= iSVCNT ;
DUTY <= iDUTY ;
DCNT <= iDCNT ;
-- servo motor counter
iSVCNT <= conv_std_logic_vector(130,8) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector(135,8) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector(140,8) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector(145,8) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector(155,8) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector(160,8) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector(165,8) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(170,8) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector(170,6) when ( SELIN = X"A" ) else -- right_white
conv_std_logic_vector(130,6) when ( SELIN = X"B" ) else -- left_white
conv_std_logic_vector(150,8) ; -- center
-- DUTY ratio
iDUTY <= conv_std_logic_vector(25,7) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector(30,7) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector(30,7) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector(35,7) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector(35,7) when ( SELIN = X"5" ) else -- center
conv_std_logic_vector(35,7) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector(30,7) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector(30,7) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(25,7) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector(25,7) when ( SELIN = X"A" ) else -- right_white
conv_std_logic_vector(25,7) when ( SELIN = X"B" ) else -- left_white
conv_std_logic_vector(30,7) ; -- default
-- delay counter
iDCNT <= conv_std_logic_vector( 69,8) when ( SELIN = X"0" ) else -- all_black
conv_std_logic_vector(103,8) when ( SELIN = X"1" ) else -- exstra_right
conv_std_logic_vector( 59,8) when ( SELIN = X"2" ) else -- big_right
conv_std_logic_vector( 59,8) when ( SELIN = X"3" ) else -- right
conv_std_logic_vector( 19,8) when ( SELIN = X"4" ) else -- tiny_right
conv_std_logic_vector( 19,8) when ( SELIN = X"6" ) else -- tiny_left
conv_std_logic_vector( 59,8) when ( SELIN = X"7" ) else -- left
conv_std_logic_vector( 59,8) when ( SELIN = X"8" ) else -- big_left
conv_std_logic_vector(103,8) when ( SELIN = X"9" ) else -- exstra_left
conv_std_logic_vector( 19,8) ; -- default
end Behavioral;
judge.vhd
BarCodeScannerの出力した路面情報を
数値にデコードします。
使う数値は、以下としました。
- SENSOR_ALL_BLACK 0
- SENSOR_EXSTRA_RIGHT 1
- SENSOR_BIG_RIGHT 2
- SENSOR_RIGHT 3
- SENSOR_TINY_RIGHT 4
- SENSOR_CENTER 5
- SENSOR_TINY_LEFT 6
- SENSOR_LEFT 7
- SENSOR_BIG_LEFT 8
- SENSOR_EXSTRA_LEFT 9
- SENSOR_RIGHT_WHITE 10
- SENSOR_LEFT_WHITE 11
- SENSOR_ALL_WHITE 12
デコーダなので、単純な組み合わせ回路に
してあります。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity judge is
port (
-- input
DIN : in std_logic_vector(7 downto 0) ;
-- output
JOUT : out std_logic_vector(3 downto 0) --;
);
end judge;
architecture Behavioral of judge is
signal iJOUT : std_logic_vector(3 downto 0);
begin
-- output
JOUT <= iJOUT ;
-- convert
iJOUT <= "0000" when ( DIN = X"00" ) else -- all_black
"0001" when ( DIN = X"07" ) else -- exstra_right
"0010" when ( DIN = X"03" ) else -- big_right
"0011" when ( DIN = X"06" ) else -- right
"0100" when ( DIN = X"04" ) else -- tiny_right
"0101" when ( DIN = X"18" ) else -- center
"0101" when ( DIN = X"1C" ) else -- center
"0101" when ( DIN = X"38" ) else -- center
"0110" when ( DIN = X"20" ) else -- tiny_left
"0111" when ( DIN = X"60" ) else -- left
"1000" when ( DIN = X"C0" ) else -- big_left
"1001" when ( DIN = X"E0" ) else -- exstra_right
"1010" when ( DIN = X"0F" ) else -- right_white
"1010" when ( DIN = X"1F" ) else -- right_white
"1011" when ( DIN = X"F8" ) else -- left_white
"1011" when ( DIN = X"F0" ) else -- left_white
"1100" when ( DIN = X"FF" ) else -- all_white
"1111" ;
end Behavioral;
シーケンサで、2種のモータを制御していますが
DCモータハンドラにpwmx.vhd、サーボモータハン
ドラにspwmx.vhdを利用しました。
pwmx.vhd
シーケンサから与えられたDUTY比を利用
して、DCモータの回転速度を制御します。
出力PWM波形は、フォトカプラを介して
パワーMOSFETのゲートに入力します。
DCモータに、電力(パワー)を供給します。
動作クロックは、100kHzとしました。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity pwmx is
port(
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
RATE : in std_logic_vector(6 downto 0) ;
-- output
POUT : out std_logic --;
);
end pwmx ;
architecture Behavioral of pwmx is
signal iPCNT : integer range 0 to 100 ;
signal iPOUT : std_logic ;
signal iPRATEX : std_logic_vector(6 downto 0) ;
begin
-- output
POUT <= iPOUT ;
-- comparetor
iPOUT <= '1' when ( iPCNT < conv_integer(iPRATEX) ) else '0' ;
-- counter
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iPCNT <= 0 ;
iPRATEX <= (others => '0') ;
elsif rising_edge( CLOCK ) then
if ( iPCNT = 100 ) then
iPCNT <= 0 ;
iPRATEX <= RATE ;
else
iPCNT <= iPCNT + 1 ;
end if ;
end if ;
end process ;
end Behavioral;
spwmx.vhd
シーケンサから与えられた回転角度カウント
値を利用して、サーボモータを制御します。
出力パルスは、フォトカプラを介して、サーボ
モータに供給します。
動作クロックは、100kHzとしました。
VHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
-- CLOCK 100kHz
entity spwmx is
port(
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
RATE : in std_logic_vector(7 downto 0) ;
-- output
SVOUT : out std_logic --;
);
end spwmx ;
architecture Behavioral of spwmx is
-- counter
signal iPCNT : integer range 0 to 2000 ;
signal iRATE : std_logic_vector(7 downto 0) ;
signal iSVOUT : std_logic ;
begin
-- output
SVOUT <= iSVOUT ;
-- selector
iSVOUT <= '1' when ( iPCNT < conv_integer(iRATE) ) else '0' ;
-- divider
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iPCNT <= 0 ;
iRATE <= conv_std_logic_vector(100,8) ;
elsif rising_edge( CLOCK ) then
if ( iPCNT = 2000 ) then
iPCNT <= 0 ;
iRATE <= RATE ;
else
iPCNT <= iPCNT + 1 ;
end if ;
end if ;
end process ;
end Behavioral;
目次
前