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 :
|
|
Le synoptique :
Le challenge :
Réaliser le plus rapidement possible le parcours suivant :
- Départ : entrer par la porte du fond de la J101
- Arrivée : sortir par la porte de devant de la J101
objectif : faire un meilleur chrono que les 2TPIL !
Comment mesurer l'angle de lacet (yaw) avec le MPU6050 ?
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.
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 :
Après test, l'appel de la méthode
.getRotation
demande environ 1 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()
|
Pour faire une intégration en analyse numérique, il
suffit de faire la somme des
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. |
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()
|
Au bilan les tests sont satisfaisants :
- la dérive est faible,
- les mesures du yaw sont fiables, justes et fidèles au degrès près,
- si on limite un peu la vitesse de rotation, nous réalisons des demi-tours précis.
Nous pouvons donc valider cette centrale inertielle pour la suite du projet.
Comment structurer le programme ?
Le programme principal doit:
Comment respecter notre cadencement à 2 ms ?
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é :
Comment contrôler la bonne exécution de notre consigne ?Pour effectuer notre Demi-Tour nous exécutons
correction(1800)
|
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() {}
|
La télécommande
Pour
prendre en main et tester le robot nous allons brancher un dongle
Bluetooth sur le PC.
Grâce au moniteur série nous allons tester notre programme Arduino à distance, en envoyant des trames du type :
255;0; soit : vitesse=255;cap=0°
Ainsi le moniteur série va nous aider à débugger le programme et à trouver les bons réglages du PID.
Pour la suite nous allons utiliser une télécommande programmée en Labview.
Pour transmettre des informations de l'arduino vers la télécommande, il faut :
- bien structurer la trame émise,
- respecter l'ordre d'émission,
- des expressions régulières ("c:", "A:" ...)
permettent à la télécommande de valider la trame reçue
et d'isoler les variables qui nous intéressent. - voici la fonction qui nous sert de base.
Pour être plus mobile, nous allons développer une appli télécommande pour notre smartphone avec appInventor.