Books | Robot_Books | Fig_9

library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.my_package.all;

entity Funcmi is
    port (
        clk          : in std_logic;
        compecho     : in std_logic;
        comptrig     : out std_logic;
        ds_echo      : in std_logic;
        dstrig       : out std_logic;
        idle         : out std_logic;
        lengctrl1    : out std_logic;
        lengctrl2    : out std_logic;
        pwm          : out std_logic;
        rengctrl1    : out std_logic;
        rengctrl2    : out std_logic;
        rst          : in std_logic;
        start        : in std_logic
    );
end Funcmi;

architecture ARC_Funcmi of Funcmi is

    type FSMStates is (
        a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11, a12, a13, a14, a15, 
        a16, a17, a18, a19, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a30, 
        a31, a32, a33, a34
    );

    signal anglecnt : std_logic_vector(19 downto 0);
    signal anglediff : std_logic_vector(19 downto 0);
    signal comprg1 : std_logic_vector(19 downto 0);
    signal comprg2 : std_logic_vector(19 downto 0);
    signal comptrig_cnt : std_logic_vector(11 downto 0);
    signal currentState : FSMStates;
    signal distrg : std_logic_vector(19 downto 0);
    signal ds_cnt : std_logic_vector(19 downto 0);
    signal dstrig_cnt : std_logic_vector(11 downto 0);
    signal pwm_cnt : std_logic_vector(15 downto 0);
    signal speedrg : std_logic_vector(15 downto 0);
    signal stopflag : std_logic;

