RC-servocontroller met PWM van een FPGA-pin

Radiogestuurde (RC) modelservo's zijn kleine actuatoren die doorgaans worden gebruikt in vliegtuigjes, auto's en boten voor hobbyisten. Ze stellen de machinist in staat het voertuig op afstand te besturen via een radioverbinding. Omdat RC-modellen al heel lang bestaan, is de de-facto standaardinterface pulsbreedtemodulatie (PWM), in plaats van een digitaal schema.

Gelukkig is het eenvoudig om PWM te implementeren met de precieze timing die een FPGA kan uitoefenen op zijn uitgangspinnen. In dit artikel zullen we een generieke servocontroller maken die werkt voor elke RC-servo die PWM gebruikt.

Hoe PWM-besturing voor een RC-servo werkt

Ik heb PWM al behandeld in een eerdere blogpost, maar we kunnen die module niet gebruiken voor het aansturen van een RC-servo. Het probleem is dat de RC-servo niet verwacht dat de PWM-pulsen zo vaak zullen aankomen. Het geeft niet om de volledige werkcyclus, alleen om de duur van de hoge periode.

De afbeelding hierboven laat zien hoe de timing van het PWM-signaal werkt.

Het ideale interval tussen pulsen is 20 ms, hoewel de duur ervan minder belangrijk is. De 20 ms vertaalt zich in een PWM-frequentie van 50 Hz. Dit betekent dat de servo elke 20 ms een nieuw positiecommando krijgt.

Wanneer een puls aankomt bij de RC-servo, bemonstert deze de duur van de hoge periode. De timing is cruciaal omdat dit interval zich direct vertaalt naar een hoekpositie op de servo. De meeste servo's verwachten een pulsbreedte te zien die varieert tussen 1 en 2 ms, maar er is geen vaste regel.

De VHDL-servocontroller

We zullen een generieke VHDL-servocontrollermodule maken die u kunt configureren om met elke RC-servo te werken die PWM gebruikt. Om dat te doen, moeten we enkele berekeningen uitvoeren op basis van de waarde van de generieke invoer.

De PWM-frequenties die door RC-servo's worden gebruikt, zijn traag in vergelijking met de megahertz-schakelfrequenties van een FPGA. Integer tellen van klokcycli geeft voldoende nauwkeurigheid van de PWM-pulslengte. Er zal echter een kleine afrondingsfout zijn tenzij de klokfrequentie perfect overeenkomt met de pulsperiode.

We zullen de berekeningen uitvoeren met echt (floating-point) getallen, maar uiteindelijk moeten we de resultaten converteren naar gehele getallen. In tegenstelling tot de meeste programmeertalen, rondt VHDL af op het dichtstbijzijnde gehele getal, maar het gedrag voor halve getallen (0,5, 1,5, enz.) is niet gedefinieerd. De simulator of synthesetool kan ervoor kiezen om beide kanten op af te ronden.

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

Om consistentie tussen platforms te garanderen, gebruiken we de ronde functie van de math_real bibliotheek, die altijd afrondt vanaf 0. De bovenstaande code toont de invoer in onze VHDL-module met de math_real bibliotheek gemarkeerd.

Als u de volledige code voor dit project nodig heeft, kunt u deze downloaden door uw e-mailadres in het onderstaande formulier in te vullen. Binnen enkele minuten ontvang je een Zip-bestand met de VHDL-code, het ModelSim-project en het Lattice iCEcube2-project voor het iCEstick FPGA-bord.

Servomodule-entiteit met generieke geneesmiddelen

Door generieke constanten te gebruiken, kunnen we een module maken die werkt voor elke PWM-compatibele RC-servo. De onderstaande code toont de entiteit van de servomodule.

De eerste constante is de klokfrequentie van de FPGA gegeven als een reëel type, terwijl pulse_hz specificeert hoe vaak de PWM-uitgang moet worden gepulseerd, en de volgende twee constanten stellen de pulsbreedte in microseconden in op de minimum- en maximumposities. De laatste generieke constante definieert hoeveel stappen er zijn tussen de min en max positie, inclusief de eindpunten.

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;

Naast de klok en reset bestaat de poortdeclaratie uit een enkele ingang en een enkel uitgangssignaal.

De positie signaal is de stuuringang naar de servomodule. Als we het op nul zetten, produceert de module min_pulse_us microseconden lange PWM-pulsen. Wanneer positie de hoogste waarde heeft, zal het max_pulse_us . produceren lange pulsen.

