RC-Servocontroller mit PWM von einem FPGA-Pin

Funkgesteuerte (RC) Modellservos sind winzige Aktuatoren, die typischerweise in Bastlermodellflugzeugen, Autos und Booten verwendet werden. Sie ermöglichen dem Bediener, das Fahrzeug über eine Funkverbindung fernzusteuern. Da es RC-Modelle schon seit langer Zeit gibt, ist die De-facto-Standardschnittstelle eher die Pulsweitenmodulation (PWM) als ein digitales Schema.

Glücklicherweise ist es einfach, PWM mit dem genauen Timing zu implementieren, das ein FPGA auf seine Ausgangspins ausüben kann. In diesem Artikel erstellen wir einen generischen Servocontroller, der für jedes RC-Servo funktioniert, das PWM verwendet.

Wie die PWM-Steuerung für ein RC-Servo funktioniert

Ich habe PWM bereits in einem früheren Blogbeitrag behandelt, aber wir können dieses Modul nicht zur Steuerung eines RC-Servos verwenden. Das Problem ist, dass das RC-Servo nicht erwartet, dass die PWM-Impulse so oft ankommen. Es kümmert sich nicht um die volle Einschaltdauer, sondern nur um die Dauer der High-Periode.

Die obige Abbildung zeigt, wie das Timing des PWM-Signals funktioniert.

Das ideale Intervall zwischen den Impulsen beträgt 20 ms, obwohl die Dauer weniger wichtig ist. Die 20 ms werden in eine PWM-Frequenz von 50 Hz übersetzt. Das bedeutet, dass das Servo alle 20 ms einen neuen Positionsbefehl erhält.

Wenn ein Impuls am RC-Servo ankommt, tastet er die Dauer der High-Periode ab. Das Timing ist entscheidend, da dieses Intervall direkt in eine Winkelposition auf dem Servo übersetzt wird. Die meisten Servos erwarten eine Impulsbreite zwischen 1 und 2 ms, aber es gibt keine feste Regel.

Der VHDL-Servocontroller

Wir erstellen ein generisches VHDL-Servocontrollermodul, das Sie so konfigurieren können, dass es mit jedem RC-Servo mit PWM funktioniert. Dazu müssen wir einige Berechnungen basierend auf dem Wert der generischen Eingaben durchführen.

Die von RC-Servos verwendeten PWM-Frequenzen sind im Vergleich zu den Megahertz-Schaltfrequenzen eines FPGA langsam. Das ganzzahlige Zählen von Taktzyklen ergibt eine ausreichende Genauigkeit der PWM-Impulslänge. Es tritt jedoch ein kleiner Rundungsfehler auf, es sei denn, die Taktfrequenz stimmt perfekt mit der Impulsperiode überein.

Wir werden die Berechnungen mit real durchführen (Gleitkomma-)Zahlen, aber schließlich müssen wir die Ergebnisse in ganze Zahlen umwandeln. Im Gegensatz zu den meisten Programmiersprachen rundet VHDL Floats auf die nächste ganze Zahl, aber das Verhalten für halbe Zahlen (0,5, 1,5 usw.) ist undefiniert. Der Simulator oder das Synthesetool kann sich für eine Rundung entscheiden.

library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.math_real.round;

Um plattformübergreifende Konsistenz zu gewährleisten, verwenden wir die Runde Funktion aus math_real Bibliothek, die immer von 0 weg rundet. Der obige Code zeigt die Importe in unser VHDL-Modul mit dem math_real Bibliothek hervorgehoben.

Wenn Sie den vollständigen Code für dieses Projekt benötigen, können Sie ihn herunterladen, indem Sie Ihre E-Mail-Adresse in das untenstehende Formular eingeben. Innerhalb weniger Minuten erhalten Sie eine ZIP-Datei mit dem VHDL-Code, dem ModelSim-Projekt und dem Lattice iCEcube2-Projekt für das iCEstick-FPGA-Board.

Servomoduleinheit mit Generika

