Pour contrôler la vitesse du robot, il est important d'asservir les moteurs en vitesse. Pour cela, il faut un capteur permettant de mesurer la vitesse réelle du moteur. Ici, nous allons utiliser un encodeur monté directement sur le moteur.

Nous utilisons un correcteur de type PID. Il faut appliquer la régulation à chaque moteur.

asservissement_vitesse() doit être appelé régulièrement pour que la correction soit efficace (entre 5 et 20 ms). Si on tarde trop à corriger, le système ne sera pas réactif.

lecture_codeurs() fait une lecture des codeurs et met à jour vitesse_moteur_droit et vitesse_moteur_gauche. Pour que les vitesses restent cohérentes entre les appels, il faut appeler asservissement_vitesse() de manière très ponctuelle ou enregistrer le temps entre deux appels. Ici, asservissement _vitesse est appelé toutes les 5 ms. Il faut également lire les codeurs de manière synchrone pour avoir une bonne régulation.

correcteur_PID() retourne la commande a envoyé au moteur en fonction de l'erreur.

La structure PID contient les coefficients pour les 3 actions kp, ki et kd du correcteur, l'ancienne erreur (pour D) et le cumul des erreurs ti (pour I).

typedef struct
{
  signed short kp;
  signed short ki;
  signed short kd;
  signed short ti;
  signed short ancienne_erreur;
} PID;
 
/* pour regulation de vitesse */
PID pid_vitesse_droite =
  { 10, 0, 0, 0, 0 };
 
PID pid_vitesse_gauche =
  { 10, 1, 0, 0, 0 };
 
/* lecture des compteurs */
void
lecture_codeurs(void)
{
  signed short d;
 
  /* chargement de tous les compteurs simultanément */
  load_all_counters();
 
  /* lecture des compteurs un par un */
 
  /* moteur droit : vitesse */
  d = distance_totale_moteur_droit;
  distance_totale_moteur_droit = read_counter_output(CODEUR_MOTEUR_DROIT);
  vitesse_moteur_droit = (distance_totale_moteur_droit - d);
 
  /* moteur gauche : vitesse */
  d = distance_totale_moteur_gauche;
  distance_totale_moteur_gauche = -read_counter_output(CODEUR_MOTEUR_GAUCHE);
  vitesse_moteur_gauche = (distance_totale_moteur_gauche - d);
}
 
/* calcule le signal de commande en appliquant un correcteur PID */
signed short
correcteur_PID(signed short erreur, PID *s)
{
  signed short p, i, d;
 
  /* terme proportionel */
  p = erreur * s->kp;
 
  /* terme integral */
  s->ti += erreur;      // cumcul des erreurs
 
  // limitation du terme integral
  if (s->ti > LIMITE_TERME_INTEGRAL)
    {
      s->ti = LIMITE_TERME_INTEGRAL;
    }
  else if (s->ti < -LIMITE_TERME_INTEGRAL)
    {
      s->ti = -LIMITE_TERME_INTEGRAL;
    }
  i = s->ti * s->ki;
 
  /* terme derive */
  d = erreur - s->ancienne_erreur;
  d *= s->kd;
  s->ancienne_erreur = erreur;      // sauvegarde de l'ancienne erreur
 
  return p + i + d;
}
 
/* regulation vitesse moteur droit et gauche
 * consigne_vitesse_droite et consigne_vitesse_gauche sont
 * les consignes de vitesse droite et gauche en [pulse/cycle] */
void
asservissement_vitesse(signed short consigne_vitesse_droite,
    signed short consigne_vitesse_gauche)
{
  signed short erreur_vitesse_droite, erreur_vitesse_gauche;
  signed short commande_vitesse_droite, commande_vitesse_gauche;
 
  lecture_codeurs();
 
  /* regulation vitesse moteur droit */
  erreur_vitesse_droite = consigne_vitesse_droite - vitesse_moteur_droit;
  commande_vitesse_droite = correcteur_PID(erreur_vitesse_droite,
      &pid_vitesse_droite);
 
  /* regulation vitesse moteur gauche */
  erreur_vitesse_gauche = consigne_vitesse_gauche - vitesse_moteur_gauche;
  commande_vitesse_gauche = correcteur_PID(erreur_vitesse_gauche,
      &pid_vitesse_gauche);
 
  controle_moteurs(commande_vitesse_droite, commande_vitesse_gauche);
}

En asservissant les deux moteurs séparément, on devrait théoriquement avoir la même réponse et le robot devrait rouler droit. Malheureusement, en pratique, 2 moteurs d'une même série ne sont pas exactement identiques (inertie, résistance…) donc même en appliquant la même consigne aux moteurs, les 2 vont réagir différemment. Si un des moteurs est plus réactif, il va accélérer plus vite que l'autre et le robot va tourner.

Il faut donc travailler sur l'ensemble des 2 moteurs en asservissant un moteur en tenant compte de l'autre. En couplant l'asservissement du moteur droit à celui du moteur gauche, si le moteur gauche prend du retard sur le moteur droit, le moteur gauche va accélérer alors que le moteur droit va ralentir.

On va donc définir une vitesse linéaire et une vitesse angulaire à partir de la vitesse de chaque moteur :

vitesselinéaire = (vitessemoteur_droit + vitessemoteur_gauche) / 2
vitesseangulaire = vitessemoteur_droit - vitessemoteur_gauche

Si on veut que le robot aille tout droit, la consigne de vitesse angulaire doit être nulle et si on veut que le robot tourne sur lui même, c'est la consigne de vitesse linéaire qui doit être nulle.