目次 前 次

---

ライントレーサ

　ライントレーサは、床に描いた線に追従して
　移動するロボット。

　ライントレーサのブロック図は、以下。



　利用するシャーシは、２個のDCモータを持ったメカ。



　センサーは、以下を３個使います。



　ライントレーサは、次のシーケンスで動作することにします。

路面情報取得
路面情報からモータに与えるDUTY比計算
DUTY比を利用してPWM波形生成
PWM波形をモータドライバに与える
１に戻る

　モータドライバは、次のハーフブリッジICを１個
　用意して、２個のモータの面倒を見ることに。



　ハーフブリッジICは、以下のように使います。

1,2ENと3,4ENはCPLDからの１本の制御信号を接続
1A、4AにCPLDから、それぞれPWM波形を与える
2A、3AはGNDと接続
1Y、2Yを左モータに接続
4Y、3Yを右モータに接続

　左右のＡ端子は、(1,0)か(0,0)の論理値を与えられる
　ので、PWM波形のDUTY比に応じてモータ回転数が確定
　します。

　EN端子を１本でまとめると、CPLDがモータの
　回転を１信号で制御できる仕様になります。

　VHDLコードで内部回路を定義していきます。

  センサー情報取得

　　３個のセンサーから、論理値が出力されてくる
　　仕様で考えると、レジスタに論理値を記憶する
　　処理でよいはず。

  -- latch sensor data
  -- iACLK = 1kHz
  process ( nRESET , iACLK )
  begin
    if ( nRESET = '0' ) then
      iSREG <= "000" ;
    elsif rising_edge( iACLK ) then
      iSREG <= SENSOR ;
    end if ;
  end process ;

　　記憶した情報は、シーケンサで処理。

  PWM波形生成

　　PWM波形は、カウンタとレジスタの値を比較して
　　論理値の'1'か'0'を出力すれば、生成できます。

  -- output
  Lout <= iLout ;
  Rout <= iRout ;

  -- generate PWM wave
  iLout <= '1' when ( iPCNT < iLPWMX ) else '0' ;
  iRout <= '1' when ( iPCNT < iRPWMX ) else '0' ;

　　Lout、Routは、ハーフブリッジICの1A、4Aに接続。

　　カウンタはiPCNTが担当し、０から99までを繰り返し。
　　レジスタiLPWMX、iRPWMXは、左と右のDUTY比を格納
　　しているとします。

　　シーケンサは、レジスタiLPWM、iRPWMにDUTY比を
　　設定するとすれば、カウンタの処理は、次の定義
　　でよいはず。

  -- counter
  -- iACLK = 1kHz
  process ( nRESET , iACLK )
  begin
    if ( nRESET = '0' ) then
      iPCNT  <= 0 ;
      iLPWMX <= 0 ;
      iRPWMX <= 0 ;
    elsif rising_edge( iACLK ) then
      if ( iPCNT = 100 ) then
        -- clear
        iPCNT <= 0 ;
        -- copy
        iLPWMX <= iLPWM ;
        iRPWMX <= iRPWM ;
      else
        iPCNT <= iPCNT + 1 ;
      end if ;
    end if ;
  end proc

　ここまで定義すれば、残りはシーケンサの中で
　センサー情報に応じた左右のDUTY比を設定する
　だけになります。

　操作しやすいように、電源スイッチの他に
　動作指定スイッチを用意して対応します。

　動作指定スイッチを使うと、IDLEかRUNの
　状態を設定できます。

　動作設定スイッチには、トグルスイッチを利用。




　スイッチの状態を、CPLDで記憶し、同時に
　ハーフブリッジICに出力します。

　VHDLコードでは、以下。

  -- output
  ICENA <= iENA ;

  -- state sensing
  -- iBCLK = 1Hz
  process ( nRESET , iBCLK )
  begin
    if ( nRESET = '0' ) then
      iENA <= '0' ;
    elsif rising_edge( iBCLK ) then
      iENA <= XENA ;
    end if ;
  end proc

　クロックは、1kHzと1Hzなので、分周して生成。
　ブロック図では、以下。



　VHDLコードは、次のように定義。

  -- divider
  process ( nRESET , CLOCK )
  begin
    if ( nRESET = '0' ) then
      iACNT <= 0 ;
    elsif rising_edge( CLOCK ) then
      if ( iACNT = ACNT_MAX ) then
        iACNT <= 0 ;
      else
        iACNT <= iACNT + 1 ;
      end if ;
    end if ;
  end process ;
  iACLK <= '1' when ( iACNT < ACNT_HALF ) else '0' ;

  process ( nRESET , iACLK )
  begin
    if ( nRESET = '0' ) then
      iBCNT <= 0 ;
    elsif rising_edge( iACLK ) then
      if ( iBCNT = BCNT_MAX ) then
        iBCNT <= 0 ;
      else
        iBCNT <= iBCNT + 1 ;
      end if ;
    end if ;
  end process ;
  iBCLK <= '1' when ( iBCNT < BCNT_HALF ) else '0' ;