Durch die Verwendung generischer Konstanten können wir ein Modul erstellen, das für jedes PWM-fähige RC-Servo funktioniert. Der folgende Code zeigt die Einheit des Servomoduls.

Die erste Konstante ist die als reeller Typ angegebene Taktfrequenz des FPGAs, während pulse_hz gibt an, wie oft der PWM-Ausgang gepulst werden soll, und die folgenden beiden Konstanten legen die Impulsbreite in Mikrosekunden an der minimalen und maximalen Position fest. Die letzte generische Konstante definiert, wie viele Schritte es zwischen der minimalen und maximalen Position gibt, einschließlich der Endpunkte.

entity servo is
  generic (
    clk_hz : real;
    pulse_hz : real; -- PWM pulse frequency
    min_pulse_us : real; -- uS pulse width at min position
    max_pulse_us : real; -- uS pulse width at max position
    step_count : positive -- Number of steps from min to max
  );
  port (
    clk : in std_logic;
    rst : in std_logic;
    position : in integer range 0 to step_count - 1;
    pwm : out std_logic
  );
end servo;

Zusätzlich zu Clock und Reset besteht die Port-Deklaration aus einem einzigen Input- und einem einzigen Output-Signal.

Die Stellung Signal ist der Steuereingang zum Servomodul. Wenn wir es auf Null setzen, erzeugt das Modul min_pulse_us Mikrosekunden lange PWM-Pulse. Wenn Position auf dem höchsten Wert ist, wird max_pulse_us erzeugt lange Impulse.

Die pwm Ausgang ist die Schnittstelle zum externen RC-Servo. Es sollte durch einen FPGA-Pin gehen und mit dem "Signal" -Eingang am Servo verbunden sein, normalerweise das gelbe oder weiße Kabel. Beachten Sie, dass Sie wahrscheinlich einen Pegelwandler verwenden müssen. Die meisten FPGAs verwenden einen Logikpegel von 3,3 V, während die meisten RC-Servos mit 5 V betrieben werden.

Die deklarative Region

Oben im deklarativen Bereich des Servomoduls deklariere ich eine Funktion, mit der wir einige Konstanten berechnen werden. Die cycles_per_us Die unten gezeigte Funktion gibt die nächste Anzahl von Taktzyklen zurück, die wir zählen müssen, um us_count zu messen Mikrosekunden.

function cycles_per_us (us_count : real) return integer is
begin
  return integer(round(clk_hz / 1.0e6 * us_count));
end function;

Direkt unter der Funktion deklarieren wir die Hilfskonstanten, mit denen wir das Timing der Ausgangs-PWM gemäß den Generics vornehmen.

Zuerst übersetzen wir die minimalen und maximalen Mikrosekundenwerte in die absolute Anzahl von Taktzyklen:min_count und max_count . Dann berechnen wir den Bereich in Mikrosekunden zwischen den beiden, von dem wir step_us ableiten , die Dauerdifferenz zwischen jedem linearen Positionsschritt. Zum Schluss wandeln wir die Mikrosekunde real um Wert auf eine feste Anzahl von Taktperioden:cycles_per_step .

constant min_count : integer := cycles_per_us(min_pulse_us);
constant max_count : integer := cycles_per_us(max_pulse_us);
constant min_max_range_us : real := max_pulse_us - min_pulse_us;
constant step_us : real := min_max_range_us / real(step_count - 1);
constant cycles_per_step : positive := cycles_per_us(step_us);

Als nächstes deklarieren wir den PWM-Zähler. Dieses ganzzahlige Signal ist ein freilaufender Zähler, der pulse_hz umschließt Mal jede Sekunde. So erreichen wir die in den Generika angegebene PWM-Frequenz. Der folgende Code zeigt, wie wir die Anzahl der Taktzyklen berechnen, bis zu denen wir zählen müssen, und wie wir die Konstante verwenden, um den Bereich der Ganzzahl zu deklarieren.

constant counter_max : integer := integer(round(clk_hz / pulse_hz)) - 1;
signal counter : integer range 0 to counter_max;

signal duty_cycle : integer range 0 to max_count;

