RobotGyro : un projet qui file droit !

Cahier des charges :

Les robots à plusieurs moteurs sont pratiques à manoeuvrer mais ne vont pas droit !

A l'aide d'un MPU6050 et de la sortie de son gyroscope gz :

• réaliser une régulation de cap pour filer droit !
• asservir les changements de direction,
• limiter la vitesse lors des changements de cap,
• avec app inventor réaliser une appli qui contrôle la vitesse et la direction du robot.
  

Les bibliothèques à inclure pour utiliser le MPU6050 sont : MPU6050, I2Cdev et Wire.

Une petite vidéo de présentation : U=RI | Arduino Ep.19 - La centrale inertielle MPU-6050

Pour notre part c'est uniquement la vitesse angulaire gz qui nous intéresse.

• Elle nous servira pour limiter la vitesse lors des changements de direction,
• Après une intégration elle nous permettra de calculer l'angle de lacet (yaw) pour garder le cap...

Attention : le format int est en dépassement au delà de 32768 d'où l'utilisation d'un format long .

// Bibliothèques utilisées et variables globales
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
MPU6050 gyro;
int gx, gy, gyz;
int offset,angle;
long Gz,gz;   // risque de dépassement 

Dans le setup nous allons appeler la fonction iniGyro()

Pour sa première utilisation nous allons calibrer l'offset :

• on élimine les 200 premières mesures,
• on fait la moyenne des 1000 mesures suivantes,
• on attend 2 ms entre 2 mesures pour éviter d'avoir des mesures en double,
• par la suite on utilise cette moyenne comme offset, pour notre MPU6050 nous avons obtenu offset=173.

Après test, l'appel de la méthode .getRotation demande environ 1 ms.
Aussi nous avons fixé la durée de notre cycle à dt = 2 ms.

La période d'échantillonnage (dt=Te) doit être régulière et très petite devant la constante de temps du process.

Cela permet de réduire l'erreur dans le calcul de l'intégrale par la méthode détaillée ci-dessous.

void iniGyro() {  // Pour initialiser le capteur dans le setup
      Wire.begin();
      gyro.initialize();
      Gz=0;
      /* Pour calibrer offset lors d'une première utilisation
      Serial.println("Ne pas bouger, reglage offset...");
      for (int i=0; i < 200; i++) {            // on élimine les 200 premières mesures
        gyro.getRotation(&gx, &gy, &gz);
        delay(2);
      }
      for (int i=0; i < 1000; i++){
          gyro.getRotation(&gx, &gy, &gz);
          Gz+=gz;
          delay(2);
      }
      offset=-(Gz/1000);     // Moyenne sur 1000 mesures
      Serial.println("Offset="+String(offset));
      Gz=0; */  
      offset=173;
}   // iniGyro()

La fonction calculAngle() nous permet de calculer

Pour faire une intégration en analyse numérique, il suffit de faire la somme des
Pour rester précis il faut choisir dt petit. Nous avons choisi dt = 2000 µs = 2 ms

Pour éviter le calcul de Kg, Mathieu a préféré utiliser la fonction map ...

Après avoir fait faire un demi tour à notre robot, nous avons trouvé Gz/1000 = 23380.
Cette manoeuvre nous a permis grâce à map de convertir Gz en degré sans avoir à calculer Kg !
Pour plus de précision nous avons exprimé l'angle en dizième de degrés (1800 -> 180°)

void calculAngle() {
  gyro.getRotation(&gx, &gy, &gyz);
  gz=gyz+offset;  // Correction Offset
  Gz+=gz*(dt/1000);  // somme (pour integration)
  angle=map(Gz/1000,-23380,23380,-1800,1800);
}  // calculAngle()

Le programme principal doit:

• être simple, lisible et bien structuré,
• être facile à modifier,
• faire appel à des fonctions,
• être cadencé de manière précise.

Comment respecter notre cadencement à 2 ms ?

• définir n : nb de cycles et utiliser la fonction % (modulo),
• tout les 100 cycles soit 200 ms (n%100==50) : contrôler s'il y a un ordre de reçu,
• tout les 100 cycles (n%100==0) : écrire les paramètres à transmettre sur le port série.
  Pas plus de  64 caractères pour ne pas dépasser le buffer série...
•  la fonction telecommande() sera remplacée au début par d'autres fonctions
  selon les tests que l'on souhaite réaliser.
• la structure du programme nous permettra d'ajouter par la suite la fonction radarUS()
  Tout les 100 cycles, si la consigne est de 0°, faire un cycle à dt = 10000 µs
  Contrôler et détecter la présence d'obstacle proche grâce au capteur à ultrason

Comment tester et débugger notre programme ?

Dans un premier temps nous allons réaliser un parcours simple : aller tout droit.

Puis un parcours plus élaboré :

• aller tout droit (Marche Avant) durant 6 s (soit 3000 cycles),
• faire un Demi-Tour (consigne à 180° soit 1800)
• revenir durant 6 s,
• faire un Demi Tour et s'arrêter.

Comment contrôler la bonne exécution de notre consigne ?

Pour effectuer notre Demi-Tour nous exécutons  correction(1800)
Losque la nouvelle consigne est correctement exécutée nous la validons et la remettons à 0. Solution possible :

• On peut définir un compteur precis
  qui s'incrémente de 1 à chaque cycle si l'écart < 10 (1°) et consigne ≠ 0 sinon precis =0.
• Si precis > 50  on considère que la nouvelle consigne est validée,
  on remet la consigne à zéro (en mode télécommande, cela autorisera l'envoi de nouvelle consignes...)
  on remet l'angle à zéro (Gz=0;precis=0;)...

void setup() {
  Serial.begin(9600);
  delay(500);
  iniMoteur();  // initialisation des pinMode Moteur
  iniGyro();
}  // setup()

void loop() {
   t0=micros();  // top départ 
   calculAngle();
   if (n%100==50) lireSerie();
   if (vitesse>0) telecommande();
                else Moteur(0,0);
   if (n%100==0) debug(); 
   while (micros()-t0<dt);  // loop() cadencée à dt µs
   n++;
}  // loop()

// Calcule l'angle de lacet grâce au MPU6050
void calculAngle() {}

// Transmet par port série les paramètres
void debug() { } 

// Reçoit les consignes
void lireSerie () {}

// Pilote les moteurs
void Moteur(int droite, int gauche) {}

// Correcteur PID (cf le synoptique)
void correction(int consigne) {}

// Parcours test : 6s en MA, DT, 6s MA, DT, STOP.
void allerRetour() {}

// Parcours piloté par la télécommande
void telecommande() {}