library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_arith.all;
use ieee.std_logic_unsigned.all;

entity motor_control is
 port (clock_25Mhz,abort: in std_logic;
       contrr, contrl : in std_logic_vector(2 downto 0);
       lmotor, rmotor: out std_logic);
end motor_control;

architecture a of motor_control is
 signal count_motor,lengthr,lengthl: std_logic_vector(14 downto 0);
 SIGNAL	count_1Mhz: STD_LOGIC_VECTOR(4 DOWNTO 0);
 signal enablel,enabler,clock_1MHz,clock_1Mhz_int : std_logic;

 begin
  	PROCESS 
	BEGIN
-- Divide by 25
		WAIT UNTIL clock_25Mhz'EVENT and clock_25Mhz = '1';
			IF count_1Mhz < 24 THEN
				count_1Mhz <= count_1Mhz + 1;
			ELSE
				count_1Mhz <= "00000";
			END IF;
			IF count_1Mhz < 12 THEN
				clock_1Mhz_int <= '0';
			ELSE
				clock_1Mhz_int <= '1';
			END IF;	

-- Ripple clocks are used in this code to save prescalar hardware
-- Sync all clock prescalar outputs back to master clock signal
			clock_1Mhz <= clock_1Mhz_int;
	END PROCESS;

  process
   begin
   
    wait until clock_1Mhz'event and clock_1Mhz='1'; 
    --set clock!!!!!!!!!
   
    lmotor<='0';
    rmotor<='0';

    --left motor

    if contrl="011" or contrl="100" then
     --stop
     enablel<='0';
    end if;

    if contrl="110" or contrl="101" then
     --fwd 1
     lengthl<="000010101100011"; --1380
     enablel<='1';
    end if;

    if contrl="111" then
     --fwd 2
     lengthl<="000010111011100"; --1500
     enablel<='1';
    end if;

    if contrl="001" or contrl="010" then
     --bck 1
     lengthl<="000010100000000"; --1280
     enablel<='1';
    end if;

    if contrl="000" then
     --bck 2
     lengthl<="000001111101000"; --1000
     enablel<='1';
    end if;

    --right motor
    
    if contrr="011" or contrr="100" then
     --stop
     enabler<='0';
    end if;

    if contrr="110" or contrr="101" then
     --fwd 1
     lengthr<="000010011001001"; --1225
     enabler<='1';
    end if;

    if contrr="111" then
     --fwd 2
     lengthr<="000001111101000"; --1000
     enabler<='1';
    end if;

    if contrr="001" or contrr="010" then
     --bck 1
     lengthr<="000010100101001"; --1321  
     enabler<='1';
    end if;

    if contrr="000" then
     --bck 2
     lengthr<="000010111011100"; --1500
     enabler<='1';
    end if;


    --count motor is a 20000 us timer

    if count_motor/="100111000100000" then
     count_motor<=count_motor+1;
    else
     count_motor<="000000000000000";
    end if;

    --rmotor pulsing

    if count_motor<=lengthr and enabler='1' and abort='0' then
     rmotor<='1';
    else
     rmotor<='0';
    end if;

    --lmotor pulsing

    if count_motor<=lengthl and enablel='1' and abort='0' then
     lmotor<='1';
    else
     lmotor<='0';
    end if;

  end process;
end a;