Club robotique de Sophia-Antipolis

Accueil > POBOTpedia > Programmation > Explorer > Accostage sur dock de rechargement ROOMBA

Accostage sur dock de rechargement ROOMBA

mardi 17 mai 2011, par Frédéric Bernat

Le précédent article permettait de décoder les trames infra-rouge utilisées par le robot Roomba. Ce qui suit est l’utilisation de ces nouvelles connaissances sur un petit robot à chenilles, utilisant une carte driver moteur SABERTOOTH 2X5 avec transmission sérielle et un Arduino Mega 2560, les moteurs et le châssis proviennent de chez TAMIYA.

 

JPEG - 53.5 ko

Comme on peut le remarquer sur la photo, la tourelle que j’avais prélevée sur le pare-choc de mon Roomba, a été installée sur une petite plaquette en bois en avant du robot. Sa hauteur par rapport au sol est la même que sur le Roomba, ceci est très important pour avoir une bonne réception des signaux infra-rouge provenant du Dock.

La vidéo ci-dessous montre l’utilisation de la télécommande pour diriger le robot. Mais la suite est beaucoup plus intéressante, puisque le robot va de lui-même rejoindre la base de rechargement grâce à un petit bout de code relativement simple.

Mais avant d’arriver au résultat espéré, il m’a d’abord fallu éliminer toutes les sources de parasites qui ont tendance à se multiplier sur un robot.

Les Parasites :

Le premier que j’ai rapidement identifié (au vu du comportement du robot une fois sous tension) provient de la carte driver des moteurs. Ayant déjà dû passer pas mal de temps sur mon robot tondeuse pour identifier et résoudre le problème, j’ai directement installé une self de choc sur la ligne de masse GND reliant la carte moteur à l’Arduino (c.f. photo ci-dessous). Bien que cette carte soit conçue pour une utilisation en direct, il semble que beaucoup de parasites arrivent encore à passer.

JPEG - 35.9 ko

Le deuxième type de parasite provient du capteur infra-rouge lui-même. En effet j’avais installé dans la tourelle le capteur de chez Vishay (TSOP1838). S’il convient très bien pour l’utilisation avec la télécommande les choses se sont gâtées lorsque je l’ai mis en face du dock.

Le dock émet trois trames différentes espacées de 4 à 6 millisecondes et visiblement c’est un souci pour le capteur, qui me restituait des trames avec beaucoup de perturbations, rendant le déplacement du robot chaotique, lorsqu’il ne se figeait pas définitivement. Pour résoudre le problème, j’ai suivi les conseils du fabricant en rajoutant un condensateur de 4,7µF et une résistance de 300 Ohms. Les choses se sont légèrement améliorées mais pas suffisamment pour effectuer un accostage correctement. J’ai donc fini par utiliser le capteur de chez Radio Shake, avec cette fois ci beaucoup plus de réussite comme en témoigne les deux vidéos ci-dessous. Bon, c’est pas du Cecil B. DeMille, mais en même temps je tourne pas un péplum.

La troisième source de parasites provient du code lui-même. L’interruption décrite dans le précédent article permettait de décoder les trames provenant de la télécommande avec succès, mais face au dock avec une vitesse d’émission plus rapide, ce bout de code s’est avéré trop lent.


void getIrKey()

if(indice<8)
if( bas)
val=micros() ;
bas=0 ;

else
bas=1 ;
kk=micros()-val ; //Else compute the length of Low signal
if((kk>3200)||(kk<900))
indice=0 ; //if the length is not in the range of 1 or 3ms then reset
else code[indice++]=kk>1900 ;



Cette interruption fonctionnait sur l’évènement CHANGE, malheureusement, il arrivait trop souvent qu’il ne détecte pas le changement. Une deuxième mouture m’a permis d’améliorer les choses mais pas suffisamment, une fois de plus.


void getIrKey()

if(indice<16)
code[indice++]=micros() ; //If pin2 LOW then store the time
detachInterrupt(0) ;

if (bas)
attachInterrupt(0,getIrKey,RISING) ;
else
attachInterrupt(0,getIrKey,FALLING) ;
bas= !bas ;

Celui-ci résolvait une partie du problème mais ne permettait pas d’appliquer un filtre avec suffisamment de rapidité.(C’est pourquoi il en était dépourvu et le traitement était effectué dans la boucle LOOP). (pas top)

Finalement j’ai trouvé la bonne manière de faire, en divisant l’interruption en deux, et en récupérant les fronts haut et bas :


void getIrKeyFal()

if(indice<16)
code[indice++]=micros() ;

detachInterrupt(0) ;

if((indice>2)&&((code[indice-1]-code[indice-2])>3200))

indice=0 ;
code[indice++]=micros() ;

attachInterrupt(0,getIrKeyRis,RISING) ;

volatile unsigned long TT ;

