L'odométrie c'est quoi ?
L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Le terme vient du grec hodos (voyage) et metron (mesure).
Sur le robot Pilou, on dispose de capteurs sur chaque roue. Le processeur chargé de l'odométrie va donc calculer le déplacement et la vitesse de chaque roue.
Voyons comment on fait :
Le déplacement
Principe
Les capteurs utilisé par roue sont deux senseurs situés à 90° au centre desquels tourne une petite roue munie d'aimants... Il y a 3 aimants.
On va donc avoir 6 inversion de polarité sur chaque capteur par tour de moteur donc 12 au total.
Le moteur est doté d'un réducteur 1:30 : on obtiendra donc 360 changements de polarité par tour de roue soit 1 changement par degré.
Les roues font 10cm de diamètre donc : une inversion = 0,87mm
Pour connaitre le sens de rotation, il faut lire les 2 capteurs : le changement de polarité de l'un avec l'état de l'autre nous donne le sens de rotation du moteur...
Le logiciel
Le logiciel doit être suffisamment rapide car il aura a agir 2 fois tous les 0,87mm ... pour un robot qui parcours 3 mètres par seconde, cela fait 7000 fois par seconde donc tous les 140 µ secondes.
Pour ce faire, il existe sur le microprocesseur 4 entrées (RB4 à RB7) qui ont comme propriété de provoquer une interruption dès que l'un d'eux change d'état... On va donc connecter les 4 capteurs à ces broches et attendre les interruptions.
Pour avoir un code rapide comme l'éclair, on utilise la technique suivante :
on crée un octet composé de deux fois les 4 bits actuels et précédents
On mémorise les 4 bits actuels pour les utiliser comme précédent la prochaine fois
On utilise l'octet pour chercher une valeur dans une table : cette valeur peut être 0 , +1, -1 suivant qu'on doit ne rien faire ou ajouter un ou retrancher un au déplacement. Il existe une table pour la roue gauche et une table pour la roue droite. Chaquer table fait 256 octets.
Voilà .... le plus fastidieux est de garnir les 2 tables : il faut analyser, roue par roue quelle est la situation actuelle, quelle était la précédente et déduire s'il faut touvher au compteur de distance...
Mais c'est plus rapide que l'éclair (environ 50 instructions machine)
Voici donc le code de l'interrupt
/****************************************
/ Interrupt changement d'état RB4 RB7
****************************************/
// Pour les encodeurs de roues droite et gauche
#int_RB
void RB_isr() {
codeur_N = input_b() & 0xF0; // 4 bits poids fort = nouvel état
codeur_A >>=4; // ancien état sur 4 bits poids faible
codeur_N += codeur_A; // ou pour faire l'index
codeur_A = codeur_N & 0xF0; // mémorisation du nouvel état dans l'ancien
deplacement_G += dep_G[codeur_N]; // déplacement roue gauche
deplacement_D += dep_D[codeur_N]; // déplacement roue droite
}
et les tables utilisées :
signed byte const dep_D[256] =
{
0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
};
signed byte const dep_G[256] =
{
0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
};
en résultat, on a deux "long" qui contiennent la distance parcourue par chaque roue : deplacement_G et deplacement_D.
Simple non?
Eh bien pas tout à fait : il faut prévoir le débordement des compteurs au bout d'un moment.... mais ceci n'est pas traité dans ce processeur mais dans le processeur de navigation.....
La vitesse
Puisque ce processeur est dédié à l'odométrie, on en profite pour qu'il calcule en permanence la vitesse de chaque roue... en effet, il faut mieux la calculer dans un processeur qui a peu de traitement compliqués et qui assurera un calcul fiable.
Qu'est ce que la vitesse? C'est la distance parcourue par unité de temps... oui mais quelle unité de temps choisir pour calculer? une fois par heure? trop long, une fois par 10msec? trop court...
Principe
On va mettre en interruption horloge toutes les 10 msec, un programme qui va
-
Stocker la différence de déplacement pae rapport au dernier passage dans un buffer "tournant"
-
Calculer le déplacement réalisé durant les 50 dernières millisecondes : ce sera la vitesse en unité de déplacement par 50msec.
On fait donc un calcul toutes les 10 milisecondes qui correspond à la vitesse moyenne des 50 dernières millisecondes
Le Logiciel
Voici donc le code du logiciel qui tourne sous interruption horloge 10 msec :
/***************************************
/ Interrupt Timer 0 (toutes les 10msec)
***************************************/
#int_TIMER0
void TIMER0_isr()
{
byte ilire;
byte index;
byte tempVitesseG;
byte tempVitesseD;
set_timer0(VAL_TMR0); //période = 10,0352 msec
// stockage de la vitesse dans le bon slot
cptTabVitesse++; // échantillonnage tics/10,0352msec
if (cptTabVitesse== NBTABVIT ) cptTabVitesse=0;
tabVitesseG[cptTabVitesse]= deplacement_G - oldDeplacementG ;
tabVitesseD[cptTabVitesse]= deplacement_D - oldDeplacementD ;
oldDeplacementG = deplacement_G; // mémorisation valeur précédente du déplacement
oldDeplacementD = deplacement_D;
// calcul de la vitesse sur les x derniers slots
cptLireVitesse++;
if (cptLireVitesse== NBTABVIT ) cptLireVitesse=0;
tempVitesseG=tempVitesseD=0;
for (index=0; index<NBSLOTSVITESSE ; index++){
ilire=cptLireVitesse+index;
if (ilire>=NBTABVIT) ilire =ilire-NBTABVIT;
tempVitesseG += tabVitesseG[ilire];
tempVitesseD += tabVitesseD[ilire];
}
vitesseG= tempVitesseG ; // maj vitesse : pas besoin d'inhibit pour la lire
vitesseD= tempVitesseD ;}
et les déclarations dans le fichier .h :
// Calcul de la vitesse
#define NBTABVIT 10 // taille totale de la table
byte tabVitesseG[10]; // table des vitesses instantanées
byte tabVitesseD[10];
#define NBSLOTSVITESSE 5 // nombre de slots de 10msec pour calculer la vitesse
byte cptTabVitesse; // le compteur de remplissage
byte cptLireVitesse; // le compteur de début de lecture pour le calcul de vitesse
byte vitesseG; // la vitesse lissée sur les NBSLOTSVITESSE dernières 10sec
byte vitesseD;
byte accelG; // l'accélérationgauche
byte accelD; // l'accélération roue droite
long oldDeplacementG; // valeur ancienne du déplacement
long oldDeplacementD;
simple ? oui .. ici, pas de problèmes
Fin
Voilà .... il reste au processeur d'odométrie à transmettre ces informations par le bus I2C au processeur d'asservissement quand ce dernier voudra bien venir les chercher....Il aura alors la dernière valeur toute fraiche du déplacement et de la vitesse
