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

---- Uncomment the following library declaration if instantiating
---- any Xilinx primitives in this code.
--library UNISIM;
--use UNISIM.VComponents.all;

entity top is
    Port ( clk : in  STD_LOGIC;
           servo : out  STD_LOGIC_VECTOR (17 downto 0);
           led : out STD_LOGIC_VECTOR(7 downto 0) 
	 );
end top;
 
 
architecture Behavioral of top is
   signal counter : std_logic_vector(4 downto 0) := (others=>'0'); -- runs to 50 to get a usec clk at 50 MHz chip clock
	signal clk_usec : std_logic := '0'; -- one rising edge per usec
	signal counter_usec_20ms : std_logic_vector(14 downto 0) := (others=>'0'); -- counts to  20 000
   signal counter_usec_1sec : std_logic_vector(20 downto 0) := (others=>'0'); -- counts to 500 000
	signal clk_sec : std_logic := '0';
	signal counter_sec : std_logic_vector(7 downto 0); -- counts to 256
	signal servo0pulse : std_logic_vector(10 downto 0) := "10111011100"; -- pulse width-1000: 1000 to 2023 ms
	signal servo1pulse : std_logic_vector(10 downto 0) := "10111011100"; -- pulse width-1000: 1000 to 2023 ms
	signal servo2pulse : std_logic_vector(10 downto 0) := "10111011100"; -- pulse width-1000: 1000 to 2023 ms
	
begin

   count_50mhz : process (clk) is
	begin
	   if(clk'event and clk='1') then
		   counter<=counter+1;
			if(counter=25) then
			   counter<=(others=>'0');
				if(clk_usec='1') then
				   clk_usec<='0';
				else
				   clk_usec<='1';
				end if;
			end if;
		end if;
	end process;
	
	count_ms : process(clk_usec) is
	begin
	   if(clk_usec'event and clk_usec='1') then
         counter_usec_20ms<=counter_usec_20ms+1;
			if(counter_usec_20ms=20000) then
			   counter_usec_20ms<=(others=>'0');
			end if;
			counter_usec_1sec<=counter_usec_1sec+1;
			if(counter_usec_1sec=500000) then
			   counter_usec_1sec<=(others=>'0');
				if(clk_sec='0') then
				   clk_sec<='1';
				else
				   clk_sec<='0';
				end if;
			end if;
		end if;
	end process;

	count_sec : process(clk_sec) is
	begin
	   if(clk_sec'event and clk_sec='1') then
		   counter_sec<=counter_sec+1;
			servo2pulse<=servo2pulse-4;
			if(servo2pulse<1200) then
			   servo2pulse<="10111011100";
		   end if;
		end if;
	end process;

   servo0 : process (counter_usec_20ms,servo0pulse) is
	begin
	   if(counter_usec_20ms<"01111101000") then
		   servo(0)<='1'; 
	   elsif(counter_usec_20ms<servo0pulse) then
		   servo(0)<='1';
		else
		   servo(0)<='0';
		end if;
	end process;
	
	servo1 : process (counter_usec_20ms,servo1pulse) is
	begin
	   if(counter_usec_20ms<"01111101000") then
		   servo(1)<='1'; 
	   elsif(counter_usec_20ms<servo1pulse) then
		   servo(1)<='1';
		else
		   servo(1)<='0';
		end if;
	end process;
	
	servo2 : process (counter_usec_20ms,servo2pulse) is
	begin
	   if(counter_usec_20ms<"01111101000") then
		   servo(2)<='1'; 
	   elsif(counter_usec_20ms<servo2pulse) then
		   servo(2)<='1';
		else
		   servo(2)<='0';
		end if;
	end process;
	
	
   servo(17 downto 3)<=(others=>'0');

   led(7 downto 0)<=counter_sec(7 downto 0);

end Behavioral;