# Servo Driver
This module takes an analog input (perhaps from a PID Controller in an adjacent slot) and converts it to a 50Hz pulse train, suitable for position control of common hobby servo motors.
The top-level module creates two counter blocks, one that governs the rate of the pulses and the other that ticks at the time resolution of the pulse width. The counters divide the input clock by
2^(EXPONENT) / Increment, i.e.
2^24 / 3 giving roughly 50Hz from the Pro's 312.5MHz clock for the overall pulse timer, and
2^15 / 107 giving roughly 2048 steps within a 2ms pulse.
The output is nominally a digital signal, with the bit values corresponding to
low voltages defined as constants,
0x7FF corresponding to zero and maximum DAC output range respectively.
This example is available on Gitlab (opens new window).
The example is configured for Moku:Pro's 312.5MHz clock. The divider ratios should be changed before you attempt to run this on another platform.
# Top-Level Module
library IEEE; use IEEE.Std_Logic_1164.all; use IEEE.Numeric_Std.all; library Moku; use Moku.Support.ScaleOffset; use Moku.Support.clip; -- This design implements a Pulse Width Modulator which translates -- InputA into a pulse train suitable for controlling a standard Servo. -- -- The input can span the entire 16 bit range and is conditioned by -- a DSP block with Controls 1 and 2. -- Control0: gain of InputA, signed(15 downto 0) -- Control1: offset of InputA, signed(15 downto 0) -- -- OutputA will be a ~50Hz pulse train with the width of each pulse -- modulated from 500usS to 2500uS depending on InputA. The output -- amplitude is set from 0 to +MAX. -- -- This is designed for Moku:Pro's 312.5MHz clock. The clock dividers -- below will need to be changed when running on other platforms. -- -- The resolution of the pulse width is 2048 increments over the -- 2ms span. -- -- When output to a DAC, this should be appropriate for a standard -- servo. architecture Behavioural of CustomWrapper is constant HI_LVL : signed(15 downto 0) := x"7FFF"; constant LO_LVL : signed(15 downto 0) := x"0000"; signal Value : signed(12 downto 0); -- The scaled input signal Count : unsigned(12 downto 0); -- Timer for pulse width signal Pulse50Hz : std_logic; -- Strobe at 50Hz to start each pulse signal Pulse : std_logic; -- Strobe we can count to set the width of each pulse begin -- Adjust the gain and offset of InputA, and output to Value INPUT_SCALE: ScaleOffset port map ( Clk => Clk, Reset => Reset, X => InputA, Scale => signed(Control0(15 downto 0)), -- 1024 Offset => signed(Control1(15 downto 0)), -- 1024 Z => Value, Valid => Pulse50Hz, OutValid => open ); -- These two Counters generate out Pulse signals by dividing -- our system clock (312.5MHz on Moku:Pro). The divisor is -- 2^(EXPONENT) / Increment and is used to generate both the -- rate at which servo pulses are output, and the ticks within -- a pulse that govern the pulse width resolution. OSC: entity WORK.Counter generic map (24) -- 2^24 / 3 ~ 50Hz from 312.5MHz. port map (Clk, Reset, '1', to_unsigned(3, 4), Pulse50Hz); OSC2: entity WORK.Counter generic map (15) -- 2^15 / 107 gives roughly 2048 steps in a 2ms. port map (Clk, Pulse50Hz, '1', to_unsigned(107, 8), Pulse); -- We start this counter at the current Value and count down on -- each assertion of Pulse. Then stop and wait until the next Pulse50 process(Clk) is begin if rising_edge(Clk) then if Pulse50Hz = '1' then -- Reset at ~50Hz, start at the current input Value -- adding 512 gives the output pulse a minimum 500uS width Count <= resize(unsigned(clip(Value, 11, 0)), Count'length) + 512; elsif Pulse = '1' and Count /= 0 then -- Count down on each Pulse and stop at zero Count <= Count - 1; end if; end if; end process; -- Finally we convert our Count value into our pulse train OutputA <= HI_LVL when Count /= 0 else LO_LVL; end architecture;
# Counter/Timer Module
library IEEE; use IEEE.Std_Logic_1164.all; use IEEE.Numeric_Std.all; -- A Counter provides an "event timer" by diving inputs events -- by 2^EXPONENT / Increment entity Counter is generic ( EXPONENT : positive := 8; PHASE90 : boolean := false ); port ( Clk : in std_logic; Reset : in std_logic; Enable : in std_logic; -- Drive to '1' to divide Clk Increment : in unsigned; Strobe : out std_logic -- Output events ); end entity; architecture Behavioural of Counter is signal Count : unsigned(EXPONENT downto 0); begin assert Increment'length <= Count'length severity FAILURE; process(Clk) is begin if rising_edge(Clk) then if Reset = '1' then Count <= (others => '0'); if PHASE90 then Count(EXPONENT - 1) <= '1'; end if; elsif Enable = '1' then -- Trim the MSB but allow overflow into it. This gives a single Clk cycle -- output pulse on Strobe. Count <= resize(Count(Count'left - 1 downto 0), Count'length) + Increment; else -- Prevent output longer the a single Clk cycle Count(Count'left) <= '0'; end if; end if; end process; -- Just use the MSB of Count overflowing as our event Strobe <= Count(Count'left); end architecture;