Schließlich deklarieren wir eine Kopie des Zählers mit dem Namen duty_cycle . Dieses Signal bestimmt die Länge der High-Periode am PWM-Ausgang.

Taktzyklen zählen

Der folgende Code zeigt den Prozess, der den freilaufenden Zähler implementiert.

COUNTER_PROC : process(clk)
begin
  if rising_edge(clk) then
    if rst = '1' then
      counter <= 0;

    else
      if counter < counter_max then
        counter <= counter + 1;
      else
        counter <= 0;
      end if;

    end if;
  end if;
end process;

Im Gegensatz zu signiert und unsigniert Typen, die sich selbst umbrechen, müssen wir explizit Null zuweisen, wenn der Zähler den Maximalwert erreicht. Weil wir bereits den Maximalwert in counter_max definiert haben konstant ist, ist es einfach mit einem If-Else-Konstrukt zu erreichen.

PWM-Ausgabeprozess

Um festzustellen, ob der PWM-Ausgang ein hoher oder niedriger Wert sein sollte, vergleichen wir den Zähler und duty_cycle Signale. Wenn der Zähler kleiner als das Tastverhältnis ist, ist die Ausgabe ein hoher Wert. Also der Wert des duty_cycle Signal steuert die Dauer des PWM-Pulses.

PWM_PROC : process(clk)
begin
  if rising_edge(clk) then
    if rst = '1' then
      pwm <= '0';

    else
      pwm <= '0';

      if counter < duty_cycle then
        pwm <= '1';
      end if;

    end if;
  end if;
end process;

Einschaltdauer berechnen

Der Arbeitszyklus sollte nie kleiner als min_count sein Taktzyklen, weil das der Wert ist, der min_pulse_us entspricht allgemeine Eingabe. Daher verwenden wir min_count als Rücksetzwert für den duty_cycle Signal, wie unten gezeigt.

DUTY_CYCLE_PROC : process(clk)
begin
  if rising_edge(clk) then
    if rst = '1' then
      duty_cycle <= min_count;

    else
      duty_cycle <= position * cycles_per_step + min_count;

    end if;
  end if;
end process;

Wenn das Modul nicht zurückgesetzt ist, berechnen wir den Arbeitszyklus als Funktion der Eingangsposition. Die Zyklen_pro_Schritt konstant ist eine Annäherung, gerundet auf die nächste ganze Zahl. Daher kann der Fehler dieser Konstante bis zu 0,5 betragen. Wenn wir mit der befohlenen Position multiplizieren, wird der Fehler skaliert. Da der FPGA-Takt jedoch erheblich schneller ist als die PWM-Frequenz, ist dies nicht wahrnehmbar.

Die Servoprüfbank

Um das RC-Servomodul zu testen, habe ich eine Prüfbank mit manueller Prüfung erstellt, die es uns ermöglicht, das Verhalten des Servomoduls in der Wellenform zu beobachten. Wenn Sie ModelSim auf Ihrem Computer installiert haben, können Sie das Beispielsimulationsprojekt herunterladen, indem Sie Ihre E-Mail-Adresse in das untenstehende Formular eingeben.

Simulationskonstanten

Um die Simulationszeit zu beschleunigen, werden wir in der Testbench eine niedrige Taktfrequenz von 1 MHz vorgeben. Ich habe auch die Schrittzahl auf nur 5 eingestellt, was ausreichen sollte, um das zu testende Gerät (DUT) in Aktion zu sehen.

Der folgende Code zeigt alle in der Testbench definierten Simulationskonstanten.

constant clk_hz : real := 1.0e6;
constant clk_period : time := 1 sec / clk_hz;

constant pulse_hz : real := 50.0;
constant pulse_period : time := 1 sec / pulse_hz;
constant min_pulse_us : real := 1000.0;
constant max_pulse_us : real := 2000.0;
constant step_count : positive := 5;

DUT-Signale

