Servo-contrôleur RC utilisant PWM à partir d'une broche FPGA

Les servomoteurs radiocommandés (RC) sont de minuscules actionneurs généralement utilisés dans les modèles réduits d'avions, de voitures et de bateaux. Ils permettent à l'opérateur de contrôler le véhicule via une liaison radio à distance. Étant donné que les modèles RC existent depuis longtemps, l'interface standard de facto est la modulation de largeur d'impulsion (PWM), plutôt qu'un schéma numérique.

Heureusement, il est facile d'implémenter PWM avec la synchronisation précise qu'un FPGA peut exercer sur ses broches de sortie. Dans cet article, nous allons créer un servo-contrôleur générique qui fonctionnera pour tout servo RC utilisant PWM.

Comment fonctionne le contrôle PWM pour un servo RC

J'ai déjà couvert PWM dans un article de blog précédent, mais nous ne pouvons pas utiliser ce module pour contrôler un servo RC. Le problème est que le servo RC ne s'attend pas à ce que les impulsions PWM arrivent aussi souvent. Il ne se soucie pas du cycle de service complet, seulement de la durée de la période haute.

L'illustration ci-dessus montre comment fonctionne la synchronisation du signal PWM.

L'intervalle idéal entre les impulsions est de 20 ms, bien que sa durée ait moins d'importance. Les 20 ms se traduisent par une fréquence PWM de 50 Hz. Cela signifie que le servo reçoit une nouvelle commande de position toutes les 20 ms.

Lorsqu'une impulsion arrive au servo RC, il échantillonne la durée de la période haute. Le timing est crucial car cet intervalle se traduit directement par une position angulaire sur le servo. La plupart des servos s'attendent à voir une largeur d'impulsion variant entre 1 et 2 ms, mais il n'y a pas de règle établie.

Le servocontrôleur VHDL

Nous allons créer un module de contrôleur d'asservissement VHDL générique que vous pouvez configurer pour fonctionner avec n'importe quel servo RC utilisant PWM. Pour ce faire, nous devons effectuer des calculs basés sur la valeur des entrées génériques.

Les fréquences PWM utilisées par les servos RC sont lentes par rapport aux fréquences de commutation en mégahertz d'un FPGA. Le comptage d'entiers de cycles d'horloge donne une précision suffisante de la longueur d'impulsion PWM. Cependant, il y aura une petite erreur d'arrondi à moins que la fréquence d'horloge ne corresponde parfaitement à la période d'impulsion.

Nous effectuerons les calculs en utilisant réel (virgule flottante), mais finalement, nous devons convertir les résultats en nombres entiers. Contrairement à la plupart des langages de programmation, les arrondis VHDL flottent à l'entier le plus proche, mais le comportement des demi-nombres (0,5, 1,5, etc.) n'est pas défini. Le simulateur ou l'outil de synthèse peut choisir d'arrondir dans les deux sens.

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

Pour assurer la cohérence entre les plates-formes, nous utiliserons le rond fonction du math_real bibliothèque, qui arrondit toujours à partir de 0. Le code ci-dessus montre les importations dans notre module VHDL avec le math_real bibliothèque en surbrillance.

Si vous avez besoin du code complet pour ce projet, vous pouvez le télécharger en saisissant votre adresse e-mail dans le formulaire ci-dessous. En quelques minutes, vous recevrez un fichier Zip avec le code VHDL, le projet ModelSim et le projet Lattice iCEcube2 pour la carte FPGA iCEstick.

Entité module servo avec génériques

En utilisant des constantes génériques, nous pouvons créer un module qui fonctionnera pour n'importe quel servo RC compatible PWM. Le code ci-dessous montre l'entité du module servo.

La première constante est la fréquence d'horloge du FPGA donnée comme un type réel, tandis que pulse_hz spécifie la fréquence à laquelle la sortie PWM doit être pulsée, et les deux constantes suivantes définissent la largeur d'impulsion en microsecondes aux positions minimale et maximale. La constante générique finale définit le nombre d'étapes entre la position min et max, y compris les points finaux.

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;

En plus de l'horloge et de la réinitialisation, la déclaration de port se compose d'une seule entrée et d'un seul signal de sortie.

Le poste signal est l'entrée de commande du module d'asservissement. Si nous le mettons à zéro, le module produira min_pulse_us impulsions PWM de plusieurs microsecondes. Quand position est à la valeur la plus élevée, il produira max_pulse_us impulsions longues.

