-- Este archivo, dado un angulo en "sangulo" de 0 a 9

-- pondr el servomotor en ese angulo. (poniendo write en '1')

-- ATENCION: Se usa informacion de esta pagina
-- http://www.boletin.upiita.ipn.mx/index.php/ciencia/219-cyt-numero-37/64-control-
automatico-de-tres-posiciones-de-un-servomotor-utilizando-vhdl-y-la-tarjeta-de-desarrollo-nexys-
ii
-- Segn el tutorial, debe haber un PWM con frecuencia de 100Hz (10ms )
-- Y el ngulo 0 corresponde a un ancho de pulso de 1ms
-- Y el ngulo 180 corresponde a un ancho de pulso de 2ms
-- Si esta informacin es falsa de acuerdo a su servomotor
-- Siempre puede adaptar las constantes
-- Lea mas abajo para saber los parametros


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


entity far is
generic(
-- limit_con formula: 50MHz/Frec_servo
limit_con : std_logic_vector(19 downto 0) := x"7A120"; -- Para 100hz
-- Angle formula : 50M*time_angle(sangulo)
-- Time_angle formula (para servos que sirva con 100Khz de referencia!)
-- time_angle(sangulo) = sangulo/180*1e-3 + 1e-3
-- Para la pereza:
-- Angle formula : 50M*(sangulo/180*1e-3 + 1e-3)

far_0 : integer := 50000; -- = 0 (time_angle = 1ms)
far_1 : integer := 55555; -- = 20 (El que no le importa)
far_2 : integer := 61111; -- = 40 (El asalta tumbas)
far_3 : integer := 66666; -- = 60 (EL DEMONIO!)
far_4 : integer := 72222; -- = 80 (El testigo de jehova)
far_5 : integer := 77777; -- = 100 (EL DIOS!)
far_6 : integer := 83333; -- = 120 (El pedofilo)
far_7 : integer := 88888; -- = 140 (El gordo)
far_8 : integer := 94444; -- = 160 (El mujeriego)
far_9 : integer := 100000 -- = 180 (time_angle = 2ms)

);
Port ( sangulo : in STD_LOGIC_VECTOR (3 downto 0);
write : in STD_LOGIC;
clock : in STD_LOGIC;
reset : in STD_LOGIC;
pulso_servo : out STD_LOGIC);
end far;

architecture Behavioral of far is

signal con : std_logic_vector(19 downto 0);
signal comparator : integer;
signal sangulos : STD_LOGIC_VECTOR (3 downto 0);
begin

-- Escribe el angulo
process(clock,reset)
begin
if reset = '1' then
sangulos <= x"0";
elsif clock'event and clock='1' then
if write='1' then
sangulos <= sangulo;
end if;
end if;
end process;

-- El que escoge los angulos
with sangulos select
comparator <= far_0 when x"0",
far_1 when x"1",
far_2 when x"2",
far_3 when x"3",
far_4 when x"4",
far_5 when x"5",
far_6 when x"6",
far_7 when x"7",
far_8 when x"8",
far_9 when x"9",
0 when others;

-- Contador maestro! (Suele llamarse "Rampa")
process(clock,reset)
begin
if reset='1' then
con <= (others => '0');
elsif clock'event and clock='1' then
if con = limit_con then
con <= (others => '0');
else
con <= con+1;
end if;
end if;
end process;

-- El "PWM"
-- No es tecnicamente un PWM, porque no se modula el ancho.
-- Mas bien tiene tiempos especificos para cada angulo.
-- Pero... el algoritmo de PWM debe aplicarse
-- Algoritmo: Rampa + comparador
pulso_servo <= '1' when con < comparator else '0';

end Behavioral;