Die in der Testbench deklarierten Signale stimmen mit den Ein- und Ausgängen des DUT überein. Wie wir im folgenden Code sehen können, habe ich den clk angegeben und zuerst signalisiert einen Anfangswert von „1“. Dies bedeutet, dass die Uhr an der hohen Position startet und das Modul anfänglich zurückgesetzt wird.

signal clk : std_logic := '1';
signal rst : std_logic := '1';
signal position : integer range 0 to step_count - 1;
signal pwm : std_logic;

Um das Taktsignal in der Testbench zu erzeugen, verwende ich den unten gezeigten regulären Einzeiler-Prozess.

clk <= not clk after clk_period / 2;

DUT-Instanziierung

Unterhalb der Clock-Stimulus-Linie fahren wir mit der Instanziierung des DUT fort. Wir weisen den Generika mit passenden Namen die Testbench-Konstanten zu. Darüber hinaus ordnen wir die Port-Signale des DUT lokalen Signalen in der Testbench zu.

DUT : entity work.servo(rtl)
generic map (
  clk_hz => clk_hz,
  pulse_hz => pulse_hz,
  min_pulse_us => min_pulse_us,
  max_pulse_us => max_pulse_us,
  step_count => step_count
)
port map (
  clk => clk,
  rst => rst,
  position => position,
  pwm => pwm
);

Testbench-Sequenzer

Um Stimuli für das DUT bereitzustellen, verwenden wir den unten gezeigten Sequenzerprozess. Zuerst setzen wir das DUT zurück. Dann verwenden wir eine For-Schleife, um alle möglichen Eingabepositionen (in unserem Fall 5) zu durchlaufen. Schließlich geben wir eine Nachricht an die Simulatorkonsole aus und beenden die Testbench, indem wir VHDL-2008 finish aufrufen Verfahren.

SEQUENCER : process
begin
  wait for 10 * clk_period;
  rst <= '0';

  wait for pulse_period;

  for i in 0 to step_count - 1 loop
    position <= i;
    wait for pulse_period;
  end loop;

  report "Simulation done. Check waveform.";
  finish;
end process;

Servosimulationswellenform

Die folgende Wellenform zeigt einen Teil der Wellenform, die die Testbench in ModelSim erzeugt. Wir können sehen, dass die Testbank den Positionseingang periodisch ändert und dass das DUT darauf reagiert, indem es PWM-Impulse erzeugt. Beachten Sie, dass der PWM-Ausgang nur bei den niedrigsten Zählerwerten hoch ist. Das ist die Arbeit unseres PWM_PROC-Prozesses.

Wenn Sie die Projektdateien herunterladen, sollten Sie in der Lage sein, die Simulation zu reproduzieren, indem Sie den Anweisungen in der Zip-Datei folgen.

Beispiel-FPGA-Implementierung

Das nächste, was ich möchte, ist, das Design auf einem FPGA zu implementieren und es ein echtes RC-Servo, den TowerPro SG90, steuern zu lassen. Wir verwenden dafür das Lattice iCEstick FPGA-Entwicklungsboard. Es ist das gleiche Board, das ich in meinem VHDL-Anfängerkurs und meinem FPGA-Fortgeschrittenenkurs verwende.

Wenn Sie den Lattice iCEstick haben, können Sie das iCEcube2-Projekt herunterladen, indem Sie das folgende Formular verwenden.

Das Servomodul kann jedoch nicht alleine agieren. Wir brauchen einige unterstützende Module, damit dies auf einem FPGA funktioniert. Zumindest brauchen wir etwas, um die Eingabeposition zu ändern, und wir sollten auch ein Reset-Modul haben.

Um die Servobewegung interessanter zu machen, werde ich das Sine-ROM-Modul verwenden, das ich in einem früheren Artikel behandelt habe. Zusammen mit dem Zählermodul aus dem zuvor erwähnten Artikel erzeugt das Sine ROM ein gleichmäßiges Bewegungsmuster von Seite zu Seite.

Lesen Sie hier mehr über das Sine-ROM-Modul:
Erstellen eines atmenden LED-Effekts mit einer im Block-RAM gespeicherten Sinuswelle