Le pwm La sortie est l'interface avec le servo RC externe. Il doit passer par une broche FPGA et se connecter à l'entrée "Signal" du servo, généralement le fil jaune ou blanc. Notez que vous devrez probablement utiliser un convertisseur de niveau. La plupart des FPGA utilisent un niveau logique de 3,3 V, tandis que la plupart des servomoteurs RC fonctionnent sur du 5 V.

La région déclarative

En haut de la région déclarative du module d'asservissement, je déclare une fonction que nous utiliserons pour calculer quelques constantes. Les cycles_per_us La fonction, illustrée ci-dessous, renvoie le nombre le plus proche de cycles d'horloge que nous devons compter pour mesurer us_count microsecondes.

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

Directement en dessous de la fonction, nous déclarons les constantes d'assistance, que nous utiliserons pour effectuer la synchronisation de la sortie PWM selon les génériques.

Tout d'abord, nous traduisons les valeurs min et max de microsecondes en nombre absolu de cycles d'horloge :min_count et max_count . Ensuite, nous calculons la plage en microsecondes entre les deux, à partir de laquelle nous dérivons step_us , la différence de durée entre chaque pas de position linéaire. Enfin, nous convertissons la microseconde réelle valeur à un nombre fixe de périodes d'horloge :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);

Ensuite, nous déclarons le compteur PWM. Ce signal entier est un compteur libre qui encapsule pulse_hz fois chaque seconde. C'est ainsi que nous atteignons la fréquence PWM donnée dans les génériques. Le code ci-dessous montre comment nous calculons le nombre de cycles d'horloge que nous devons compter et comment nous utilisons la constante pour déclarer la plage de l'entier.

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;

Enfin, nous déclarons une copie du compteur nommé duty_cycle . Ce signal déterminera la durée de la période haute sur la sortie PWM.

Comptage des cycles d'horloge

Le code ci-dessous montre le processus qui implémente le compteur à exécution libre.

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;

Contrairement à signé et non signé types qui s'auto-enroulent, nous devons attribuer explicitement zéro lorsque le compteur atteint la valeur maximale. Parce que nous avons déjà la valeur max définie dans le counter_max constante, c'est facile à réaliser avec une construction If-Else.

Processus de sortie PWM

Pour déterminer si la sortie PWM doit être une valeur haute ou basse, nous comparons le compteur et duty_cycle signaux. Si le compteur est inférieur au rapport cyclique, la sortie est une valeur élevée. Ainsi, la valeur du duty_cycle signal contrôle la durée de l'impulsion PWM.

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;

Calcul du rapport cyclique

Le cycle de service ne doit jamais être inférieur à min_count cycles d'horloge car c'est la valeur qui correspond au min_pulse_us entrée générique. Par conséquent, nous utilisons min_count comme valeur de réinitialisation pour le duty_cycle signal, comme indiqué ci-dessous.

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;

Lorsque le module n'est pas en reset, on calcule le rapport cyclique en fonction de la position de l'entrée. Les cycles_per_step constante est une approximation, arrondie à l'entier le plus proche. Par conséquent, l'erreur sur cette constante peut aller jusqu'à 0,5. Lorsque nous multiplions avec la position commandée, l'erreur augmente. Cependant, l'horloge FPGA étant beaucoup plus rapide que la fréquence PWM, cela ne sera pas perceptible.

Le banc de test des servos

Pour tester le module d'asservissement RC, j'ai créé un banc de test à vérification manuelle qui nous permettra d'observer le comportement du module d'asservissement dans la forme d'onde. Si ModelSim est installé sur votre ordinateur, vous pouvez télécharger l'exemple de projet de simulation en saisissant votre adresse e-mail dans le formulaire ci-dessous.

Constantes de simulation

Pour accélérer le temps de simulation, nous spécifierons une fréquence d'horloge basse de 1 MHz dans le banc de test. J'ai également défini le nombre de pas uniquement sur 5, ce qui devrait être suffisant pour que nous puissions voir l'appareil sous test (DUT) en action.

Le code ci-dessous montre toutes les constantes de simulation définies dans le testbench.

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;

Signaux DUT

