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;