void getIrKeyRis()

if(indice<16)
code[indice++]=micros() ;

detachInterrupt(0) ;

TT=code[indice-1]-code[indice-2] ;
if (TT<900)
indice=0 ;
else if((indice==2)&&(TT<2500))
indice=0 ;

attachInterrupt(0,getIrKeyFal,FALLING) ;


Par souci de rapidité j’ai aussi modifié la façon de calculer le code en supprimant la boucle for :


ir_value |= ((((code[1]-code[0]))>1900) << 7)
| ((((code[3]-code[2]))>1900) << 6)
| ((((code[5]-code[4]))>1900) << 5)
| ((((code[7]-code[6]))>1900) << 4)
| ((((code[9]-code[8]))>1900) << 3)
| ((((code[11]-code[10]))>1900) << 2)
| ((((code[13]-code[12]))>1900) << 1)
| ((((code[15]-code[14]))>1900)) ;

Tout ceci est très bien, mais sans l’algorithme adéquat le robot n’ira jamais se recharger sur la base.

L’Algorithme :

JPEG - 39.2 ko

Cet algo est relativement simple et ne présente pas de difficultés majeures pour son implémentation. Je n’en ai implémenté qu’une partie, afin de vérifier la validité de mon approche. Il y a également deux cas de perte de signal qui ne sont pas traités, et aussi la gestion de l’accostage sans contact établi. Je vous laisse le soin de les rajouter (ou pas).

Le code qui suit n’est pas parfait, il me reste encore pas mal d’optimisations, le rajout des capteurs de proximité et de contact électrique, pour avoir un système complet, néanmoins il fournit déjà une base pour tous ceux qui veulent rendre leur robot encore plus autonome. (voir les vidéos)

Le petit bout de code qui va bien :


#include
#include
#include

<WProgram.h>

#include "math.h"

// Set to 9600 through Sabertooth dip switches
#define SABER_BAUDRATE 9600

void initSabertooth( void )

Serial1.begin( SABER_BAUDRATE ) ;

// 2 second time delay for the Sabertooth to init
delay( 2000 ) ;
// Send full stop command
Serial1.print(0,BYTE) ;

#define IR_PIN 2

volatile unsigned long code[16] ;
volatile unsigned long val ;
unsigned long var ;
unsigned int ir_value=0 ;
unsigned int SensPrec ;
unsigned int SensCur ;
byte Change ;
byte Signal ;
int Droite=1 ;
int Gauche=0 ;
byte StopR=64 ;
byte StopL=192 ;
byte AvanceTTLentR=60 ;
byte AvanceTLentR=54 ;
byte AvanceLentR=44 ;
byte AvanceMoyenR=32 ;
byte AvanceRapideR=15 ;
byte AvanceTTLentL=188 ;
byte AvanceTLentL=182 ;
byte AvanceLentL=172 ;
byte AvanceMoyenL=160 ;
byte AvanceRapideL=144 ;
byte ReculeLentR=74 ;
byte ReculeMoyenR=96 ;
byte ReculeeRapideR=112 ;
byte ReculeLentL=202 ;
byte ReculeMoyenL=224 ;
byte ReculeRapideL=240 ;
int TimeOut ;
boolean Lent ;
unsigned long Apres ;
volatile int indice ;

void setup()
var=0 ;
TimeOut=200 ;
Lent=false ;
pinMode(IR_PIN, INPUT) ;

Serial.begin(9600) ;
Serial.println("Ready...") ;

initSabertooth( ) ;
ir_value=0x00 ;
indice=0 ;
Signal=0 ;
SensPrec=0 ;
SensCur=0 ;
Change=0 ;
Apres=0 ;
attachInterrupt(0,getIrKeyFal,FALLING) ;
sei() ;

void getIrKeyFal()

if(indice<16)
code[indice++]=micros() ;

detachInterrupt(0) ;

if((indice>2)&&((code[indice-1]-code[indice-2])>3200))

indice=0 ;
code[indice++]=micros() ;

attachInterrupt(0,getIrKeyRis,RISING) ;

volatile unsigned long TT ;

void getIrKeyRis()

if(indice<16)
code[indice++]=micros() ;

detachInterrupt(0) ;

TT=code[indice-1]-code[indice-2] ;
if (TT<900)
indice=0 ;
else if((indice==2)&&(TT<2500))
indice=0 ;

attachInterrupt(0,getIrKeyFal,FALLING) ;

unsigned long time ;
unsigned long duree ;

void loop()

if(indice==16)
detachInterrupt(0) ;

indice=0 ;
ir_value=0 ;
ir_value |= ((((code[1]-code[0]))>1900) << 7)
| ((((code[3]-code[2]))>1900) << 6)
| ((((code[5]-code[4]))>1900) << 5)
| ((((code[7]-code[6]))>1900) << 4)
| ((((code[9]-code[8]))>1900) << 3)
| ((((code[11]-code[10]))>1900) << 2)
| ((((code[13]-code[12]))>1900) << 1)
| ((((code[15]-code[14]))>1900)) ;