Les signaux déclarés dans le banc de test correspondent aux entrées et sorties du DUT. Comme nous pouvons le voir dans le code ci-dessous, j'ai donné le clk et première signale une valeur initiale de '1'. Cela signifie que l'horloge démarrera à la position haute et que le module sera initialement en réinitialisation.

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

Pour générer le signal d'horloge dans le banc d'essai, j'utilise le processus régulier à une ligne illustré ci-dessous.

clk <= not clk after clk_period / 2;

Instanciation DUT

Sous la ligne de stimulus d'horloge, nous procédons à l'instanciation du DUT. Nous attribuons les constantes de testbench aux génériques avec des noms correspondants. De plus, nous mappons les signaux de port du DUT aux signaux locaux dans le banc de test.

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

Séquenceur de banc d'essai

Pour fournir des stimuli au DUT, nous utilisons le processus de séquenceur illustré ci-dessous. Tout d'abord, nous réinitialisons le DUT. Ensuite, nous utilisons une boucle For pour parcourir toutes les positions d'entrée possibles (5 dans notre cas). Enfin, nous imprimons un message à la console du simulateur et terminons le testbench en appelant le VHDL-2008 finish procédure.

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;

Forme d'onde de simulation d'asservissement

La forme d'onde ci-dessous montre une partie de la forme d'onde que le testbench produit dans ModelSim. Nous pouvons voir que le banc d'essai change périodiquement l'entrée de position et que le DUT répond en produisant des impulsions PWM. Notez que la sortie PWM est élevée uniquement sur les valeurs de compteur les plus basses. C'est le travail de notre processus PWM_PROC.

Si vous téléchargez les fichiers du projet, vous devriez pouvoir reproduire la simulation en suivant les instructions trouvées dans le fichier Zip.

Exemple d'implémentation FPGA

La prochaine chose que je veux est d'implémenter la conception sur un FPGA et de le laisser contrôler un servo RC réel, le TowerPro SG90. Nous utiliserons la carte de développement Lattice iCEstick FPGA pour cela. C'est la même carte que celle que j'utilise dans mon cours VHDL pour débutant et mon cours FPGA avancé.

Si vous avez le Lattice iCEstick, vous pouvez télécharger le projet iCEcube2 en utilisant le formulaire ci-dessous.

Cependant, le module d'asservissement ne peut pas agir seul. Nous avons besoin de quelques modules de support pour que cela fonctionne sur un FPGA. Au moins, nous avons besoin de quelque chose pour changer la position d'entrée, et nous devrions également avoir un module de réinitialisation.

Pour rendre le servomouvement plus intéressant, je vais utiliser le module Sine ROM que j'ai abordé dans un article précédent. Avec le module Counter de l'article mentionné précédemment, la ROM Sine générera un mouvement fluide d'un côté à l'autre.

En savoir plus sur le module Sine ROM ici :
Comment créer un effet LED respiratoire à l'aide d'une onde sinusoïdale stockée dans le bloc RAM

Le diagramme de flux de données ci-dessous montre les sous-modules et comment ils sont connectés.

Entité du module supérieur

L'entité du module supérieur se compose des entrées d'horloge et de réinitialisation et de la sortie PWM, qui contrôle le servo RC. J'ai routé le pwm signal à la broche 119 sur le FPGA Lattice iCE40 HX1K. C'est la broche la plus à gauche sur le rack d'en-tête le plus à gauche. L'horloge provient de l'oscillateur embarqué de l'iCEstick, et j'ai connecté le premier signal à une broche configurée avec une résistance pull-up interne.

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

Signaux et constantes

Dans la région déclarative du module supérieur, j'ai défini des constantes qui correspondent à l'iCEstick Lattice et à mon servo TowerPro SG90.

En expérimentant, j'ai découvert que 0,5 ms à 2,5 ms me donnaient les 180 degrés de mouvement que je voulais. Diverses sources sur Internet suggèrent d'autres valeurs, mais ce sont celles qui ont fonctionné pour moi. Je ne suis pas tout à fait sûr d'utiliser un servo TowerPro SG90 légitime, il pourrait s'agir d'une contrefaçon.

Si tel est le cas, ce n'était pas intentionnel car je l'ai acheté auprès d'un vendeur sur Internet, mais cela pourrait expliquer les différentes valeurs de période d'impulsion. J'ai vérifié les durées avec mon oscilloscope. C'est ce qui est écrit dans le code ci-dessous.

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;

