メカ変更
左右に別個のモータを配置する方式では、首振り運動が
発生します。低速では、首振りでも走行に問題はないと
言えますが、高速にすると首振りがあると、路面情報を
誤認識して、コースアウトします。
高速走行マシンに仕上げたいので、前輪をサーボモータ
後輪をDCモータにしたメカに変更します。
シャーシにDCモータをつけた状態は、以下です。
サーボモータは、手元にあった素性のわからないものです。
サーボモータの特性がわからないと、駆動回路やファームウエア
を作れないので、動作テストシステムを考えました。
サーボモータの駆動回路は、以下です。
駆動回路は、非常に単純です。
パルスをフォトカプラの入力に与え、別に用意した電源から
必要な波高値のパルスを出力します。テスト用電源は、以下
の簡単なものを用意しました。
３Ｖのコイン電池を並列にしたものを、直列にして６Ｖに
しています。３Ｖは、ＤＣモータの駆動に利用します。
ＤＩＰスイッチは、DUTY比を設定するため、100〜200の
整数をカバーできる８ビットにしました。
トリガースイッチで、DUTY比を設定したいので、２モータの
ドライブ回路のスイッチを使います。
サーボモータの特性を調べるために利用したVHDLコードは
以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity svtst is
generic (
TOPX : integer := 5 ;
XMAX : integer := 24 ;
TOPY : integer := 9 ;
YMAX : integer := 500 ;
TOPZ : integer := 3 ;
ZMAX : integer := 5 --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- PWM input
DUTYX : in std_logic_vector(7 downto 0) ;
-- front SERVO
SVOUT : out std_logic ; -- front SERVO control
-- latch trigger
SGTRG : in std_logic ;
-- monitor
MOUT : out std_logic_vector(7 downto 0) --;
);
end svtst;
architecture Behavioral of svtst 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 ;
-- 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 ;
-- clock
signal iPCLK : std_logic ;
signal iPWCLK : std_logic ;
signal iSCLK : std_logic ;
-- trigger
signal iSGTRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG : std_logic ;
-- duty ratio
signal iDUTYX : std_logic_vector(7 downto 0) ;
signal iDUTY : integer range 0 to 255 ;
-- Servo PWM
signal iSVRATE : std_logic_vector(7 downto 0) ;
signal iSVOUT : std_logic ;
-- sequencer
signal iSTATE : std_logic_vector(1 downto 0) ;
begin
-- component clock generator (48MHz/48 = 1MHz)
CLKX : clkgenx generic map (TOPX,XMAX) port map (nRESET,CLOCK,iPCLK);
-- component clock generator (100kHz/1000 = 100Hz)
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,iSCLK,iPWCLK);
-- component clock generator ( 1MHz/10 = 100kHz)
CLKZ : clkgenx generic map (TOPZ,ZMAX) port map (nRESET,iPCLK,iSCLK);
-- component SPWMX
SPWM : spwmx port map (nRESET,iSCLK,iSVRATE,iSVOUT);
-- output
SVOUT <= iSVOUT ; -- front SERVO control
MOUT <= not iSVRATE ;
-- trigger
process (nRESET,iPWCLK)
begin
if ( nRESET = '0' ) then
iSGTRG_SFT <= "000" ;
elsif rising_edge(iPWCLK) 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' ;
-- get DUTY ratio
process (nRESET,iPWCLK)
begin
if ( nRESET = '0' ) then
iDUTYX <= conv_std_logic_vector(150,8) ;
elsif rising_edge(iPWCLK) then
iDUTYX <= DUTYX ;
end if ;
end process ;
-- master sequencer
process (nRESET,iPWCLK)
begin
if ( nRESET = '0' ) then
iSTATE <= "00" ;
iDUTY <= 100 ;
iSVRATE <= conv_std_logic_vector(100,8) ;
elsif rising_edge(iPWCLK) then
case conv_integer(iSTATE) is
-- wait start trigger
when 0 => if ( iTRG = '1' ) then
iSTATE <= "01" ;
else
iSTATE <= "00" ;
end if ;
-- judge
when 1 => if ( conv_integer(iDUTYX) < 50 ) then
iDUTY <= 50 ;
elsif ( conv_integer(iDUTYX) > 150 ) then
iDUTY <= 150 ;
else
iDUTY <= conv_integer(iDUTYX) ;
end if ;
iSTATE <= "11" ;
-- copy
when 3 => iSTATE <= "10" ;
iSVRATE <= conv_std_logic_vector(iDUTY,8) ;
-- return first state
when 2 => iSTATE <= "00" ;
-- default
when others => iSTATE <= "00" ;
end case ;
end if ;
end process ;
end Behavioral;
DUTY比をDIPスイッチで指定後、トリガースイッチで
パルス出力回路で使うカウンタ値を変更します。
実際のパルスを出力する回路の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 1000 ;
signal iRATE : std_logic_vector(7 downto 0) ;
signal iSVOUT : std_logic ;
begin
-- output
SVOUT <= iSVOUT ;
-- comparator
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 = 1000 ) then
iPCNT <= 0 ;
iRATE <= RATE ;
else
iPCNT <= iPCNT + 1 ;
end if ;
end if ;
end process ;
end Behavioral;
手持ちのサーボモータは、10ms周期で0.5msから1.5msの
パルス幅で、０度〜180度となりました。
サーボモータの特性がわかったので、ドライブ回路を設計します。
基板は、以下のようになりました。
DCモータを回転させる電源電圧は３Ｖ程度とし、FETのゲート
には、５Ｖ程度を印加します。
モータドライブ回路に加えて、センサー情報、走行モードを
LEDで表示します。
この基板をテストするVHDLコードは、以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity drvtst is
generic (
TOPX : integer := 5 ;
XMAX : integer := 24 ;
TOPY : integer := 9 ;
YMAX : integer := 500 --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- sensor input
SW : in std_logic_vector(7 downto 0) ;
-- sensor output
SOUT : out std_logic_vector(7 downto 0) --;
);
end drvtst;
architecture Behavioral of drvtst 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 ;
-- clock
signal iPCLK : std_logic ;
signal iPWCLK : std_logic ;
-- input
signal iSW : std_logic_vector(7 downto 0) ;
signal iSOUT : std_logic_vector(7 downto 0) ;
begin
-- component clock generator (48MHz/48 = 1MHz)
CLKX : clkgenx generic map (TOPX,XMAX) port map (nRESET,CLOCK,iPCLK);
-- component clock generator (1MHz/1000 = 1kHz)
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,iPCLK,iPWCLK);
-- input
iSW(1 downto 0) <= SW(1 downto 0) ;
iSW(7 downto 2) <= not SW(7 downto 2) ;
-- output
SOUT <= iSOUT ;
-- generate sensor information
process (nRESET,iPWCLK)
begin
if ( nRESET = '0' ) then
iSOUT <= (others => '1') ;
elsif rising_edge(iPWCLK) then
iSOUT <= iSW ;
end if ;
end process ;
end Behavioral;
スイッチで設定した論理値を、そのままか反転して基板上の
LEDに出力しますが、スイッチの押すタイミングを工夫すると
DCモータへPWM波形を出力できます。
ドライブ回路のテストを終えてから、実際にマシンを動かす
VHDLコードをFPGAにダウンロードして、走行させました。
このVHDLコードは以下です。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity mcrvcx1 is
generic (
TOPX : integer := 6 ;
XMAX : integer := 47 ;
TOPY : integer := 8 ;
YMAX : integer := 239 ;
TOPZ : integer := 9 ;
ZMAX : integer := 499 ;
SENSOR_ALL_BLACK : integer := 0 ;
SENSOR_TINY_RIGHT : integer := 1 ;
SENSOR_RIGHT : integer := 2 ;
SENSOR_BIG_RIGHT : integer := 3 ;
SENSOR_CENTER : integer := 4 ;
SENSOR_TINY_LEFT : integer := 5 ;
SENSOR_LEFT : integer := 6 ;
SENSOR_BIG_LEFT : integer := 7 ;
SENSOR_LEFT_WHITE : integer := 8 ;
SENSOR_RIGHT_WHITE : integer := 9 ;
SENSOR_ALL_WHITE : integer := 10 -- ;
);
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) ;
-- 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
MOUT : out std_logic_vector(1 downto 0) --;
);
end mcrvcx1;
architecture Behavioral of mcrvcx1 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(5 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 ;
-- clock
signal iPCLK : std_logic ;
signal iPWCLK : std_logic ;
signal iSCLK : std_logic ;
-- trigger
signal iSGTRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG : std_logic ;
-- sensor
signal iSENSOR : std_logic_vector(7 downto 0) ;
-- master sequencer
signal iSTATE : std_logic_vector(5 downto 0) ;
signal iDIRECTION : std_logic_vector(1 downto 0) ;
-- judge run sequencer
signal iTCNT : integer range 0 to 300 ;
signal iJSENSOR : std_logic_vector(7 downto 0) ;
signal iJUDGE : std_logic_vector(3 downto 0) ;
signal iMODE : integer range 0 to 3 ;
-- PWM
signal iSVRATE : std_logic_vector(7 downto 0) ;
signal iSVOUT : std_logic ;
signal iPWMRATE : std_logic_vector(5 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/96 = 500kHz)
CLKX : clkgenx generic map (TOPX,XMAX) port map (nRESET,CLOCK,iPCLK);
-- component clock generator (48MHz/480 = 100kHz)
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,CLOCK,iPWCLK);
-- component clock generator (100kHz/1000 = 100Hz)
CLKZ : clkgenx generic map (TOPZ,ZMAX) port map (nRESET,iPWCLK,iSCLK);
-- judge
JUDGEX : judge port map (iJSENSOR,iJUDGE) ;
-- component PWM
PWM : pwmx port map (nRESET,iPCLK,iPWMRATE,iPWMOUT);
-- component SPWMX
SPWM : spwmx port map (nRESET,iPWCLK,iSVRATE,iSVOUT);
-- output
SOUT <= not SENSOR ;
SVOUT <= iSVOUT ; -- front SERVO control
POUT <= iPWMOUT ; -- rear PWM wave
GLED <= not iGLED ;
RLED <= not iRLED ;
MOUT <= not iMSEL ;
-- monitor
iGLED <= '1' when ( conv_integer(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' ;
-- generate sensor information
process (nRESET,iPWCLK)
begin
if ( nRESET = '0' ) then
iSENSOR <= (others => '0') ;
elsif rising_edge(iPWCLK) then
iSENSOR <= SENSOR ;
end if ;
end process ;
-- master sequencer
process (nRESET,iSCLK)
begin
if ( nRESET = '0' ) then
iSTATE <= (others => '0') ;
iDIRECTION <= "00" ;
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 0,6);
elsif rising_edge(iSCLK) then
case conv_integer(iSTATE) is
-- wait start trigger
when 0 => if ( iTRG = '1' ) then
iSTATE <= conv_std_logic_vector(1,6) ;
else
iSTATE <= (others => '0') ;
end if ;
-- send blind run trigger (10%)
when 1 => iSTATE <= conv_std_logic_vector( 2,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 10,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
-- timer up (10%)
when 2 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector( 3,6) ;
else
iSTATE <= conv_std_logic_vector( 2,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- send blind run trigger (25%)
when 3 => iSTATE <= conv_std_logic_vector( 4,6) ;
iPWMRATE <= conv_std_logic_vector(25,6) ;
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
-- timer up (25%)
when 4 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector( 5,6) ;
else
iSTATE <= conv_std_logic_vector( 4,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- send blind run trigger (35%)
when 5 => iSTATE <= conv_std_logic_vector( 6,6) ;
iPWMRATE <= conv_std_logic_vector(35,6) ;
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
-- timer up (35%)
when 6 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector( 7,6) ;
else
iSTATE <= conv_std_logic_vector( 6,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- send blind run trigger (50%)
when 7 => iSTATE <= conv_std_logic_vector( 8,6) ;
iPWMRATE <= conv_std_logic_vector(50,6) ;
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- timer up (50%)
when 8 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector( 9,6) ;
else
iSTATE <= conv_std_logic_vector( 8,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- branch NORMAL
when 9 => iSTATE <= conv_std_logic_vector(10,6) ;
-- <NORMAL>
-- <NORMAL> get sensor data
when 10 => iJSENSOR <= iSENSOR ;
iSTATE <= conv_std_logic_vector(11,6) ;
-- <NORMAL> (judge)
when 11 => if ( iJUDGE = SENSOR_ALL_WHITE ) then
iSTATE <= conv_std_logic_vector(14,6) ;
elsif ( iJUDGE = SENSOR_LEFT_WHITE ) then
iDIRECTION <= "10" ; -- left
iSTATE <= conv_std_logic_vector(30,6) ;
elsif ( iJUDGE = SENSOR_RIGHT_WHITE ) then
iDIRECTION <= "01" ; -- right
iSTATE <= conv_std_logic_vector(30,6) ;
elsif ( iJUDGE = SENSOR_CENTER ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 35,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
elsif ( iJUDGE = SENSOR_TINY_RIGHT ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector( 89,8); --
iPWMRATE <= conv_std_logic_vector( 30,6);
iTCNT <= 50 ; -- 0.5sec 100Hz = 10ms
elsif ( iJUDGE = SENSOR_RIGHT ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector( 79,8); --
iPWMRATE <= conv_std_logic_vector( 25,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
elsif ( iJUDGE = SENSOR_BIG_RIGHT ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector( 70,8); --
iPWMRATE <= conv_std_logic_vector( 20,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
elsif ( iJUDGE = SENSOR_TINY_LEFT ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector(111,8); --
iPWMRATE <= conv_std_logic_vector( 30,6);
iTCNT <= 50 ; -- 0.5sec 100Hz = 10ms
elsif ( iJUDGE = SENSOR_LEFT ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector(120,8); --
iPWMRATE <= conv_std_logic_vector( 25,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
elsif ( iJUDGE = SENSOR_BIG_LEFT ) then
iSTATE <= conv_std_logic_vector( 12,6);
iSVRATE <= conv_std_logic_vector(130,8); --
iPWMRATE <= conv_std_logic_vector( 20,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
else
iDIRECTION <= "00" ; -- center
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
iSTATE <= conv_std_logic_vector(12,6) ;
end if ;
-- <NORMAL> time up
when 12 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(13,6) ;
else
iSTATE <= conv_std_logic_vector(12,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <NORMAL> return first state
when 13 => iSTATE <= conv_std_logic_vector(10,6) ;
-- <CRANK> slow move
when 14 => iSTATE <= conv_std_logic_vector( 15,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 25,6);
iTCNT <= 300 ; -- 3sec 100Hz = 10ms
-- <CRANK> time up
when 15 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(16,6) ;
else
iSTATE <= conv_std_logic_vector(15,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> get road information
when 16 => iJSENSOR <= iSENSOR ;
iDIRECTION <= "00" ; -- center
iSTATE <= conv_std_logic_vector(17,6) ;
-- <CRANK> judge
when 17 => if ( iJUDGE = SENSOR_RIGHT_WHITE ) then
iSTATE <= conv_std_logic_vector(19,6);
iDIRECTION <= "01" ; -- right
elsif ( iJUDGE = SENSOR_LEFT_WHITE ) then
iSTATE <= conv_std_logic_vector(19,6);
iDIRECTION <= "10" ; -- left
-- keep this state and delay
else
iSTATE <= conv_std_logic_vector( 18,6);
iSVRATE <= conv_std_logic_vector(100,8);
iPWMRATE <= conv_std_logic_vector( 30,6);
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
end if ;
-- <CRANK> timer up
when 18 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(17,6);
else
iSTATE <= conv_std_logic_vector(18,6);
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> turn
when 19 => iSTATE <= conv_std_logic_vector(20,6);
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- right_turn
if ( iDIRECTION = "01" ) then
iSVRATE <= conv_std_logic_vector(150,8); -- max angle
iPWMRATE <= conv_std_logic_vector( 30,6);
-- left_white
elsif ( iDIRECTION = "10" ) then
iSVRATE <= conv_std_logic_vector( 50,8); -- min angle
iPWMRATE <= conv_std_logic_vector( 30,6);
end if ;
-- <CRANK> turn time up
when 20 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(21,6) ;
else
iSTATE <= conv_std_logic_vector(20,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> get road information
when 21 => iJSENSOR <= iSENSOR ;
iSTATE <= conv_std_logic_vector(22,6) ;
-- <CRANK> judge center
when 22 => -- center
if ( iJUDGE = SENSOR_CENTER ) then
iSTATE <= conv_std_logic_vector(24,6) ;
else
iSTATE <= conv_std_logic_vector(23,6) ;
iTCNT <= 100 ; -- 1sec 100Hz = 10ms
end if ;
-- <CRANK> turn delay center time up
when 23 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(22,6) ;
else
iSTATE <= conv_std_logic_vector(23,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> complete turn
when 24 =>
iSTATE <= conv_std_logic_vector(25,6) ;
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 30,6);
-- <CRANK> delay exit turn time up
when 25 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(26,6);
else
iSTATE <= conv_std_logic_vector(25,6);
iTCNT <= iTCNT - 1 ;
end if ;
-- <CRANK> center move
when 26 => iSTATE <= conv_std_logic_vector( 27,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 35,6);
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- <CRANK> exit center move time up
when 27 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(28,6);
else
iSTATE <= conv_std_logic_vector(27,6);
iTCNT <= iTCNT - 1 ;
end if ;
-- dummy
when 28 => iSTATE <= conv_std_logic_vector(29,6);
-- <CRANK> NORMAL run
when 29 => iSTATE <= conv_std_logic_vector(10,6);
-- <LANE_CHANGE>
-- <LANE_CHANGE> blind run
when 30 => iSTATE <= conv_std_logic_vector( 31,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 35,6);
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- <LANE_CHANGE> blind run delay time up
when 31 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(32,6);
else
iSTATE <= conv_std_logic_vector(31,6);
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> get sensor data
when 32 => iJSENSOR <= iSENSOR ;
iSTATE <= conv_std_logic_vector(33,6);
-- <LANE_CHANGE> judge all black
when 33 => if ( iJUDGE = SENSOR_ALL_BLACK ) then
iSTATE <= conv_std_logic_vector(34,6);
else
iSTATE <= conv_std_logic_vector(31,6);
end if ;
-- <LANE_CHANGE> turn
when 34 => iSTATE <= conv_std_logic_vector(35,6);
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- right_turn
if ( iDIRECTION = "01" ) then
iSVRATE <= conv_std_logic_vector(128,8);
iPWMRATE <= conv_std_logic_vector( 40,6);
-- left_white
elsif ( iDIRECTION = "10" ) then
iSVRATE <= conv_std_logic_vector( 72,8);
iPWMRATE <= conv_std_logic_vector( 40,6);
end if ;
-- <LANE_CHANGE> turn time up
when 35 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(36,6) ;
else
iSTATE <= conv_std_logic_vector(35,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> center move
when 36 => iSTATE <= conv_std_logic_vector( 37,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 35,6);
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- <LANE_CHANGE> turn time up
when 37 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(38,6) ;
else
iSTATE <= conv_std_logic_vector(37,6) ;
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> get sensor data
when 38 => iJSENSOR <= iSENSOR ;
iSTATE <= conv_std_logic_vector(39,6) ;
-- <LANE_CHANGE> judge center
when 39 => if ( iJUDGE = SENSOR_CENTER ) then
iSTATE <= conv_std_logic_vector(40,6) ;
else
iSTATE <= conv_std_logic_vector(39,6) ;
end if ;
-- <LANE_CHANGE> turn
when 40 => iSTATE <= conv_std_logic_vector(41,6) ;
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- right_turn (reverse)
if ( iDIRECTION = 1 ) then
iSVRATE <= conv_std_logic_vector( 72,8);
iPWMRATE <= conv_std_logic_vector( 35,6);
-- left_white (reverse)
elsif ( iDIRECTION = 2 ) then
iSVRATE <= conv_std_logic_vector(128,8);
iPWMRATE <= conv_std_logic_vector( 35,6);
end if ;
-- <LANE_CHANGE> time up
when 41 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(42,6);
else
iSTATE <= conv_std_logic_vector(41,6);
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> center move
when 42 => iSTATE <= conv_std_logic_vector( 43,6);
iSVRATE <= conv_std_logic_vector(100,8); -- center
iPWMRATE <= conv_std_logic_vector( 35,6);
iTCNT <= 200 ; -- 2sec 100Hz = 10ms
-- <LANE_CHANGE> time up
when 43 => if ( iTCNT = 0 ) then
iSTATE <= conv_std_logic_vector(44,6);
else
iSTATE <= conv_std_logic_vector(43,6);
iTCNT <= iTCNT - 1 ;
end if ;
-- <LANE_CHANGE> return NORMAL
when 44 => iSTATE <= conv_std_logic_vector(10,6) ;
iDIRECTION <= "00" ; -- center
-- default
when others => iSTATE <= (others => '0') ;
end case ;
end if ;
end process ;
iMODE <= 0 when ( conv_integer(iSTATE) < 10 ) else -- BLIND_RUN
1 when ( 9 < conv_integer(iSTATE) and conv_integer(iSTATE) < 15 ) else -- NORMAL
2 when ( 13 < conv_integer(iSTATE) and conv_integer(iSTATE) < 30 ) else -- CRANK
3 ;
end Behavioral;
実際に走行させると、DCモータとサーボモータが同時に
動くとき、電力を一気に消費するため、制御性が悪く
なりました。
4.8Vに、5V出力DCDCコンバータを接続してから、制御
基板に出力し、安定性を上げました。
さらに、DCモータへの出力電圧を安定化するため
シリーズパスレギュレータを入れました。
パワートランジスタのベースエミッタ間電圧を0.6Vとして
OPアンプの電圧出力が、3.9V程度になるようにしました。
LM358は、出力電圧の最大値は5Vでは、3.5V程度になり
パワートランジスタのエミッタから2.9Vが出力される計算
です。電流容量を増やすため、２トランジスタ構成に。
シリーズパスレギュレータを入れた制御基板は以下です。
センサーボードを載せたマシンの全体は、以下となりました。
BarCodeScanner、GameBoyCameraを載せるために、ダイソーから
プラスチック製カードスタンドを入手。
カードスタンドは、そのままでも、ほどよい傾斜があるのと
自由に角度を変更できるので採用です。
目次
前
次