　シーケンサの状態遷移図は、次のようにすればよいはず。



　４状態があるので、２ビットジョンソンカウンタをベースに
　した、シーケンサで処理できます。

　骨格だけを定義します。

  -- sequencer
  process ( nRESET , iBCLK )
  begin
    if ( nRESET = '0' ) then
      iSEQ <= "00" ;
    elsif rising_edge( iBCLK ) then
      case iSEQ is
        -- idle
        when "00" => iSEQ <= "01" ;
        -- get sensor data
        when "01" => iSEQ <= "11" ;
        -- calculate duty ratios
        when "11" => iSEQ <= "10" ;
        -- transfer duty ratios
        when "10" => iSEQ <= "00" ;
        -- default
        when others => iSEQ <= "00" ;
      end case ;
    end if ;
  end process ;

　路面センサーからの論理値をコピー。

  -- sequencer
  process ( nRESET , iBCLK )
  begin
    if ( nRESET = '0' ) then
      iSEQ  <= "00"  ;
      iSENX <= "000" ;
    elsif rising_edge( iBCLK ) then
      case iSEQ is
        -- idle
        when "00" => iSEQ <= "01" ;
        -- get sensor data
        when "01" => iSEQ  <= "11" ;
                     iSENX <= iSREG ;
        -- calculate duty ratios
        when "11" => iSEQ <= "10" ;
        -- transfer duty ratios
        when "10" => iSEQ <= "00" ;
        -- default
        when others => iSEQ <= "00" ;
      end case ;
    end if ;
  end process ;

　DUTY比は、センサー値から決定されます。
　３ビットなので、０から７に対応した
　iLPWM、iRPWMの値を格納。

  -- sequencer
  process ( nRESET , iBCLK )
  begin
    if ( nRESET = '0' ) then
      iSEQ  <= "00"  ;
      iSENX <= "000" ;
      iLPWM <= 50 ;
      iRPWM <= 50 ;
    elsif rising_edge( iBCLK ) then
      case iSEQ is
        -- idle
        when "00" => iSEQ <= "01" ;
        -- get sensor data
        when "01" => iSEQ  <= "11" ;
                     iSENX <= iSREG ;
        -- calculate duty ratios
        when "11" => iSEQ <= "10" ;
                     if ( iSENX = "001" ) then
                       iLPWM <= 50 ;
                       iRPWM <= 60 ;
                     elsif ( iSENX = "011" ) then
                       iLPWM <= 40 ;
                       iRPWM <= 60 ;
                     elsif if ( iSENX = "100" ) then
                       iLPWM <= 60 ;
                       iRPWM <= 50 ;
                     elsif if ( iSENX = "110" ) then
                       iLPWM <= 60 ;
                       iRPWM <= 40 ;
                     else 
                       iLPWM <= 50 ;
                       iRPWM <= 50 ;
                     end if ;
        -- transfer duty ratios
        when "10" => iSEQ <= "00" ;
        -- default
        when others => iSEQ <= "00" ;
      end case ;
    end if ;
  end process ;

　シーケンサまで定義したので、まとめます。

library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;

entity xmcr is
  Port (
    -- system 
    nRESET : in  std_logic ;
    CLOCK  : in  std_logic ; -- 4MHz
    -- sensor inputs
    SENSOR : in  std_logic_vector(2 downto 0) ;
    -- state input
    XENV   : in  std_logic ;
    -- state output
    ICENA  : out std_logic ;
    -- output
    Lout   : out std_logic ;
    Rout   : out std_logic --;
  );
end xmcr;

architecture Behavioral of xmcr is
  --
  CONSTANT ACNT_MAX  : integer :=  3999 ;
  CONSTANT BCNT_MAX  : integer :=   999 ;
  CONSTANT ACNT_HALF : integer :=  2000 ;
  CONSTANT BCNT_HALF : integer :=   500 ;
  CONSTANT LAST      : integer :=   100 ;
  -- divider
  signal iACNT  : integer range 0 to ACNT_MAX ;
  signal iACLK  : std_logic ;
  signal iBCNT  : integer range 0 to BCNT_MAX ;
  signal iBCLK  : std_logic ;
  -- sensor
  signal iSREG  : std_logic_vector(2 downto 0) ;
  -- latch
  signal iENV   : std_logic ;
  -- PWM
  signal iPCNT  : integer range 0 to LAST ;
  signal iLout  : std_logic ;
  signal iRout  : std_logic ;
  signal iLPWMX : integer range 0 to LAST ;
  signal iLPWM  : integer range 0 to LAST ;
  signal iRPWMX : integer range 0 to LAST ;
  signal iRPWM  : integer range 0 to LAST ;
  -- sequencer
  signal iSEQ   : std_logic_vector(1 downto 0) ;
  signal iSENX  : std_logic_vector(2 downto 0) ;