J'ai défini le cnt signal pour que le compteur à exécution libre ait une largeur de 25 bits, ce qui signifie qu'il s'enroulera en environ 2,8 secondes lorsqu'il fonctionnera sur l'horloge 12 MHz de l'iCEstick.

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

Enfin, nous déclarons les signaux qui connecteront les modules de niveau supérieur selon le diagramme de flux de données que j'ai présenté précédemment. Je montrerai comment les signaux ci-dessous interagissent plus tard dans cet article.

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

Instanciation du module servo

L'instanciation du module d'asservissement est similaire à la façon dont nous l'avons fait dans le banc d'essai :constant à générique et signal local à signal de port.

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

Instanciation du compteur d'auto-emballage

J'ai utilisé le module de compteur d'auto-emballage dans les articles précédents. C'est un compteur libre qui compte jusqu'à counter_bits , puis revient à zéro. Il n'y a pas grand-chose à dire à ce sujet, mais si vous voulez l'inspecter, vous pouvez télécharger l'exemple de projet.

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

Instanciation ROM sinusoïdale

J'ai expliqué en détail le module Sine ROM dans un article précédent. Pour faire court, il traduit une valeur numérique linéaire en une onde sinusoïdale complète avec la même amplitude min/max. L'entrée est l'adresse signal, et les valeurs sinusoïdales apparaissent sur les données sortie.

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

Nous utiliserons les affectations simultanées indiquées ci-dessous pour connecter le module Counter, le module Sine ROM et le module Servo.

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

L'entrée de position du module Servo est une copie de la sortie Sine ROM, mais nous devons convertir la valeur non signée en un entier car ils sont de types différents. Pour l'entrée d'adresse ROM, nous utilisons les bits supérieurs du compteur à exécution libre. En faisant cela, le cycle de mouvement de l'onde sinusoïdale se terminera lorsque le cnt le signal s'enroule, après 2,8 secondes.

Test sur l'iCEstick Lattice

J'ai branché l'ensemble du circuit sur une planche à pain, comme le montre le croquis ci-dessous. Étant donné que le FPGA utilise 3,3 V tandis que le servo fonctionne à 5 V, j'ai utilisé une alimentation externe de 5 V et un sélecteur de niveau pouvant être intégré. Sans tenir compte du convertisseur de niveau, la sortie PWM de la broche FPGA va directement au fil "Signal" sur le servo TowerPro SG90.

Après avoir appuyé sur l'interrupteur d'alimentation, le servo doit se déplacer d'avant en arrière dans un mouvement fluide de 180 degrés, s'arrêtant légèrement aux positions extrêmes. La vidéo ci-dessous montre ma configuration avec le signal PWM visualisé sur l'oscilloscope.

Réflexions finales

Comme toujours, il existe de nombreuses façons d'implémenter un module VHDL. Mais je préfère l'approche décrite dans cet article, en utilisant des types entiers comme compteurs. Tous les calculs lourds se produisent au moment de la compilation, et la logique résultante n'est que des compteurs, des registres et des multiplexeurs.

Le danger le plus important lorsqu'il s'agit d'entiers 32 bits en VHDL est qu'ils débordent silencieusement dans les calculs. Vous devez vérifier qu'aucune sous-expression ne débordera pour les valeurs de la plage d'entrée attendue. Notre module d'asservissement fonctionnera pour n'importe quelle fréquence d'horloge réaliste et paramètres d'asservissement.

Notez que ce type de PWM ne convient pas à la plupart des applications autres que les servos RC. Pour le contrôle de puissance analogique, le rapport cyclique est plus important que la fréquence de découpage.

En savoir plus sur le contrôle de l'alimentation analogique à l'aide de PWM :
Comment créer un contrôleur PWM en VHDL

Si vous souhaitez essayer les exemples par vous-même, vous pouvez commencer rapidement en téléchargeant le fichier Zip que j'ai préparé pour vous. Entrez votre adresse e-mail dans le formulaire ci-dessous et vous recevrez tout ce dont vous avez besoin pour commencer en quelques minutes ! Le package contient le code VHDL complet, le projet ModelSim avec un script d'exécution, le projet Lattice iCEcube2 et le fichier de configuration du programmeur Lattice Diamond.

Dites-moi ce que vous en pensez dans la section des commentaires !