microzed_apo: Correct JX1_LVDS_21_N pin assignment on FPGA_IO header.

[fpga/zynq/canbench-sw.git] / system / ip / servo_led_ps2_1.0 / hdl / servo_led_ps2_v1_0.vhd

diff --git a/system/ip/servo_led_ps2_1.0/hdl/servo_led_ps2_v1_0.vhd b/system/ip/servo_led_ps2_1.0/hdl/servo_led_ps2_v1_0.vhd
index d331942707e12ed5b69cf1509f199973ce81fd4f..dafc3bd6ed7d90d42f925b3a1415f3b8d7f280d4 100644 (file)
--- a/system/ip/servo_led_ps2_1.0/hdl/servo_led_ps2_v1_0.vhd
+++ b/system/ip/servo_led_ps2_1.0/hdl/servo_led_ps2_v1_0.vhd
@@ -54,11 +54,17 @@ architecture arch_imp of servo_led_ps2_v1_0 is
        -- component declaration
        component servo_led_ps2_v1_0_S00_AXI is
                generic (
+               servo_pwm_width         : integer       := 24;
                C_S_AXI_DATA_WIDTH      : integer       := 32;
                C_S_AXI_ADDR_WIDTH      : integer       := 5
                );
                port (
                S_REG0          : out std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
+               servo_pwm_period: out std_logic_vector(servo_pwm_width-1 downto 0);
+               servo1_pwm_duty: out std_logic_vector(servo_pwm_width-1 downto 0);
+               servo2_pwm_duty: out std_logic_vector(servo_pwm_width-1 downto 0);
+               servo3_pwm_duty: out std_logic_vector(servo_pwm_width-1 downto 0);
+               servo4_pwm_duty: out std_logic_vector(servo_pwm_width-1 downto 0);
 
                S_AXI_ACLK      : in std_logic;
                S_AXI_ARESETN   : in std_logic;
@@ -84,18 +90,69 @@ architecture arch_imp of servo_led_ps2_v1_0 is
                );
        end component servo_led_ps2_v1_0_S00_AXI;
 
-       signal s_reg0           : std_logic_vector(32-1 downto 0);
+       component cnt_div is
+               generic (
+               cnt_width_g : natural := 4
+               );
+               port
+               (
+               clk_i     : in std_logic;                               --clk to divide
+               en_i      : in std_logic;                               --enable bit?
+               reset_i   : in std_logic;                               --asynch. reset
+               ratio_i   : in std_logic_vector(cnt_width_g-1 downto 0);--initial value
+               q_out_o   : out std_logic                               --generates puls when counter underflows
+               );
+       end component;
+
+       component pulse_gen is
+               generic (
+               duration_width_g : natural := 4
+               );
+               port (
+               clk_i      : in std_logic;                              --clk to divide
+               en_i       : in std_logic;                              --enable bit?
+               reset_i    : in std_logic;                              --asynch. reset
+               trigger_i  : in std_logic;                              --start to generate pulse
+               duration_i : in std_logic_vector(duration_width_g-1 downto 0);--duration/interval of the pulse
+               q_out_o    : out std_logic                              --generates pulse for given duration
+               );
+       end component;
+
+       constant servo_pwm_width : integer := 24;
+
+       signal servo_pwm_period: std_logic_vector(servo_pwm_width-1 downto 0);
+       signal servo1_pwm_duty: std_logic_vector(servo_pwm_width-1 downto 0);
+       signal servo2_pwm_duty: std_logic_vector(servo_pwm_width-1 downto 0);
+       signal servo3_pwm_duty: std_logic_vector(servo_pwm_width-1 downto 0);
+       signal servo4_pwm_duty: std_logic_vector(servo_pwm_width-1 downto 0);
 