De pwm uitgang is de interface naar de externe RC-servo. Het moet door een FPGA-pin gaan en verbinding maken met de "Signaal" -ingang op de servo, meestal de gele of witte draad. Houd er rekening mee dat u waarschijnlijk een niveau-omzetter moet gebruiken. De meeste FPGA's gebruiken een logisch niveau van 3,3 V, terwijl de meeste RC-servo's op 5 V werken.

De declaratieve regio

Bovenaan het declaratieve gebied van de servomodule verklaar ik een functie die we zullen gebruiken om een ​​paar constanten te berekenen. De cycles_per_us functie, hieronder weergegeven, retourneert het dichtstbijzijnde aantal klokcycli dat we moeten tellen om us_count te meten microseconden.

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

Direct onder de functie declareren we de helperconstanten, die we zullen gebruiken om de timing van de output-PWM te maken volgens de generieke waarden.

Eerst vertalen we de min en max microsecondewaarden naar het absolute aantal klokcycli:min_count en max_count . Vervolgens berekenen we het bereik in microseconden tussen de twee, waaruit we step_us afleiden , het duurverschil tussen elke lineaire positiestap. Ten slotte converteren we de microseconde echte waarde naar een vast aantal klokperioden: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);

Vervolgens declareren we de PWM-teller. Dit integer-signaal is een vrijlopende teller die pulse_hz . omhult keer per seconde. Dat is hoe we de PWM-frequentie bereiken die in de generieke geneesmiddelen wordt gegeven. De onderstaande code laat zien hoe we het aantal klokcycli berekenen waartoe we moeten tellen, en hoe we de constante gebruiken om het bereik van het gehele getal te declareren.

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;

Ten slotte declareren we een kopie van de teller met de naam duty_cycle . Dit signaal bepaalt de lengte van de hoge periode op de PWM-uitgang.

Tellen van klokcycli

De onderstaande code toont het proces dat de vrijlopende teller implementeert.

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;

In tegenstelling tot ondertekend en niet ondertekend typen die zichzelf inpakken, moeten we expliciet nul toewijzen wanneer de teller de maximale waarde bereikt. Omdat we de maximale waarde al hebben gedefinieerd in de counter_max constant, het is gemakkelijk te bereiken met een If-Else-constructie.

PWM-uitvoerproces

Om te bepalen of de PWM-output een hoge of lage waarde moet zijn, vergelijken we de teller en duty_cycle signalen. Als de teller kleiner is dan de duty cycle, is de output een hoge waarde. Dus de waarde van de duty_cycle signaal regelt de duur van de PWM-puls.

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;

De inschakelduur berekenen

De duty cycle mag nooit minder zijn dan min_count klokcycli omdat dat de waarde is die overeenkomt met de min_pulse_us generieke invoer. Daarom gebruiken we min_count als de resetwaarde voor de duty_cycle signaal, zoals hieronder weergegeven.

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;

Wanneer de module niet in reset staat, berekenen we de duty cycle als functie van de ingangspositie. De cycles_per_step constante is een benadering, afgerond op het dichtstbijzijnde gehele getal. Daarom kan de fout op deze constante oplopen tot 0,5. Wanneer we vermenigvuldigen met de opgedragen positie, wordt de fout groter. Omdat de FPGA-klok echter veel sneller is dan de PWM-frequentie, zal dit niet merkbaar zijn.

De servo-testbank

Om de RC-servomodule te testen, heb ik een handmatige testbank gemaakt waarmee we het gedrag van de servomodule in de golfvorm kunnen observeren. Als u ModelSim op uw computer hebt geïnstalleerd, kunt u het voorbeeldsimulatieproject downloaden door uw e-mailadres in het onderstaande formulier in te voeren.

Simulatieconstanten

Om de simulatietijd te versnellen, zullen we in de testbench een lage klokfrequentie van 1 MHz specificeren. Ik heb ook het aantal stappen slechts 5 ingesteld, wat voor ons voldoende zou moeten zijn om het te testen apparaat (DUT) in actie te zien.

De onderstaande code toont alle simulatieconstanten die in de testbench zijn gedefinieerd.

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-signalen

