Calcul de la moyenne
Le calcul de la moyenne des valeurs renvoyées par le Wii Motion Plus à vitesse nulle est une étape très importante. Cette moyenne nous donne notre zéro de référence. Le moindre écart entre la moyenne calculée et la moyenne réelle sera ajouté à chaque étape d’intégration, participant à la dérive. Malheureusement pour nous, la moyenne, même sur 2000 valeurs, bouge grandement.
Sur le graphique ci-dessous, on peut imaginer que la moyenne réelle est de 7575,5. Si nous avons la malchance de calculer notre moyenne autour de t=66s, nous aurions un écart de 1 par rapport à la moyenne réelle. 1 valant 1/20 de degré par seconde, cela nous donne une dérive de 4,5° au bout de 90 secondes. Ce n’est pas catastrophique, mais cela justifie d’utiliser une moyenne sur 2000 valeurs et non sur 100 valeurs.
De plus nous observons que la moyenne varie aussi sur des temps longs (de l’ordre de 30 secondes). Ceci peut expliquer en partie le comportement de la dérive sur plusieurs minutes comme illustré ci-dessous.
Calibration
Le Wii Motion Plus présente deux modes de fonctionnement. Le premier, que nous appellerons "mode lent", est actif lorsque le Wii Motion Plus bouge à basse vitesse. Le second, que nous appellerons "mode rapide" est actif lorsque le Wii Motion Plus bouge rapidement. La seule différence entre ces deux modes est l’échelle des valeurs.
Mode lent
Pour calibrer le mode lent, la méthode est relativement simple. Après un calcul de la moyenne à vitesse nulle, il est possible d’intégrer les écarts à cette valeur. Ne pas oublier de tenir compte du pas de temps pour l’intégration. Après avoir fait tourner le Wii Motion Plus d’un demi-tour, on obtient la valeur de 180°. J’obtiens un chiffre proche de 3600 et j’en déduis que l’information de vitesse est renvoyée en 0.05°/s, soit 1/20 de degré par seconde.
Mode rapide
Calibrer le mode rapide est une histoire bien plus compliquée parce qu’il n’est pas facile de connaître le déplacement réel effectué en mode rapide. La méthode la plus simple requiert l’utilisation d’un tableur, d’un obstacle que l’on pourra considérer comme fixe et d’un logiciel permettant de récupérer les valeurs du Wii motion plus dans le tableur. Il est important de récupérer la valeur de la vitesse, le bit spécifiant l’activation du mode rapide et le nombre de pas de temps entre deux valeurs.
Au début de l’acquisition le Wii Motion Plus est calé contre l’obstacle. Le Wii Motion Plus est ensuite décalé rapidement de l’obstacle (en le faisant tourner) puis y est re-plaqué doucement. Le mouvement est répété plusieurs fois de suite avec un temps de pause à chaque fois que le Wii Motion Plus est plaqué contre l’obstacle.
Au tableur, les données sont importées puis intégrées en se servant d’un coefficient multiplicateur de 1 pour les valeurs reçues en mode rapide. Ce facteur doit pouvoir être changé aisément. Normalement, les valeurs de l’angle du Wii Motion Plus plaqué contre l’obstacle doivent se décaler après chaque mouvement, toujours dans le même sens. A l’aide du tableur il est possible de calculer les écarts entre la première valeur et celles des plaquages suivants. En créant un critère de qualité, on peut ensuite déterminer empiriquement un facteur pour que ce critère soit optimal.
Dans un premier temps j’utilisais un critère qui était la somme des écarts. Me souvenant que le Wii Motion Plus dérive naturellement d’1° par minute (à peu près), j’ai pondéré les écarts par un coefficient 1/n (n étant le numéro du plaquage). En fin de compte, les deux critères donnaient à peu près le même facteur.
Il convient de valider le facteur en procédant à une deuxième acquisition du même type.
En réalisant une acquisition avec le mouvement inversé, Wii Motion Plus écarté lentement de l’obstacle, re-plaqué rapidement, j’ai obtenu un facteur différent. Les facteurs obtenus étaient autour de 4 ou 5... J’ai choisi de garder deux facteurs, un par sens. Une fois ces deux coefficients choisis, j’ai pu faire tourner le Wii Motion Plus dans un sens puis dans l’autre sans trop le ménager et constater une très faible dérive semblable à celle observé en statique. Cependant, une dérive notable apparaît si le Wii Motion Plus tourne à grande vitesse, de l’ordre de 800°/s. Il est possible que l’alimentation en 5V du Wii Motion Plus fausse la linéarité du capteur, mais je ne vois pas de protocole réalisable permettant d’étalonner le capteur en fonction de la vitesse de rotation.
Et on recommence...
Pour l’instant, notre microcontrôleur ne se contente que de récupérer les valeurs du Wii Motion Plus et de nous les envoyer. C’est cool, mais pour un robot autonome, ça ne sert à rien. Il faut que les calculs soient réalisés à bord du microcontrôleur.
D’un autre côté, une simple erreur de transmission entre le microcontrôleur et le PC et nos calculs se faussent. C’est ce qui m’arrive lorsque j’utilise un code de contrôle des servomoteurs se basant sur les interruptions et que j’utilise en même temps le code de microchip pour la liaison USB (classe CDC).
La grande difficulté réside dans le choix des types de variables. C’est peut-être un tort, mais je ne crois pas à l’utilisation des types à virgule flottante.
Le plus petit déplacement que nous devons être capable d’enregistrer est le déplacement dû à la plus petite vitesse détectable pendant un pas d’intégration, soit 1/20e de degré par seconde pendant 3 millisecondes, soit 0,00015 degrés.
La valeur de l’angle du Wii Motion Plus doit pouvoir dépasser 360°. Nous devons donc avoir au moins 2 400 000 valeurs possibles. Ceci demande un type d’au moins 22 bits :
Nombre de bits = ln(2400000)/ln(2)
Mais n’oublions pas notre moyenne. Nous aurons forcément un écart entre la moyenne calculée et la moyenne stockée pouvant atteindre 1/2 unité si l’opération d’arrondi se passe bien, d’une unité si l’opération d’arrondi se passe mal (troncature).
Avec la précision précédente cela nous fait une erreur maximale de 2,25° ou de 4,5° au bout de 90 secondes. Une erreur qu’il faut rajouter à celle due à la fluctuation de la moyenne au cours du temps.
A chaque bit que nous rajoutons à notre précision, nous divisons notre nouvelle erreur par 2. Nous avons choisi de rajouter 2 bits, un choix comme un autre. Cela nous fait un nombre sur 24 bits au minimum. Dans un premier temps, nous commençons les calculs avec un nombre sur 32 bits. Ceci nous laisse une marge plus que suffisante.
Pour le calcul de la moyenne à vitesse nulle, le microcontrôleur additionne les 2000 valeurs avant de faire une division. Notre code est le suivant :
moyenne_X = moyenne_temp/(2000 >> F_PRECISION);
// F_PRECISION est le nombre de bits rajoutés pour notre moyenne.
Ceci revient à décaler la valeur de la moyenne vers la gauche. Ensuite le microcontrôleur récupère la valeur du Wii Motion Plus, reconstitue la vitesse, décale celle-ci vers la gauche pour être homogène avec la moyenne puis y soustrait cette moyenne. Pour l’instant le code ne supporte que le mode lent :
vitesse_X = ( (long)( (int)wmp[0] | (((int)wmp[3] & 0xFC) << 6)) << F_PRECISION ) - moyenne_X;
Et enfin, nous passons à l’étape d’intégration :
// timer est le nombre de pas de temps écoulé depuis la dernière intégration
angle_X += vitesse_X *timer; // en 1/(2^F_PRECISION * 333) °
L’angle est enfin calculé, il est prêt à être envoyé à l’ordinateur pour conversion et affichage. Dans la pratique, presque toutes les variables ci-dessus ont été envoyées à l’ordinateur afin de déboguer le programme.
Asservissement d’un servomoteur
Maintenant que le microcontrôleur connaît la valeur de l’angle, on peut passer aux choses intéressantes. Nous pouvons, par exemple, fixer le Wii Motion Plus à un servomoteur et demander à celui-ci de toujours être tourné dans la même direction quels que soient les mouvements de son support.
Pour information, voilà la ligne de code pour l’asservissement du servomoteur. Elle est appelée toutes les 45 millisecondes :
servo_position = Servo_Get(0) - ((signed int)((signed long)(angle_X)/1600))/5;
Et voilà la vidéo :