+       signal fsm_clk : std_logic;
+       signal fsm_rst : std_logic;
+
+       signal pwm_cycle_start : std_logic;
+
+       signal s_reg0   : std_logic_vector(32-1 downto 0);
+
+       signal servo1_pwm: std_logic;
+       signal servo2_pwm: std_logic;
+       signal servo3_pwm: std_logic;
+       signal servo4_pwm: std_logic;
 begin
 
 -- Instantiation of Axi Bus Interface S00_AXI
 servo_led_ps2_v1_0_S00_AXI_inst : servo_led_ps2_v1_0_S00_AXI
        generic map (
+               servo_pwm_width         => servo_pwm_width,
                C_S_AXI_DATA_WIDTH      => C_S00_AXI_DATA_WIDTH,
                C_S_AXI_ADDR_WIDTH      => C_S00_AXI_ADDR_WIDTH
        )
        port map (
                S_REG0          => s_reg0,
+               servo_pwm_period => servo_pwm_period,
+               servo1_pwm_duty => servo1_pwm_duty,
+               servo2_pwm_duty => servo2_pwm_duty,
+               servo3_pwm_duty => servo3_pwm_duty,
+               servo4_pwm_duty => servo4_pwm_duty,
 
                S_AXI_ACLK      => s00_axi_aclk,
                S_AXI_ARESETN   => s00_axi_aresetn,
@@ -121,11 +178,78 @@ servo_led_ps2_v1_0_S00_AXI_inst : servo_led_ps2_v1_0_S00_AXI
        );
 
        -- Add user logic here
+cnt_div_inst: cnt_div
+       generic map (
+               cnt_width_g => servo_pwm_width
+       )
+       port map (
+               clk_i => fsm_clk,
+               en_i => '1',
+               reset_i => fsm_rst,
+               ratio_i =>servo_pwm_period,
+               q_out_o => pwm_cycle_start
+       );
+
+servo1_pwm_inst: pulse_gen
+       generic map (
+               duration_width_g => servo_pwm_width
+       )
+       port map (
+               clk_i => fsm_clk,
+               en_i => '1',
+               reset_i => fsm_rst,
+               trigger_i => pwm_cycle_start,
+               duration_i => servo1_pwm_duty,
+               q_out_o => servo1_pwm
+       );
+
+servo2_pwm_inst: pulse_gen
+       generic map (
+               duration_width_g => servo_pwm_width
+       )
+       port map (
+               clk_i => fsm_clk,
+               en_i => '1',
+               reset_i => fsm_rst,
+               trigger_i => pwm_cycle_start,
+               duration_i => servo2_pwm_duty,
+               q_out_o => servo2_pwm
+       );
+
+servo3_pwm_inst: pulse_gen
+       generic map (
+               duration_width_g => servo_pwm_width
+       )
+       port map (
+               clk_i => fsm_clk,
+               en_i => '1',
+               reset_i => fsm_rst,
+               trigger_i => pwm_cycle_start,
+               duration_i => servo3_pwm_duty,
+               q_out_o => servo3_pwm
+       );
+
+servo4_pwm_inst: pulse_gen
+       generic map (
+               duration_width_g => servo_pwm_width
+       )
+       port map (
+               clk_i => fsm_clk,
+               en_i => '1',
+               reset_i => fsm_rst,
+               trigger_i => pwm_cycle_start,
+               duration_i => servo4_pwm_duty,
+               q_out_o => servo4_pwm
+       );
 
-       SERVO1 <= s_reg0(0);
-       SERVO2 <= s_reg0(1);
-       SERVO3 <= s_reg0(2);
+       SERVO1 <= s_reg0(0) xor servo1_pwm;
+       SERVO2 <= s_reg0(1) xor servo2_pwm;
+       SERVO3 <= s_reg0(2) xor servo3_pwm;
+       SERVO4 <= s_reg0(3) xor servo4_pwm when s_reg0(8) = '1'
+                 else 'Z';
 
+       fsm_clk <= s00_axi_aclk;
+       fsm_rst <= not s00_axi_aresetn;
        -- User logic ends
 
 end arch_imp;