begin

    process (clk , rst)


    procedure proc_Funcmi is 
    begin


    case currentState is
    when a1 =>
        if (start) = '1' then
            dstrig <= '1';
            dstrig_cnt <= (others => '0');
            ds_cnt <= (others => '0');
            currentState <= a5;

        else
            currentState <= a1;
            idle <= '1';

        end if;

    when a2 =>
        
            stopflag <= '0';
            currentState <= a3;

    when a3 =>
        if (distrg > x"b71b0") then
            speedrg <= x"2bf2";
            currentState <= a8;

        elsif (not (distrg > x"b71b0") and distrg > x"3d090") then
            speedrg <= x"186a";
            currentState <= a8;

        elsif (not (distrg > x"b71b0") and not (distrg > x"3d090") and distrg > x"186a0") then
            speedrg <= x"0c35";
            currentState <= a8;

        else
            stopflag <= '1';
            currentState <= a7;

        end if;

    when a4 =>
        
            dstrig_cnt <= std_logic_vector(unsigned(dstrig_cnt) + 1);
            currentState <= a11;

    when a5 =>
        
            dstrig_cnt <= std_logic_vector(unsigned(dstrig_cnt) + 1);
            currentState <= a24;

    when a6 =>
        if (pwm_cnt < speedrg) then
            pwm_cnt <= std_logic_vector(unsigned(pwm_cnt) + 1);
            currentState <= a26;

        else
            pwm <= '0';
            currentState <= a25;

        end if;

    when a7 =>
        if (stopflag) = '1' then
            rengctrl1 <= '1';
            rengctrl2 <= '1';
            lengctrl1 <= '1';
            lengctrl2 <= '1';
            currentState <= a1;
            idle <= '1';

        else
            dstrig <= '1';
            dstrig_cnt <= (others => '0');
            ds_cnt <= (others => '0');
            currentState <= a4;

        end if;

    when a8 =>
        
            pwm_cnt <= (others => '0');
            pwm <= '1';
            currentState <= a6;

    when a9 =>
        if (ds_echo) = '1' then
            ds_cnt <= std_logic_vector(unsigned(ds_cnt) + 1);
            currentState <= a10;

        else
            currentState <= a9;

        end if;

    when a10 =>
        if (ds_echo) = '1' then
            ds_cnt <= std_logic_vector(unsigned(ds_cnt) + 1);
            currentState <= a10;

        else
            distrg <= ds_cnt;
            currentState <= a2;

        end if;

    when a11 =>
        if (dstrig_cnt = x"1f4") then
            dstrig <= '0';
            currentState <= a9;

        else
            dstrig_cnt <= std_logic_vector(unsigned(dstrig_cnt) + 1);
            currentState <= a11;

        end if;

    when a12 =>
        if (anglediff >= x"00003") then
            rengctrl1 <= '0';
            rengctrl2 <= '0';
            lengctrl1 <= '0';
            lengctrl2 <= '0';
            currentState <= a16;

        else
            rengctrl1 <= '1';
            rengctrl2 <= '0';
            lengctrl1 <= '0';
            lengctrl2 <= '1';
            currentState <= a20;

        end if;

    when a13 =>
        if (pwm_cnt < speedrg) then
            pwm_cnt <= std_logic_vector(unsigned(pwm_cnt) + 1);
            currentState <= a28;

        else
            pwm <= '0';
            currentState <= a27;

        end if;

    when a14 =>
        
            pwm_cnt <= (others => '0');
            pwm <= '1';
            currentState <= a13;

    when a15 =>
        if (distrg < x"186a0") then
            dstrig <= '1';
            dstrig_cnt <= (others => '0');
            ds_cnt <= (others => '0');
            currentState <= a4;

        else
            comptrig <= '1';
            comptrig_cnt <= (others => '0');
            anglecnt <= (others => '0');
            anglecnt <= (others => '0');
            currentState <= a21;

        end if;

    when a16 =>
        
            dstrig <= '1';
            dstrig_cnt <= (others => '0');
            ds_cnt <= (others => '0');
            currentState <= a5;

    when a17 =>
        
            anglediff <= std_logic_vector(unsigned(comprg2) - unsigned(comprg1));
            currentState <= a12;

    when a18 =>
        
            rengctrl1 <= '1';
            rengctrl2 <= '0';
            lengctrl1 <= '0';
            lengctrl2 <= '1';
            currentState <= a20;

    when a19 =>
        
            comptrig_cnt <= std_logic_vector(unsigned(comptrig_cnt) + 1);
            currentState <= a31;

    when a20 =>
        
            speedrg <= x"0c35";
            currentState <= a14;

    when a21 =>
        
            comptrig_cnt <= std_logic_vector(unsigned(comptrig_cnt) + 1);
            currentState <= a34;

    when a22 =>
        if (ds_echo) = '1' then
            ds_cnt <= std_logic_vector(unsigned(ds_cnt) + 1);
            currentState <= a23;

        else
            currentState <= a22;

        end if;

    when a23 =>
        if (ds_echo) = '1' then
            ds_cnt <= std_logic_vector(unsigned(ds_cnt) + 1);
            currentState <= a23;

        else
            distrg <= ds_cnt;
            currentState <= a15;

        end if;

    when a24 =>
        if (dstrig_cnt = x"1f4") then
            dstrig <= '0';
            currentState <= a22;

        else
            dstrig_cnt <= std_logic_vector(unsigned(dstrig_cnt) + 1);
            currentState <= a24;

        end if;

    when a25 =>
        
            pwm_cnt <= std_logic_vector(unsigned(pwm_cnt) + 1);
            currentState <= a26;

    when a26 =>
        if (pwm_cnt < x"30d4" and pwm_cnt < speedrg) then
            pwm_cnt <= std_logic_vector(unsigned(pwm_cnt) + 1);
            currentState <= a26;

        elsif (pwm_cnt < x"30d4" and not (pwm_cnt < speedrg)) then
            pwm <= '0';
            currentState <= a25;

        elsif (not (pwm_cnt < x"30d4") and (stopflag) = '1') then
            rengctrl1 <= '1';
            rengctrl2 <= '1';
            lengctrl1 <= '1';
            lengctrl2 <= '1';
            currentState <= a1;
            idle <= '1';

        else
            dstrig <= '1';
            dstrig_cnt <= (others => '0');
            ds_cnt <= (others => '0');
            currentState <= a4;

        end if;

    when a27 =>
        
            pwm_cnt <= std_logic_vector(unsigned(pwm_cnt) + 1);
            currentState <= a28;

    when a28 =>
        if (pwm_cnt < x"30d4" and pwm_cnt < speedrg) then
            pwm_cnt <= std_logic_vector(unsigned(pwm_cnt) + 1);
            currentState <= a28;

        elsif (pwm_cnt < x"30d4" and not (pwm_cnt < speedrg)) then
            pwm <= '0';
            currentState <= a27;

        else
            comptrig <= '1';
            comptrig_cnt <= (others => '0');
            anglecnt <= (others => '0');
            anglecnt <= (others => '0');
            currentState <= a19;

        end if;

    when a29 =>
        if (compecho) = '1' then
            anglecnt <= std_logic_vector(unsigned(anglecnt) + 1);
            currentState <= a30;

        else
            currentState <= a29;

        end if;

    when a30 =>
        if (compecho) = '1' then
            anglecnt <= std_logic_vector(unsigned(anglecnt) + 1);
            currentState <= a30;

        else
            comprg2 <= anglecnt;
            currentState <= a17;

        end if;

    when a31 =>
        if (comptrig_cnt = x"1f4") then
            comptrig <= '0';
            currentState <= a29;

        else
            comptrig_cnt <= std_logic_vector(unsigned(comptrig_cnt) + 1);
            currentState <= a31;

        end if;

    when a32 =>
        if (compecho) = '1' then
            anglecnt <= std_logic_vector(unsigned(anglecnt) + 1);
            currentState <= a33;

        else
            currentState <= a32;

        end if;

    when a33 =>
        if (compecho) = '1' then
            anglecnt <= std_logic_vector(unsigned(anglecnt) + 1);
            currentState <= a33;

        else
            comprg1 <= anglecnt;
            currentState <= a18;

        end if;

    when a34 =>
        if (comptrig_cnt = x"1f4") then
            comptrig <= '0';
            currentState <= a32;

        else
            comptrig_cnt <= std_logic_vector(unsigned(comptrig_cnt) + 1);
            currentState <= a34;

        end if;

    end case;
    end proc_Funcmi;

    begin
        if (rst = '1') then
            anglecnt <= (others => '0');
            anglediff <= (others => '0');
            comprg1 <= (others => '0');
            comprg2 <= (others => '0');
            comptrig <= '0';
            comptrig_cnt <= (others => '0');
            distrg <= (others => '0');
            ds_cnt <= (others => '0');
            dstrig <= '0';
            dstrig_cnt <= (others => '0');
            idle <= '0';
            lengctrl1 <= '0';
            lengctrl2 <= '0';
            pwm <= '0';
            pwm_cnt <= (others => '0');
            rengctrl1 <= '0';
            rengctrl2 <= '0';
            speedrg <= (others => '0');
            stopflag <= '0';

            currentState <= a1;
            idle <= '1';

        elsif (clk'event and clk = '1') then
            idle <= '0';
            proc_Funcmi;
        end if;
    end process;

end ARC_Funcmi;