Lab4_DeanNguyen

--Author: Dean Nguyen

LIBRARY ieee;
USE ieee.std_logic_1164.ALL;
USE ieee.std_logic_unsigned.ALL;
USE ieee.numeric_std.ALL;

Entity servo_controller IS
    PORT(
        clk                 : in std_logic;                             -- 50 Mhz system clock
        reset_n             : in std_logic;                             -- active low system rese
        write                   : in std_logic;                             -- active high write enable
        writedata           : in std_logic_vector(31 downto 0); --data from the CPU to be stored in the component
        address             : in std_logic;                             --address of register to be written to (from CPU)

        out_wave_export : out std_logic;
        irq                     : out std_logic                         --signal to interrupt the processor
        );
End Entity servo_controller;

Architecture behavioral of servo_controller is

    Type servo_loc is array(1 downto 0) of std_logic_vector(31 downto 0);
    Signal Registers : servo_loc;

    Type state_type is (Sweep_Right, Int_Right, Sweep_Left, Int_Left);
    Signal Current_state, Next_state : state_type;

    Signal out_wave : std_logic;

    constant FORTY_FIVE             : std_logic_vector(31 downto 0) := x"0000C350";
    constant ONE_THIRTY_FIVE    : std_logic_vector(31 downto 0) := x"000186A0";

    signal min_angle_count  : std_logic_vector(31 downto 0);
    signal max_angle_count  : std_logic_vector(31 downto 0);
    signal angle_count      : std_logic_vector(31 downto 0);
    signal pulse_count      : integer;
    signal servo_direction  : integer;

Begin

    out_wave_export <= out_wave;

    --this process loads data from the CPU.  The CPU provides the address,
    --the data and the write enable signal
    PROCESS(clk, reset_n, min_angle_count, max_angle_count, angle_count)
    BEGIN
        IF (reset_n = '0') THEN
            --Registers <= (OTHERS => "00000000000000000000000000000000");
            Registers(0) <= FORTY_FIVE;     -- 50000     (45 degrees)
            Registers(1) <= ONE_THIRTY_FIVE;    -- 100000 (135 degrees)
        ELSIF (clk'event AND clk = '1') THEN
            IF (write = '1') THEN
                if (address = '0') then
                    Registers(0) <= writedata;
                elsif (address = '1') then
                    Registers(1) <= writedata;
                end if;
            --Registers(to_integer(unsigned(address))) <= writedata;
            --when write enable is active, the ram location at the given address
            --is loaded with the input data
            END IF;
        END IF;
        min_angle_count <= Registers(0); -- sets value of min angle
        max_angle_count <= Registers(1); -- sets value of max angle
    END PROCESS;

    counter_proc : process(clk, reset_n, out_wave, pulse_count, angle_count, max_angle_count, min_angle_count, servo_direction)
    begin
        if(reset_n = '0') then
            angle_count <= min_angle_count;     -- sets the angle count to value of min angle
            servo_direction <= 0;
            pulse_count <= 0;
        elsif(rising_edge(clk) AND clk = '1') then
            pulse_count <= pulse_count + 1;
            if(500000 = pulse_count) then
                if((angle_count <= max_angle_count) and (servo_direction = 0)) then
                    angle_count <= angle_count + 10000;
                    if(angle_count = max_angle_count) then
                        servo_direction <= 1;
                    end if;
                elsif((angle_count >= min_angle_count) and (servo_direction = 1)) then
                    angle_count <= angle_count - 10000;
                    if(angle_count = min_angle_count) then
                        servo_direction <= 0;
                    end if;
                end if;
                pulse_count <= 0;
            end if;
        end if;

        if(pulse_count < angle_count) then
            out_wave <= '1';
        else
            out_wave <= '0';
        end if;
    end process;

    sync_states : Process(clk,reset_n)
    begin
        If (reset_n = '0') then
            Current_state <= Sweep_Right;
        Elsif (rising_edge(clk)) then
            Current_state <= Next_state;
        End If;
    end process;

    state_order : process(write, angle_count, max_angle_count, min_angle_count)
    begin
        Case(Current_state) is
            When Sweep_Right =>
                If(angle_count >= max_angle_count) then
                    Next_state <= Int_Right;
                else
                    Next_state <= Sweep_Right;
                End if;
            When Int_Right =>
                If(write = '1') then
                    Next_state <= Sweep_Left;
                Else
                    Next_state <= Int_Right;
                End if;
            When Sweep_Left =>
                If(angle_count <= min_angle_count) then
                    Next_state <= Int_Left;
                Else
                    Next_state <= Sweep_Left;
                End if;
            When Int_Left =>
                If(write = '1') then
                    Next_state <= Sweep_Right;
                Else
                    Next_state <= Int_Left;
                End if;
        End Case;
    end process;

    state_do : Process(Next_state, reset_n, clk)
    begin
        if(rising_edge(clk)) then
            Case(Next_state) is
                When(Sweep_Right) =>
                    irq <= '0';
                When(Int_Right) =>
                    irq <= '1';
                When(Sweep_Left) =>
                    irq <= '0';
                When(Int_Left) =>
                    irq <= '1';
            End Case;
        end if;
    end process;

End Architecture behavioral;