Das folgende Datenflussdiagramm zeigt die Submodule und wie sie verbunden sind.

Oberste Modulentität

Die Einheit des oberen Moduls besteht aus den Takt- und Reset-Eingängen und dem PWM-Ausgang, der das RC-Servo steuert. Ich habe die pwm weitergeleitet Signal an Pin 119 auf dem Lattice iCE40 HX1K FPGA. Das ist der Pin ganz links auf dem Header-Rack ganz links. Der Takt kommt vom integrierten Oszillator des iCEsticks, und ich habe den ersten angeschlossen Signal an einen Pin, der mit einem internen Pull-Up-Widerstand konfiguriert ist.

entity top is
  port (
    clk : in std_logic;
    rst_n : in std_logic; -- Pullup
    pwm : out std_logic
  );
end top; 

Signale und Konstanten

Im deklarativen Bereich des obersten Moduls habe ich Konstanten definiert, die mit dem Lattice iCEstick und meinem TowerPro SG90-Servo übereinstimmen.

Durch Experimentieren fand ich heraus, dass 0,5 ms bis 2,5 ms mir die 180-Grad-Bewegung gab, die ich wollte. Verschiedene Quellen im Internet schlagen andere Werte vor, aber diese sind diejenigen, die für mich funktioniert haben. Ich bin mir nicht ganz sicher, ob ich ein echtes TowerPro SG90-Servo verwende, es könnte eine Fälschung sein.

Wenn das der Fall ist, war es unbeabsichtigt, da ich es von einem Internethändler gekauft habe, aber es könnte die unterschiedlichen Pulsperiodenwerte erklären. Ich habe die Dauern mit meinem Oszilloskop überprüft. Sie sind das, was in dem unten gezeigten Code geschrieben ist.

constant clk_hz : real := 12.0e6; -- Lattice iCEstick clock
constant pulse_hz : real := 50.0;
constant min_pulse_us : real := 500.0; -- TowerPro SG90 values
constant max_pulse_us : real := 2500.0; -- TowerPro SG90 values
constant step_bits : positive := 8; -- 0 to 255
constant step_count : positive := 2**step_bits;

Ich habe cnt eingestellt Signal für den freilaufenden Zähler 25 Bit breit sein, was bedeutet, dass es in etwa 2,8 Sekunden umbricht, wenn es auf dem 12-MHz-Takt des iCEsticks läuft.

constant cnt_bits : integer := 25;
signal cnt : unsigned(cnt_bits - 1 downto 0);

Schließlich deklarieren wir die Signale, die die Top-Level-Module gemäß dem zuvor vorgestellten Datenflussdiagramm verbinden. Ich werde später in diesem Artikel zeigen, wie die unten stehenden Signale interagieren.

signal rst : std_logic;
signal position : integer range 0 to step_count - 1;
signal rom_addr : unsigned(step_bits - 1 downto 0);
signal rom_data : unsigned(step_bits - 1 downto 0);

Servomodul-Instanziierung

Die Instanziierung des Servomoduls ist ähnlich wie wir es in der Testbench gemacht haben:konstant zu generisch und lokales Signal zu Portsignal.

SERVO : entity work.servo(rtl)
generic map (
  clk_hz => clk_hz,
  pulse_hz => pulse_hz,
  min_pulse_us => min_pulse_us,
  max_pulse_us => max_pulse_us,
  step_count => step_count
)
port map (
  clk => clk,
  rst => rst,
  position => position,
  pwm => pwm
);

Self-Wrapping-Zähler-Instanziierung

Ich habe das selbstverpackende Zählermodul in früheren Artikeln verwendet. Es ist ein freilaufender Zähler, der bis zu counter_bits zählt , und geht dann wieder auf Null. Dazu gibt es nicht viel zu sagen, aber wenn Sie es sich ansehen möchten, können Sie das Beispielprojekt herunterladen.

COUNTER : entity work.counter(rtl)
generic map (
  counter_bits => cnt_bits
)
port map (
  clk => clk,
  rst => rst,
  count_enable => '1',
  counter => cnt
);