if(ir_value==168)
Signal=1 ;
if ((SensCur !=0)&&(SensCur !=1))
SensPrec=SensCur ;
Apres=millis() ;
SensCur=1 ;
var=millis() ;

else if((ir_value==164)||(ir_value==162))
Signal=1 ;
if ((SensCur !=0)&&(SensCur !=2))
SensPrec=SensCur ;
Apres=millis() ;
SensCur=2 ;
var=millis() ;

else if (ir_value==172)
Signal=1 ;
var=millis() ;
Apres=millis() ;
else
if (ir_value==161)
Signal=1 ;
Lent=true ;

//Serial.println( ir_value) ;
//Serial.println( SensPrec) ;

if (SensPrec==0)
EnAvant() ;
else
if (SensPrec==1)
VirageLent(Droite) ;
else
if (SensPrec==2)
VirageLent(Gauche) ;

attachInterrupt(0,getIrKeyFal,FALLING) ;
else
duree=(millis()-var) ;
if((duree>TimeOut)&&(var !=0))
Arret() ;
// Serial.println( "duree") ;
var=0 ;

if (Signal)
duree=(millis()-Apres) ;
if((duree>500)&&(Apres !=0))
detachInterrupt(0) ;

//Serial.println( duree) ;
Signal=0 ;
DemiTour(SensPrec) ;
SensPrec=0 ;
Apres=0 ;
var=millis() ;
attachInterrupt(0,getIrKeyFal,FALLING) ;



void translate()

switch (ir_value)
case 0b10000100:Serial.println("\rSpot") ;
break ;
case 0b10001000:Serial.println("\rClean") ;
break ;
case 0b10000101:Serial.println("\rMax") ;
break ;
case 0b10000001:Serial.println("\rLeftc") ;
break ;
case 0b10000010:Serial.println("\rForward") ;
break ;
case 0b10000011:Serial.println("\rRightc") ;
break ;
case 161:Serial.println("\rDock Behind") ;
break ;
case 164:Serial.println("\rDock Right") ;
break ;
case 168:Serial.println("\rDock Left") ;
break ;
case 172:Serial.println("\rDock Center") ;
break ;
default:Serial.println( ir_value) ;

/******************************************************/
/* Routine de Gestion des Déplacements */
/******************************************************/
void Arret()

SetMotor(StopR) ;
SetMotor(StopL) ;

void Virage(byte steer)

if (steer==Droite)
SetMotor(AvanceTLentR) ;
SetMotor(AvanceLentL) ;
else
SetMotor(AvanceLentR) ;
SetMotor(AvanceTLentL) ;

TimeOut=400 ;

void VirageLent(byte steer)

if (steer==Droite)
SetMotor(AvanceTTLentR) ;
SetMotor(AvanceTLentL) ;
else
SetMotor(AvanceTLentR) ;
SetMotor(AvanceTTLentL) ;

TimeOut=400 ;

void EnAvant()

if (Lent)
SetMotor(AvanceTLentR) ;
SetMotor(AvanceTLentL) ;

else
SetMotor(AvanceLentR) ;
SetMotor(AvanceLentL) ;

TimeOut=400 ;

void Aleatoire()

long X=random(1) ;

if(X)
Virage(Gauche) ;
else
Virage(Droite) ;
TimeOut=500 ;

void ArcdeCercle()

long X=random(1) ;

if(X)
Virage(Gauche) ;
else
Virage(Droite) ;
TimeOut=500 ;

void DemiTour(int S)

if (S==1)
SetMotor(ReculeLentL) ;
SetMotor(AvanceLentR) ;

else
SetMotor(AvanceLentL) ;
SetMotor(ReculeLentR) ;

TimeOut=500 ;

/******************************************************/
/* Routine de Gestion des moteurs */
/******************************************************/

// Public : Set a speed (0 to 127 for backwards, 128 to 255 for forward) based on a motor index
void SetMotor(byte steer)

Serial1.print(steer,BYTE) ;

Vos commentaires

Un message, un commentaire ?

modération a priori

Attention, votre message n’apparaîtra qu’après avoir été relu et approuvé.

Qui êtes-vous ?

Pour afficher votre trombine avec votre message, enregistrez-la d’abord sur gravatar.com (gratuit et indolore) et n’oubliez pas d’indiquer votre adresse e-mail ici.

Ajoutez votre commentaire ici
  • Ce formulaire accepte les raccourcis SPIP [->url] {{gras}} {italique} <quote> <code> et le code HTML <q> <del> <ins>. Pour créer des paragraphes, laissez simplement des lignes vides.

Ajouter un document