Controlling an RC Servo with an FPGA
This time, we will look at how to control a regular RC hobby servo. They come in many different sizes and gearings, but for the basic control, they all use a simple PWM (Pulse Width Modulation) signal, to indicate the desired position.
The width of the pulse should be between 1000 and 2000µs, with 1500µs representing the center position. Some brands have slight differences as to where the center point is, and some also allow pulses to be smaller than 1000µs and larger than 2000µs, but you can’t count on that. You also need to send a pulse to the servo at least every 20ms, otherwise the servo will likely enter standby, and it will no longer hold it’s position if there is a load on the arm.
The implementation below is loosely based on the design described at fpga4fun.com, but instead of Verilog, I have decided to use VHDL.
In addition to controlling the servo, I have tried to separate the project into multiple entities, as a preparation to expand with other controls.
Servo Driver
First we have the VHDL code to implement the servo driver. It has an input for the clock signal and an 8bit vector for the position. On the output side, it has a single output for the servo control line.
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.NUMERIC_STD.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity ServoDriver is
Port ( Clk : in STD_LOGIC;
Position : in STD_LOGIC_VECTOR (7 downto 0);
Servo : out STD_LOGIC);
end ServoDriver;
architecture Behavioral of ServoDriver is
constant ClockDiv: integer := 63;
signal ClockTick: std_logic := '0';
signal ClockCount: std_logic_vector (6 downto 0) := "0000000";
signal PulseCount: std_logic_vector (11 downto 0) := "000000000000";
begin
process (Clk)
begin
if Clk='1' and Clk'event then
if ClockCount = ClockDiv-2 then
ClockTick <= '1';
else
ClockTick <= '0';
end if;
if ClockTick='1' then
ClockCount <= "0000000";
else
ClockCount <= ClockCount + 1;
end if;
end if;
end process;
process (Clk)
begin
if Clk='1' and Clk'event then
if ClockTick='1' then
PulseCount <= PulseCount + 1;
end if;
if PulseCount < ("0001" & Position) then
Servo <= '1';
else
Servo <= '0';
end if;
end if;
end process;
end Behavioral;Button controller
To test out the ServoDriver above, I made a little entity, that uses button presses to increment and decrement a counter. It’s fairly simple, the most tricky part is the conversions, to make sure it’s running unsigned all the time, especially when the 8bit ServoPos signal is wrapping around.
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.NUMERIC_STD.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity Controller is
Port ( ButtonDown : in STD_LOGIC;
ButtonUp : in STD_LOGIC;
Position : out STD_LOGIC_VECTOR (7 downto 0));
end Controller;
architecture Behavioral of Controller is
signal ServoPos: unsigned(7 downto 0) := "00000000";
signal Button: std_logic;
signal Direction: std_logic;
begin
process (Button)
begin
if Button='1' and Button'event then
if Direction='1' then
ServoPos <= ServoPos + 4;
Position <= conv_std_logic_vector(ServoPos, 8);
else
ServoPos <= ServoPos - 4;
Position <= conv_std_logic_vector(ServoPos, 8);
end if;
end if;
end process;
Button <= ButtonDown xor ButtonUp;
Direction <= ButtonUp and not ButtonDown;
end Behavioral;Top module
Finally, I made an entity to connect the Controller with the ServoDriver. It makes two instances, one of each entity, and has an 8bit signal (Position) between the two.
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.NUMERIC_STD.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity ServoUpDown is
Port ( Clk : in STD_LOGIC;
ButtonUp : in STD_LOGIC;
ButtonDown : in STD_LOGIC;
Servo : out STD_LOGIC);
end ServoUpDown;
architecture Behavioral of ServoUpDown is
signal Position : std_logic_vector (7 downto 0);
begin
Servo1: entity ServoDriver port map (Clk, Position, Servo);
Controller1: entity Controller port map (ButtonDown, ButtonUp, Position);
end Behavioral;UCF file for the AVNET Xilinx® Spartan®-3A Evaluation Kit
I’m using the following UCF file to connect the above with the outside world. The servo has the negative lead connected to GND on the board, positive to +5v, and the signal wire connected to D10. The two buttons are PUSH_A and PUSH_B on the board, connected via the PSoC.
#Created by Constraints Editor (xc3s400a-ft256-4) - 2010/06/17
NET "Clk" TNM_NET = "Clk";
TIMESPEC TS_Clk = PERIOD "Clk" 62.5 ns HIGH 50 %;
# PlanAhead Generated physical constraints
NET "Clk" LOC = C10;
NET "ButtonUp" LOC = K3;
NET "ButtonDown" LOC = H5;
NET "Servo" LOC = D10;
# PlanAhead Generated IO constraints
NET "Clk" IOSTANDARD = LVCMOS33;
NET "ButtonUp" IOSTANDARD = LVCMOS33;
NET "ButtonDown" IOSTANDARD = LVCMOS33;
NET "Servo" IOSTANDARD = LVCMOS33;The servo should ideally use 5v logic for the signal, but many servos will accept a 3.3v signal without any problems including the Hyperion Atlas Digital servo I’m using (HP-DS13-ACB).
Below is a short video showing the servo move as the buttons are pressed.
