Différences

Cette page vous affiche les différences entre la révision choisie et la version actuelle de la page.

--- robotics:asservissement_vitesse [2013/02/19 14:29]
ldo [Asservissement de vitesse du robot]
+++ robotics:asservissement_vitesse [2013/03/18 14:30] (Version actuelle)
ldo [Asservissement de vitesse d'un moteur]
@@ Ligne 6: / Ligne 6: @@
 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.
+Nous utilisons un correcteur de type PID. Il faut appliquer la régulation à chaque moteur. \\
-{{:robotics:asservissement_vitesse_moteur.jpg?200|}}
+{{:robotics:asservissement_vitesse_moteur.jpg?400|}}
+=== Implémentation ===
+**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).
+<code c>
+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);
+}
+</code>
 ==== Asservissement de vitesse du robot ====
 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.
@@ Ligne 16: / Ligne 121: @@
 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
+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.
+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. \\
-{{:robotics:asservissement_vitesse_robot.jpg?200|}}
+{{:robotics:asservissement_vitesse_robot.jpg?600|}}

Geonobot Wiki

Outils pour utilisateurs

Outils du site

Différences

Outils de la page