begin
  -- output
  Lout  <= iLout ;
  Rout  <= iRout ;
  ICENA <= iENA ;

  -- divider
  process ( nRESET , CLOCK )
  begin
    if ( nRESET = '0' ) then
      iACNT <= 0 ;
    elsif rising_edge( CLOCK ) then
      if ( iACNT = ACNT_MAX ) then
        iACNT <= 0 ;
      else
        iACNT <= iACNT + 1 ;
      end if ;
    end if ;
  end process ;
  iACLK <= '1' when ( iACNT < ACNT_HALF ) else '0' ;

  process ( nRESET , iACLK )
  begin
    if ( nRESET = '0' ) then
      iBCNT <= 0 ;
    elsif rising_edge( iACLK ) then
      if ( iBCNT = BCNT_MAX ) then
        iBCNT <= 0 ;
      else
        iBCNT <= iBCNT + 1 ;
      end if ;
    end if ;
  end process ;
  iBCLK <= '1' when ( iBCNT < BCNT_HALF ) else '0' ;

  -- latch sensor data
  process ( nRESET , iACLK )
  begin
    if ( nRESET = '0' ) then
      iSREG <= "000" ;
    elsif rising_edge( iACLK ) then
      iSREG <= SENSOR ;
    end if ;
  end process ;

  -- generate PWM wave
  iLout <= '1' when ( iPCNT < iLPWMX ) else '0' ;
  iRout <= '1' when ( iPCNT < iRPWMX ) else '0' ;

  -- counter
  process ( nRESET , iACLK )
  begin
    if ( nRESET = '0' ) then
      iPCNT  <= 0 ;
      iLPWMX <= 0 ;
      iRPWMX <= 0 ;
    elsif rising_edge( iACLK ) then
      if ( iPCNT = 100 ) then
        -- clear
        iPCNT <= 0 ;
        -- copy
        iLPWMX <= iLPWM ;
        iRPWMX <= iRPWM ;
      else
        iPCNT <= iPCNT + 1 ;
      end if ;
    end if ;
  end proc

  -- state sensing
  process ( nRESET , iBCLK )
  begin
    if ( nRESET = '0' ) then
      iENA <= '0' ;
    elsif rising_edge( iBCLK ) then
      iENA <= XENA ;
    end if ;
  end proc

  -- sequencer
  process ( nRESET , iBCLK )
  begin
    if ( nRESET = '0' ) then
      iSEQ  <= "00"  ;
      iSENX <= "000" ;
      iLPWM <= 50 ;
      iRPWM <= 50 ;
    elsif rising_edge( iBCLK ) then
      case iSEQ is
        -- idle
        when "00" => iSEQ <= "01" ;
        -- get sensor data
        when "01" => iSEQ  <= "11" ;
                     iSENX <= iSREG ;
        -- calculate duty ratios
        when "11" => iSEQ <= "10" ;
                     if ( iSENX = "001" ) then
                       iLPWM <= 50 ;
                       iRPWM <= 60 ;
                     elsif ( iSENX = "011" ) then
                       iLPWM <= 40 ;
                       iRPWM <= 60 ;
                     elsif if ( iSENX = "100" ) then
                       iLPWM <= 60 ;
                       iRPWM <= 50 ;
                     elsif if ( iSENX = "110" ) then
                       iLPWM <= 60 ;
                       iRPWM <= 40 ;
                     else 
                       iLPWM <= 50 ;
                       iRPWM <= 50 ;
                     end if ;
        -- transfer duty ratios
        when "10" => iSEQ <= "00" ;
        -- default
        when others => iSEQ <= "00" ;
      end case ;
    end if ;
  end process ;

end Behavioral;

　ピンアサインは、以下。

# system
NET "nRESET" loc = "P39" ;
NET "CLOCK"  loc = "P5"  ;

# sensor
NET "SENSOR<0>"  loc = "P1" ;
NET "SENSOR<1>"  loc = "P2" ;
NET "SENSOR<2>"  loc = "P3" ;

# state control
NET "XENV" loc = "P4" ;

# half bridge control
NET "ICENA" loc = "P11" ;
NET "Lout"  loc = "P12" ;
NET "Rout"  loc = "P13" ;

　ライントレーサ実現に、XC9572、SN75441ONEの
　２個のICでまとめられることが、わかります。

---

目次 前 次