De in de testbench gedeclareerde signalen komen overeen met de in- en uitvoer van de TU Delft. Zoals we aan de onderstaande code kunnen zien, heb ik de clk . gegeven en eerste signaleert een beginwaarde van ‘1’. Dit betekent dat de klok op de hoogste stand begint en dat de module in eerste instantie op reset staat.

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

Om het kloksignaal in de testbench te genereren, gebruik ik het reguliere one-linerproces dat hieronder wordt weergegeven.

clk <= not clk after clk_period / 2;

DUT-instantie

Onder de klokstimuluslijn gaan we verder met het instantiëren van de DUT. We kennen de testbench-constanten toe aan de generieke geneesmiddelen met overeenkomende namen. Verder brengen we de poortsignalen van de TU Delft in kaart met lokale signalen in de testbench.

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
);

Testbank-sequencer

Om de DUT te voorzien van stimuli, gebruiken we het onderstaande sequencerproces. Eerst resetten we de DUT. Vervolgens gebruiken we een For-lus om alle mogelijke invoerposities te herhalen (5 in ons geval). Ten slotte printen we een bericht naar de simulatorconsole en beëindigen we de testbench door de VHDL-2008 finish aan te roepen. procedure.

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;

Servo simulatie golfvorm

De onderstaande golfvorm toont een deel van de golfvorm die de testbench produceert in ModelSim. We kunnen zien dat de testbank periodiek de positie-invoer verandert en dat de DUT reageert door PWM-pulsen te produceren. Merk op dat de PWM-output alleen hoog is bij de laagste tellerwaarden. Dat is het werk van ons PWM_PROC-proces.

Als u de projectbestanden downloadt, zou u de simulatie moeten kunnen reproduceren door de instructies in het zip-bestand te volgen.

Voorbeeld FPGA-implementatie

Het volgende dat ik wil, is het ontwerp op een FPGA implementeren en het een echte RC-servo, de TowerPro SG90, laten besturen. Daarvoor gebruiken we het Lattice iCEstick FPGA-ontwikkelbord. Het is hetzelfde bord dat ik gebruik in mijn VHDL-cursus voor beginners en mijn geavanceerde FPGA-cursus.

Als je de Lattice iCEstick hebt, kun je het iCEcube2-project downloaden via onderstaand formulier.

De servomodule kan echter niet alleen handelen. We hebben enkele ondersteunende modules nodig om dit op een FPGA te laten werken. We hebben tenminste iets nodig om de invoerpositie te veranderen, en we zouden ook een resetmodule moeten hebben.

Om de servobeweging interessanter te maken, ga ik de Sine ROM-module gebruiken die ik in een eerder artikel heb behandeld. Samen met de Counter-module uit het eerder genoemde artikel, zal de Sine ROM een vloeiend zij-aan-zij bewegingspatroon genereren.

Lees hier meer over de Sine ROM-module:
Hoe een ademend LED-effect te creëren met behulp van een sinusgolf die is opgeslagen in blok-RAM

Het onderstaande gegevensstroomschema toont de submodules en hoe ze zijn aangesloten.

Top module entiteit

De entiteit van de bovenste module bestaat uit de klok- en reset-ingangen en de PWM-uitgang, die de RC-servo bestuurt. Ik heb de pwm . gerouteerd signaal naar pin 119 op de Lattice iCE40 HX1K FPGA. Dat is de meest linkse pin op het meest linkse headerrek. De klok komt van de ingebouwde oscillator van de iCEstick en ik heb de eerste aangesloten signaal naar een pin die is geconfigureerd met een interne pull-up-weerstand.

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

Signalen en constanten

In het declaratieve gebied van de bovenste module heb ik constanten gedefinieerd die overeenkomen met de Lattice iCEstick en mijn TowerPro SG90-servo.

Door te experimenteren ontdekte ik dat 0,5 ms tot 2,5 ms me de 180 graden beweging gaf die ik wilde. Verschillende bronnen op internet suggereren andere waarden, maar dit zijn degenen die voor mij hebben gewerkt. Ik weet niet helemaal zeker of ik een legitieme TowerPro SG90-servo gebruik, het kan een vervalsing zijn.

Als dat het geval is, was het onbedoeld omdat ik het van een internetverkoper kocht, maar het zou de verschillende waarden van de pulsperiode kunnen verklaren. Ik heb de looptijden geverifieerd met mijn oscilloscoop. Dit is wat er in de onderstaande code staat.

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;

