NoRobo : Filtre complémentaire pour le calcul de l’angle

Ceci est le sixième article de la série Un robot arduino. J’avance en compréhension mais pas en résultats depuis le deuxième article…

Je poursuis ici les tentatives de faire fonctionner correctement le filtre complémentaire sur le gyroscope MPU6050. Ca va balancer !

Où est-ce que j’en suis ?

Mon code à ce stade est NoRobo-Joystick-BT-commander-2016-04-15W.ino (sur GitHub).

Mon schéma de montage (modifié pour cabler différemment les moteurs) est représenté ci-dessous avec 123d.circuits.io :

Les connexions sont donc les suivantes :

Sur les 16 pattes du L293D :

Le composant HC-05 pour le bluetooth est connecté comme suit :

Quant au gyro,

Est-ce que le robot fonctionne ?

Attention, si la liaison USB est connectée à l’ordinateur, la liaison bluetooth ne fonctionne pas. Dans ce code, on ne peut avoir qu’une seule liaison série.

Le robot fonctionne. Le joystick le fait correctement aller vers l’avant, l’arrière, à droite ou à gauche.

J’ai donc une base saine pour poursuivre les travaux !

Calculs avec le filtre complémentaire de la Maman Geek !

Mon montage est celui qui est présenté plus haut mais j’ai débranché les ports RX et TX du HC-05 et connecté la liaison série USB pour utiliser la console série.

J’ai également débranché la broche INT du gyroscope.

J’ai ouvert côte à côte le sketch actuel du NoRobo et le sketch de la GeekMom, GY_521_send_serial.ino, disponible sur le site de la Maman Geek ici, tout en bas de l’article.

Let’s go !

J’ai copié la quasi totalité du code de la Maman Geek en remplacement du code relatif au gyroscope du NoRobo. Le sketch, NoRobo-Gyro-2016-04-15A.ino, fait maintenant plus de 1400 lignes…

Ca ne fonctionne pas correctement : il n’y a pas de temps indiqué, et l’angle dérive au fil du temps.

Une hypothèse est qu’en modifiant l’horloge 0, j’ai modifié les valeurs de millis(). C’est même certainement ça (je suis contente d’avoir passé tant de temps à comprendre les horloges de l’arduino Uno…).

J’essaie donc d’enlever les changements de l’horloge 0. Normalement la lecture du smartphone android en bluetooth se fait via l’horloge 1.

Je commente donc les lignes 1304 à 1309 :

/* DO NOT CHANGE TIMER 0
  TCCR0A = 0;
  TCCR0B = 0;
  TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM00);
  TCCR0B = _BV(CS00);
* DO NOT CHANGE TIMER 0 */

Je recharge le sketch modifié. Et ça ne change rien…

C’est un problème de timer quand même car si au setup je commente Bl_Setup();, qui lance le setup des moteurs, mais aussi des horloges, j’obtiens une valeur pour le temps écoulé.

En fait, j’obtiens les mêmes valeurs que GeekMum lorsque :

• Serial.begin est réglé à 19200 bauds, comme dans GY_521_send_serial.ino;
• Dans le setup, je commente B0_setup(); : les timers ne sont pas modifiés, mais évidemment les moteurs ne sont pas non plus initialisés ;
• j’enlève les fonctions liées au bluetooth de la loop, bluetooth(); et sendBlueToothData();

J’ai alors une boucle d’environ 0.041 secondes, comme avec GY_521_send_serial.ino.

Que faire ?

Je ne comprends pas ce qui se passe…

Mais je constate que dès lors qu’il n’y a pas de paramétrage des horloges, il y a bien un temps constant entre deux mesures.

Je peux donc :

• enlever toutes les modifications d’horloge et la fonction liée à l’overflow du timer 1
• régler serial.begin à 57600 bauds puisque c’est la vitesse à laquelle j’ai réglé mon module bluetooth (pourquoi s’embêter à le modifier…)
• remplacer les opérations réalisées selon la valeur de freqCounter par des opérations liées au délai mesuré par les parties de code liées au gyroscope.
• Vérifier que ça fonctionne :
  • robot marche avec le joystick android ; OUI !
  • le délai est stable dans la console série et l’angle idem.
• Voir dans le module série quel est le délai moyen calculé
• faire les calculs de filtre complémentaire sur la base de ce délai

Je le fais dans le sketch NoRobo-Gyro-2016-04-15B.ino.

Pour calculer le délai moyen, j’ai légèrement modifié NoRobo-Gyro-2016-04-15B.ino et il devient NoRobo-Gyro-2016-04-15-MEAN-TIME.ino

J’obtiens les valeurs suivantes au bout de 1024 mesures :

Comptage : 1024
moyenne : 0.014285
Minimum : 0.004000
Maximum : 0.017000

Ma boucle se réalise toutes les 0.014 secondes.

Et comme nous avons vu dans le précédent article que notre GeekMum utilise les calculs suivants :

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.

Il me suffit maintenant d’aller modifier NoRobo-Gyro-2016-04-15B.ino en ligne 1040  et NoRobo-Gyro-2016-04-15-MEAN-TIME.ino () en ligne 1050 pour que :

float alpha = 0.96; devienne float alpha = 0.986; , soit 1 – 0.014285

Une fois que tout s’est stabilisé (au bout de moins d’une seconde)

DEL:0.0139999998#ACC:-1.02,9.33,0.00#GYR:-0.54,0.29,-0.01#FIL:-0.94,9.56,-0.01

Il est 18h35. Je vais faire autre chose. Lorsque je reviens, mon angle doit être proche de 9.56 toujours !

A 18h51, l’angle est toujours autour de 9.56 !!!

J’ai réussi cette étape !!!!!

Voici les deux sketch utilisés :

• pour trouver la durée moyenne entre deux échantillonnages du gyroscope : NoRobo-Gyro-2016-04-15-MEAN-TIME.ino
• Pour calculer correctement (Qu’est ce que je suis contente ! 🙂 🙂 🙂 🙂 ) : NoRobo-Gyro-2016-04-15B.ino

Et maintenant ?

je suis à deux doigts de balancer le robot ! Il ne me reste plus qu’à comprendre quelques détails. J’y crois !

La suite au prochain numéro de cette série Un robot arduino !

Ca va balancer, prochainement !