Code : Localisation sur le terrain

On a décidé de commencer l'asservissement. Avant de pouvoir réaliser cet asservissement il faut pouvoir connaitre le positionnement du robot sur le terrain.


Matériel


Pour ça on a monté une base roulante avec le minimum requis, c'est-à-dire :

  • une base (en bois, octogonale)
  • des roues motrices (au centre)
  • des roues codeuses (extérieur)
  • des codeurs (entre les roues centrales et les roues sur l'extérieur, cachés dans un boitier rouge)
  • un Arduino Mega (en bas à gauche)
  • un montage électrique (en bas à droite) pour connecter les sorties des codeurs à l'Arduino




Positionnement du robot sur le terrain

 

Principe général


Pour connaitre le positionnement du robot sur le terrain, il faut:
  • la position en x et en y du robot
  • l'angle de direction du robot
  • le référentiel : les positions x, y et l'angle initial du robot par rapport auxquels on va définir les autres valeurs


Gestion des codeurs


Les codeurs utilisés sont des simples codeurs à 600 impulsions par rotation sur deux phases. Ils contiennent un disque percé régulièrement qui permet avec un capteur optique de détecter les tours. La sortie est une alternance de signal haut et bas.
La technologie est similaire à ce qu'on peut trouver dans une roulette de souris ou dans les anciennes souris à boules.
Chaque codeur a 2 sorties avec un léger décalage qui permet de détecter le sens de rotation.

 

 

On avance ou on recule ?


L'Arduino permet de récupérer pour chaque sortie de chaque codeur les changements d'état (montée et descente).
Les sorties de chaque codeur correspondent aux entrées sur l'Arduino.

Pour exemple, on définit que lorsque l'on avance, les sorties du codeur sont comme suit :

En regardant l'entrée 1 :
  • Quand il y a un changement d'état vers haut, l'entrée 2 est à haut
  • Quand il y a un changement d'état vers bas, l'entrée 2 est à bas
Et en regardant l'entrée 2 :
  • Quand il y a un changement d'état vers haut, l'entrée 1 est à bas
  • Quand il y a un changement d'état vers bas, l'entrée 1 est à haut




De la même manière lorsque l'on recule on a :

En regardant l'entrée 1 :
  • Quand il y a un changement d'état vers haut, l'entrée 2 est à bas
  • Quand il y a un changement d'état vers bas, l'entrée 2 est à haut
Et en regardant l'entrée 2 :
  • Quand il y a un changement d'état vers haut, l'entrée 1 est à haut
  • Quand il y a un changement d'état vers bas, l'entrée 1 est à bas



En fonction des changements d'état, on peut donc définir si les roues avancent ou reculent.

 

 

Compte des impulsions


Dans le reste du document, un changement d'état correspond à un tic.


Pour connaitre la distance parcourue, il faut compter les changements d'état ou tics.
Chaque entrée de chaque codeur et reliée à une interruption de l'Arduino. Un compteur est incrémenté lorsque le robot avance et décrémenté lorsqu'il recule.

Pour la roue gauche, entrée 1 :
si ( robot == AVANCER)
    nbr_tic_du_codeur_gauche_entrée_1 ++
si ( robot == RECULER )
    nbr_tic_du_codeur_gauche_entrée_1 --

Pour la roue gauche, entrée 2 :
si ( robot == AVANCER)
    nbr_tic_du_codeur_gauche_entrée_2 ++
si ( robot == RECULER )
    nbr_tic_du_codeur_gauche_entrée_2 --

Pour la roue droite, entrée 1 :
si ( robot == AVANCER)
    nbr_tic_du_codeur_droite_entrée_1 ++
si ( robot == RECULER )
    nbr_tic_du_codeur_droite_entrée_1 --

Pour la roue droite, entrée 2 :
si ( robot == AVANCER)
    nbr_tic_du_codeur_droite_entrée_2 ++
si ( robot == RECULER )
    nbr_tic_du_codeur_droite_entrée_2 --


 

 

Initialisation



Rien de plus facile ! Il suffit de créer des variables de position x, y et d'angle puis de les mettre à la position souhaitée ou à zéro (ici, définition de la position initiale là où se situe le robot au départ) :

position_x = 0
position_y = 0
angle = 0


 

Calcul de la position et de l'angle après déplacement


Comme on a 2 codeurs avec pour chacun 2 entrées, il faut définir la position de chaque roue :

position_roue_gauche = 1/2 (nbr_tic_du_codeur_gauche_entrée_1 + nbr_tic_du_codeur_gauche_entrée_2) 

position_roue_droite= 1/2 (nbr_tic_du_codeur_droite_entrée_1 + nbr_tic_du_codeur_droite_entrée_2) 



Une fois la valeur récupérée, on remet à zéro les compteurs de tics de chaque codeur :

nbr_tic_du_codeur_gauche_entrée_1 = 0
nbr_tic_du_codeur_gauche_entrée_2 = 0
nbr_tic_du_codeur_droite_entrée_1 = 0
nbr_tic_du_codeur_droite_entrée_2 = 0



À partir des positions des roues on peut maintenant calculer l'angle dans lequel se situe le robot :

angle = ancien_angle + 1/2 * (position_roue_gauche + position_roue_droite) * 360 / (2*PI*Rayon_Robot)


Afin de calculer l'angle du robot, il faut partir de :
  • l'angle précédent
  • la position du robot : la différence entre la position de la roue gauche et de la roue droite
  • le Rayon_Robot : doit être calculé en fonction du nombre de tics des codeurs afin d'avoir les mêmes unités partout
  • 360 / 2 * PI : pour avoir l'angle en degrés ce qui est plus facile pour vérifier les angles en tant qu'humain !


On a défini l'angle du robot par rapport au point de départ !



Pour la position, il faut calculer le déplacement réalisé en x et en y par le robot, pour ça on fait une projection de la distance parcourue sur les axes x et y avec les cosinus et sinus de l'angle du robot :

deplacement_en_x = 1/2 * (position_roue_gauche + position_roue_droite) * sin(angle)

deplacement_en_y = 1/2 * (position_roue_gauche + position_roue_droite) * cos(angle)



Attention : les fonctions sinus et cosinus peuvent demander un angle en radian et non en degrés, dans ce cas une conversion est nécessaire :

1 radian = 1 degrés * 180 / PI = 57.2958 °


Et ainsi, on calcule la nouvelle position du robot :  

position_x = ancienne_position_x + deplacement_en_x
position_y = ancienne_position_y + deplacement_en_y



Ça y est on a nos nouvelles valeurs de x, y  et d'angle !