Ik heb de cnt . ingesteld signaal voor de vrijlopende teller met een breedte van 25 bits, wat betekent dat de teller ongeveer 2,8 seconden duurt als hij op de 12 MHz-klok van de iCEstick draait.

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

Ten slotte declareren we de signalen die de modules op het hoogste niveau zullen verbinden volgens het gegevensstroomschema dat ik eerder heb gepresenteerd. Ik zal later in dit artikel laten zien hoe de onderstaande signalen op elkaar inwerken.

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);

Constantiatie van servomodule

De concretisering van de servomodule is vergelijkbaar met hoe we het in de testbench hebben gedaan:constant naar generiek en lokaal signaal naar poortsignaal.

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
);

Zelfsluitende teller-instantie

Ik heb in eerdere artikelen de self-wrapping counter-module gebruikt. Het is een vrijlopende teller die telt tot counter_bits en gaat dan weer naar nul. Er valt niet veel over te zeggen, maar als je het wilt inspecteren, kun je het voorbeeldproject downloaden.

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

Sine ROM-instantiatie

Ik heb de Sine ROM-module in een eerder artikel in detail uitgelegd. Om het kort te zeggen, het vertaalt een lineaire getalswaarde naar een volledige sinusgolf met dezelfde min/max-amplitude. De invoer is de addr signaal, en de sinuswaarden verschijnen op de data uitvoer.

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
);

We zullen de onderstaande gelijktijdige toewijzingen gebruiken om de Counter-module, de Sine ROM-module en de Servo-module aan te sluiten.

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

De positie-invoer van de servomodule is een kopie van de Sine ROM-uitvoer, maar we moeten de niet-ondertekende waarde converteren naar een geheel getal omdat ze van verschillende typen zijn. Voor de invoer van het ROM-adres gebruiken we de bovenste bits van de vrijlopende teller. Door dit te doen, wordt de sinusgolfbewegingscyclus voltooid wanneer de cnt signaal wikkelt zich na 2,8 seconden.

Testen op de Lattice iCEstick

Ik heb het hele circuit aangesloten op een breadboard, zoals blijkt uit de onderstaande schets. Omdat de FPGA 3,3 V gebruikt terwijl de servo op 5 V werkt, heb ik een externe 5 V-voeding en een breadboard-niveauverschuiver gebruikt. Zonder rekening te houden met de niveau-omzetter, gaat de PWM-uitvoer van de FPGA-pin rechtstreeks naar de "Signaal" -draad op de TowerPro SG90-servo.

Nadat de aan / uit-schakelaar is ingedrukt, moet de servo heen en weer bewegen in een vloeiende beweging van 180 graden, waarbij hij enigszins stopt bij de uiterste posities. De onderstaande video toont mijn opstelling met het PWM-signaal gevisualiseerd op de oscilloscoop.

Laatste gedachten

Zoals altijd zijn er veel manieren om een ​​VHDL-module te implementeren. Maar ik geef de voorkeur aan de benadering die in dit artikel wordt beschreven, waarbij ik integer-types als tellers gebruik. Alle zware berekeningen gebeuren tijdens het compileren en de resulterende logica bestaat alleen uit tellers, registers en multiplexers.

Het grootste gevaar bij het omgaan met 32-bits gehele getallen in VHDL is dat ze stil overlopen in berekeningen. U moet controleren of er geen subexpressie zal overlopen voor waarden in het verwachte invoerbereik. Onze servomodule werkt voor elke realistische klokfrequentie en servo-instellingen.

Merk op dat dit soort PWM niet geschikt is voor de meeste andere toepassingen dan RC-servo's. Voor analoge vermogensregeling is de duty-cycle belangrijker dan de schakelfrequentie.

Lees hier meer over analoge vermogensregeling met PWM:
Een PWM-controller maken in VHDL

Als je de voorbeelden zelf wilt uitproberen, kun je snel aan de slag door het zip-bestand te downloaden dat ik voor je heb gemaakt. Vul in onderstaand formulier je e-mailadres in en je ontvangt binnen enkele minuten alles wat je nodig hebt om aan de slag te gaan! Het pakket bevat de volledige VHDL-code, het ModelSim-project met een run-script, het Lattice iCEcube2-project en het Lattice Diamond-programmeerconfiguratiebestand.

Laat me weten wat je ervan vindt in het commentaargedeelte!