Dans l’article précédent de cette série, « Un robot arduino« , j’ai vu comment lire les données d’un gyroscope avec un arduino. Aujourd’hui, cinquième article, je place ce gyroscope sur le NoRobo et je prépare le code pour connaitre l’inclinaison du robot vers l’avant ou vers l’arrière. Il a toujours 3 roues et cette fonction est inutile pour l’instant. Mais c’est le préalable pour que le robot tienne « debout » sur deux roues !
Positionner le gyroscope
La dernière fois, j’avais vu qu’il fallait placer le gyro à l’horizontale, led vers le dessous, avec la flèche X pointant vers l’avant du robot et la flèche Y dans le même axe que les roues.
J’ai scotché le gyro sur une petite plaque, en faisant attention à ce qu’il soit le plus droit possible. Ensuite j’ai mis cette plaque sous le robot, en respectant le sens prévu pour X et Y, et donc Z qui va pointer vers le bas du robot (vers nous sur la photo).
Les broches suivent l’ordre de ce qui est écrit sur la fiche : de la gauche (VCC) à la droite (INT).
J’ai ensuite positionné le gyroscope le plus possible dans l’axe des roues. Pour l’instant il n’y a pas assez d’espace entre les moteurs et la plaque au dessus. J’ai donc placé le gyroscope un peu vers l’avant.
Ajouter le code pour lire les données du gyroscope
Au début, j’ai tenté d’ajouter bêtement (c’est confortable !) le code proposé par le tutoriel du robot à balancier déjà cité. Malheureusement, ça n’a pas du tout fonctionné comme espéré puisque la bascule avant / arrière n’était même pas repérée…
J’ai donc pris le temps de comprendre quels étaient les axes « engagés » dans le robot à balancier du tutoriel et dans le mien. J’avoue que j’y ai passé énormément de temps, je me représente très mal l’espace… Mais voilà le résultat :
J’en ai donc déduit qu’il suffisait de chercher et remplacer les variables suivantes :
- AcX devient AcZ;
- AcY devient AcX;
- GyZ devient GyY.
Le code qui en résulte est NoRobo-Joystick-BT-commander-2016-04-12B.ino (disponible sur GitHub ici également). Les commandes de lecture / écriture bluetooth sont désactivées et j’ai ajouté des Serial.println pour afficher les valeurs de AcX, AcZ et GyY.
Au démarrage, le programme me dit de placer le robot à l’horizontale sur sa face avant ou arrière. J’ai même ajouté des plaques à la structure pour être bien à plat sur la face arrière malgré le cordon série. Pendant un certain temps, il calibre le gyro (il en calcule les « offset », valeurs dont il tiendra ensuite compte pour corriger les lectures en cours de fonctionnement.
Ensuite, le programme affiche les valeurs de AcX, AcZ et GyY. Au dessus, on voit les premières valeurs mesurées alors que le robot est couché sur son arrière. Logiquement robot_angle devrait être à -90, et les accélérations à 0 puisque le robot ne bouge pas du tout… On voit bien qu’il y a un problème.
Lorsque je remet le robot « debout », je lis les valeurs suivantes :
GyY = 117 | AcX = 4624 | AcZ = -14732 | robot_angle = 15.49 | Acc_angle = 17.43
Et si je laisse le robot sans bouger pendant plusieurs minutes, je vois que l’angle du robot reste stable, même si sa valeur devrait être proche de 0. Par contre, si je penche le robot vers l’avant puis je le remet « debout », les nouvelles lectures sont proches des valeurs suivantes :
GyY = 110 | AcX = 4564 | AcZ = -14824 | robot_angle = 3.98 | Acc_angle = 17.11
L’angle réel du robot est inchangé, proche de 0, mais sa valeur mesurée a beaucoup changé !
Je refais la même opération, mon angle devient négatif alors que le NoRobo est toujours debout !
GyY = 57 | AcX = -1 | AcZ = -1 | robot_angle = -44.51 | Acc_angle = -45.00
Disons que ce programme ne calcule pas réellement l’angle du robot.
Une cause possible des anomalies est que j’alimente l’arduino avec le port USB d’un ordinateur pour ces essais. La tension réelle aux bornes du gyro est de seulement 4.3V. Mais si j’alimente l’arduino avec une autre source, tout en conservant la liaison USB, le problème reste identique.
La cause du bug vient donc du calcul, qui est peut-être faux avec cette orientation du gyroscope. Il faut donc que je comprennes les calculs à réaliser pour donner l’angle du robot…
Comment fait-on le calcul de l’angle ?
Il existe un guide en français plutôt compliqué et pas spécifique au MPU 6050. J’ai préféré d’autres articles en anglais.
Dans les 3 articles de Mike Jacobs sur l’utilisation d’un gyroscope pour ses robots, il explique très simplement comment fonctionne le gyroscope et comment calculer des angles de rotation. Ca m’a permis de comprendre un peu le fonctionnement général.
Une autre personne fait part de ses essais dans son site. Dans l’article « Gyroscopes and Accelerometers on a Chip« , notre GeekMom (Maman Geek !) explique le fonctionnement d’un gyroscope MPU 6050 et la façon de calculer les angles. Voici ce que j’en ai compris (et jugé utile) :
- son gyro, un gy-521 comme le mien, n’est connecté que par les broches SDA, SCL, GND et VCC. En particulier, elle n’utilise pas INT, qui gère les interruptions. Je suis dans la même situation.
- Elle récupère les données brutes (raw data) d’une manière semblable à celle que j’utilise, mais beaucoup plus compliquée car elle accède à tous les registres du gyroscope.
le gyroscope utilise la gravité (9.81 N vers le bas) comme base de calcul. Si l’axe Z est dans l’alignement de la gravité (donc vertical), il est impossible de calculer l’angle de rotation autour de cet axe. Et j’ai positionné mon gyroscope de telle sorte que l’axe Z est vertical…. Mais je ne cherche pas à déterminer l’angle de rotation autour de Z, donc en principe pas de souci.
- L’angle en X correspond à la rotation autour de l’axe Y, l’angle en Y correspond à une rotation négative autour de X.
- Si mon gyroscope était positionné de telle sorte que X et Y soient dans le plan horizontal (mon cas) et Z vers le haut (le mien est vers le bas), mon angle en X (GyY) serait calculé par une formule d’arc tangente, qui correspond à une tangente négative ou positive selon l’orientation de l’angle (cf Wikipedia) . AcX, AcY et AcZ sont les accélérations selon les axes X, Y et Z. Nota : pour réviser la trigonométrie, j’ai lu ce document.
- Le gyroscope calcule la vélocité angulaire – la vitesse de changement de l’angle. Selon la documentation du MPU 6050, en divisant les valeurs brutes par 131, on obtient une valeur en degré / seconde.
- On doit donc avoir une base de temps connue pour pouvoir en déduire le changement d’angle. Si on connait l’angle de départ (avec la formule ci-dessus), on peut calculer le changement en intégrant les valeurs de GyY. Mais s’il y a des erreurs de mesure (et il y en a), cette intégrale va être fausse. Pour annuler ces erreurs, on utilise des filtres, dont le plus connu est le « Kalman filter » (wikipedia), assez complexe. Le filtre « complémentaire » est une approximation plutôt juste.
- Les formules utilisées pour le filtre complémentaire sont (je cite la Maman Geek) :
Filtered Angle = α × (Gyroscope Angle) + (1 − α) × (Accelerometer Angle) où
α = τ/(τ + Δt) et (Gyroscope Angle) = (Last Measured Filtered Angle) + ω×Δt
Δt = sampling rate, τ = time constant greater than timescale of typical accelerometer noise
I had a sampling rate of about 0.04 seconds and chose a time constant of about 1 second, giving α ≈ 0.96.
Le sketch GY_521_send_serial utilise ces formules. Il est disponible sur le site de la Maman Geek ici, tout en bas de l’article.
En examinant le code utilisé sur le NoRobo, je ne parviens pas à voir de défaut. Je suis presque certaine que l’erreur provient d’un signe devenu mauvais en changeant la position du référentiel (mon gyro est horizontal, celui du tutoriel est vertical et perpendiculaire à l’axe des roues). Mais quand je vois les difficultés que j’ai eu simplement à positionner x, y et z sur le NoRobo, je me sens incapable de trouver l’erreur.
Et en plus, j’ai quand même un doute sur le fonctionnement de mon gyroscope. Est-ce que je le calibre correctement ? Est-ce qu’il est capable de fournir des données stables et justes ?
Comment calibre-t-on gyroscope ?
J’ai d’abord essayé MPU6050_DMP6.ino, disponible dans les exemples de sketch liés à la bibliothèque MPU6050 (on peut la trouver dans le repository GitHub i2cdevlib de Jeff Rowberg. Et je me suis rendue compte qu’il fallait indiquer des valeurs d’offset pour le gyroscope. Ces valeurs sont obtenues en calibrant le gyroscope. En passant par cet article de Stan, je suis arrivée au script de Luis Rodenas dans Ce forum. Le script, MPU6050_calibration.ino, réalise la calibration et renvoie les valeurs d’offset à utiliser pour le gyroscope calibré.
A ma grande surprise, la calibration se réalise avec le MPU aussi horizontal que possible. J’ai ensuite vu cette recommandation plusieurs fois et, pour la première fois, il m’est venu à l’esprit que mon problème avec le script du NoRobo vient du fait que je fais la calibration selon les instructions, en mettant le NoRobo soit sur sa face avant, soit sur sa face arrière.
A partir de maintenant, je fais donc la calibration du gyroscope du NoRobo en plaçant le NoRobo sur sa « tête », roues en l’air.
Mais ça ne suffit pas à résoudre le problème…
Nota : cette expérience m’a aussi fait comprendre à quoi sert la broche INT du gyroscope. Elle permet de déclencher des « interruptions », c’est à dire de cadencer la lecture des mesures. Pour faire la calibration avec MPU6050_calibration.ino, j’ai dû connecter la broche INT à la broche 2 de l’arduino.
Est-ce que mon gyroscope fonctionne ?
J’ai essayé d’utiliser le sketch GY_521_send_serial réalisé par la Maman Geek, disponible sur le site ici, tout en bas de l’article. Et là, tout va bien ! J’ai placé mon robot « debout » dès le début. Le calibrage s’est fait puis La console série (19200 bauds) affiche des valeurs qui paraissent normales et qui sont stables dans le temps si on ne bouge pas le robot :
DEL:0.0430000019#ACC:0.37,-17.86,0.00#GYR:-0.23,-1.28,-0.86#FIL:0.20,-17.81,-0.86 puis 5 minutes plus tard DEL:0.0440000009#ACC:0.52,-17.75,0.00#GYR:-2.31,-15.22,-9.99#FIL:0.16,-17.74,-9.99
Avec
- DEL qui indique le délai entre deux lectures du gyroscope (ici 0.04 seconde),
- #ACC qui donne les valeurs d’accélération en X, Y et Z, en degrés par secondes, calculés avec la formule d’arctangente
- #GYR les angles gyro en X, Y et Z non filtrés
- #FIL les angles après passage au filtre complémentaire.
On observe que l’accélération en Y n’est pas nulle (-17) mais très faible, les angles bruts de gyro sont très variables mais les angles filtrés sont stables, au moins pour celui qui m’intéresse (autour de Y). Par contre, je ne vois aucune raison que cet angle soit à -17° alors que le robot est à plat. J’ai mis un niveau dessus et il n’est pas parfaitement plan mais presque.
Si je bascule le NoRobo vers l’arrière à environ 45° :
#FIL:0.85,-38.47,-8.69
et quand je retourne à la position « debout » :
#FIL:0.25,-17.89,-9.19
Si je bascule le NoRobo vers l’avant à environ 45°
#FIL:-2.00,-7.00,-9.33
et quand je retourne à la position « debout » :
#FIL:0.33,-17.66,-9.91
Et si je met le NoRobo sur sa face arrière (à -90° en principe)
#FIL:1.51,-76.63,-8.16
Basculer vers l’arrière de 90° indique un angle de approximativement -76°, soit -76+18= -58° de déplacement. Vers l’avant, le mouvement fait bien bouger l’angle positivement. Lorsque je crois avoir pivoté de 45°, je lis +20° par rapport à la position « debout » qui devrait être 0.
J’en conclus que le gyro fonctionne, même s’il est très très imprécis. Mais au moins il donne des mesures reproductibles.
Est-ce que ça n’est pas un problème d’interruptions ?
Le sketch de la Maman Geek n’utilise pas d’interruptions mais cale les mesures sur un des timers interne de l’arduino en utilisant la fonction millis().
Dans le sketch du NoRobo, il n’y a pas d’utilisation de millis() mais timer1 est supposé déclencher des interruptions. Lorsque je vais voir sur le tutoriel balancing robot, je découvre que la broche INT du gyro est connectée. C’est peut-être pour ça que mon NoRobo ne parvient pas à calculer correctement l’angle.
Dans le troisième article de cette série, j’évoquais déjà la notion d’interrupt mais sans y comprendre grand chose… Et comme je pense que le problème du moteur droit qui fait des a-coups lors du démarrage vient aussi d’un problème d’interrupt, il est temps de nous pencher sur le sujet…
Le résumé de mes recherches sur le sujet figure dans un article séparé, ici.
A l’issue de ces recherches, j’ai décidé d’essayer l’action suivante :
- connecter la broche INT du gyroscope à la broche 2 de l’arduino (les autres broches liées à des interruptions sont toutes prises dans le balancing robot, qui utilise des moteurs brushless et a donc besoin de 2 fois 3 broches PWM.
- connecter les deux moteurs sur des broches pilotées par le timer 1 ((broche 9 et 10).
Dans le programme NoRobo-Joystick-BT-commander-2016-04-15.ino (ici sur GitHub) , j’ai maintenant les instructions suivantes :
// moteur de gauche int motorG_enable = 10; //pwm sur timer1 int motorG_forward = 12; int motorG_backward = 13; // moteur de droite int motorD_enable = 9; //pwm sur timer1 int motorD_forward = 7; int motorD_backward = 8;
Conséquence pour le moteur : Le moteur gauche (qui est le droit en fait lorsqu’on regarde le robot debout, mais marqué « G ») a toujours des secousses au démarrage, jusqu’au début du calibrage du gyro. Ensuite, il tourne régulièrement pendant le calibrage puis s’arrête une fois le calibrage fini…
En fait, j’ai finalement résolu le problème en ajoutant les lignes suivantes dans le setup :
// empecher les moteurs de demarrer durant le setup pinMode (9, OUTPUT); pinMode (10, OUTPUT); digitalWrite(9, 0); digitalWrite(10, 0); // FIN empecher les moteurs de demarrer durant le setup
Ca met à 0 les deux « motor_enable » et ils ne peuvent donc pas démarrer sans recevoir un ordre.
Conséquence pour le gyro : ça ne change rien…
Il est clair que je n’ai pas vraiment compris la cause de ces deux problèmes…
Quelles solutions ?
Quant au gyro, je pense que le plus simple sera d’utiliser les éléments du code de la Maman Geek pour calculer l’angle de mon NoRobo.
La suite au prochain numéro de cette série Un robot arduino !