Sinus-ROM-Instanziierung

Ich habe das Sine-ROM-Modul in einem früheren Artikel ausführlich erklärt. Kurz gesagt, es übersetzt einen linearen Zahlenwert in eine volle Sinuswelle mit derselben Min/Max-Amplitude. Die Eingabe ist die Adresse Signal, und die Sinuswerte erscheinen auf den Daten Ausgabe.

SINE_ROM : entity work.sine_rom(rtl)
generic map (
  data_bits => step_bits,
  addr_bits => step_bits
)
port map (
  clk => clk,
  addr => rom_addr,
  data => rom_data
);

Wir werden die unten gezeigten gleichzeitigen Zuweisungen verwenden, um das Zählermodul, das Sinus-ROM-Modul und das Servomodul anzuschließen.

position <= to_integer(rom_data);
rom_addr <= cnt(cnt'left downto cnt'left - step_bits + 1);

Der Positionseingang des Servomoduls ist eine Kopie des Sinus-ROM-Ausgangs, aber wir müssen den vorzeichenlosen Wert in eine Ganzzahl umwandeln, da sie unterschiedlichen Typs sind. Für die Eingabe der ROM-Adresse verwenden wir die obersten Bits des freilaufenden Zählers. Dadurch wird der Sinuswellen-Bewegungszyklus abgeschlossen, wenn cnt Signalumbrüche nach 2,8 Sekunden.

Testen auf dem Lattice iCEstick

Ich habe die gesamte Schaltung auf einem Steckbrett angeschlossen, wie die Skizze unten zeigt. Da das FPGA 3,3 V verbraucht, während das Servo mit 5 V läuft, habe ich ein externes 5-V-Netzteil und einen steckbaren Level-Shifter verwendet. Ohne Berücksichtigung des Pegelwandlers geht der PWM-Ausgang vom FPGA-Pin direkt an die „Signal“-Leitung des TowerPro SG90-Servos.

Nach dem Betätigen des Netzschalters sollte sich das Servo in einer sanften 180-Grad-Bewegung hin und her bewegen und an den äußersten Positionen leicht anhalten. Das folgende Video zeigt mein Setup mit dem auf dem Oszilloskop visualisierten PWM-Signal.

Abschließende Gedanken

Wie immer gibt es viele Möglichkeiten, ein VHDL-Modul zu implementieren. Ich bevorzuge jedoch den in diesem Artikel beschriebenen Ansatz, bei dem Integer-Typen als Zähler verwendet werden. Alle aufwendigen Berechnungen finden zur Kompilierzeit statt, und die resultierende Logik besteht nur aus Zählern, Registern und Multiplexern.

Die größte Gefahr beim Umgang mit 32-Bit-Ganzzahlen in VHDL besteht darin, dass sie in Berechnungen unbemerkt überlaufen. Sie müssen sicherstellen, dass kein Teilausdruck für irgendwelche Werte im erwarteten Eingabebereich überläuft. Unser Servomodul funktioniert für jede realistische Taktfrequenz und Servoeinstellung.

Beachten Sie, dass diese Art von PWM für die meisten Anwendungen außer RC-Servos nicht geeignet ist. Für die analoge Leistungssteuerung ist das Tastverhältnis wichtiger als die Schaltfrequenz.

Lesen Sie hier mehr über die analoge Leistungssteuerung mit PWM:
How to create a PWM controller in VHDL

Wenn Sie die Beispiele selbst ausprobieren möchten, können Sie schnell loslegen, indem Sie die Zip-Datei herunterladen, die ich für Sie vorbereitet habe. Geben Sie Ihre E-Mail-Adresse in das unten stehende Formular ein und Sie erhalten alles, was Sie brauchen, um innerhalb weniger Minuten loszulegen! Das Paket enthält den vollständigen VHDL-Code, das ModelSim-Projekt mit einem Run-Skript, das Lattice iCEcube2-Projekt und die Konfigurationsdatei des Lattice Diamond-Programmierers.

Teilen Sie mir Ihre Meinung im Kommentarbereich mit!