L'idée m'est venue suite à l'amélioration d'un char radio-commandé qui m'avait laissé sur les bras un châssis à chenilles avec ses deux moteurs et après avoir découvert l'Arduino sur le web.

Le châssis de char lui donne un petit air guerrier mais, pour citer Doc Brown, quitte à faire un robot autant qu'il ait de la gueule.

Le concept est classique : le robot avance et un détecteur mesure la distance aux obstacles situés devant. En dessous d'un certain seuil, il effectue une manoeuvre d'évitement. Ce comportement est intégré à la fonction conduit().

Pour la détection des obstacles, j'ai opté pour le télémètre à infra-rouge. Une diode émet un rayon infra-rouge, un récepteur capte le reflet et détermine la distance par triangulation. Ce système présente l'avantage d'être peu cher et très précis, mais l'inconvénient de pouvoir être ébloui par les sources fortement émettrices dans l'infra-rouge comme le soleil. De plus, il prend sa mesure sur une zone très ponctuelle, paramètre à prendre en compte dans la conception si on ne veut pas rater un éléphant sous prétexte qu'il n'est pas bien en face.

Un autre détecteur classique est le sonar mais il est plus cher et a d'autres faiblesses, notamment sa sensibilité aux échos parasites. Le détecteur idéal est le télémètre laser mais je ne suis pas riche à ce point là.

Concept général

Le robot est piloté par la carte Arduino qui lit les capteurs et actionne les moteurs via un module basé sur un pont en H L298. Les moteurs sont assez costauds et tirent une bonne ampère en usage normal et peuvent monter à 3 si on les sollicite un peu, d'où la nécessité d'une carte qui tient la route!

Sur le dessus du robot, les capteurs sont montés sur un servo qui balaye en permanence le paysage. Ceci permet de compenser l'étroitesse de leur champ de vision et de ne pas rater les obstacles. Pour balayer le maximum d'espace en un minimum de temps, trois capteurs sont montés à 45° les uns par rapport aux autres. Ainsi le servo n'a qu'à balayer de plus ou moins 23° pour couvrir 135° de champ.

Première version

tank_v1_600.JPG

La première version du robot est dotée des trois télémètres montés en étoile sur le support en alu. Le fonctionnement est ultra-simple : si un obstacle est détecté par le capteur de gauche on tourne de 90° à droite et réciproquement, si il est détecté par le capteur central on fait demi-tour.

La distance est mesurée par la fonction calculDistance() identique à celle de mon montage précédent.

Globalement ça marche, mais le robot est vraiment bête comme ses pieds. Le comportement en "tout ou rien" fait qu'il peut se coincer dans un couloir en rebondissant sans fin d'un mur à l'autre ou simplement dans le coin d'une pièce en faisant des quart de tours à l'infini.

De plus, à 23° de part et d'autre de l'axe du châssis, la zone couverte par le capteur central touche celle couverte par chacun des capteurs latéraux. Donc suivant la position du servo, un même obstacle peut générer soi un demi-tour, soit un virage à 90° suivant le capteur qui le voit d'où un comportement totalement imprévisible.

D'autre part, les télémètres infrarouges ont une "zone morte" à courte distance (en-dessous de 20cm pour le modèle utilisé dans mon montage) où la valeur renvoyée devient incohérente : plus l'obstacle est près, plus le capteur le voit loin... Donc si un obstacle surgit dans cette zone morte le robot va se précipiter dessus.

Enfin, il est aveugle pour les obstacles situés plus bas que les capteurs et ne détecte pas s'il y a bien un sol devant, en cas d'arrivée sur un escalier par exemple. D'où la...

Deuxième version

Bumper et interruptions

bumper.JPG

Pour cette deuxième version, j'ai corrigé tout d'abord le problème des petits obstacles en ajoutant un bumper à l'avant. Un morceau de plat d'acier est monté sur ressort et connecté au +5v. En cas de choc avec un obstacle, il fait contact avec la masse et met le pin 2 à HIGH. Pour détecter le contact, j'ai utilisé une interruption.

L'arduino a deux interruptions matérielles sur les pins numériques 2 et 3. La particularité d'une interruption est qu'on n'a pas besoin de lire le capteur en permanence en boucle : lorsqu'il est actionné, la boucle du code est interrompue (d'où le terme interruption) et la fonction correspondante est exécutée, après quoi la boucle reprend son cours. Pour détecter des actions brèves comme un choc, c'est l'outil idéal.

Par contre, la fonction appelée ne peut retourner aucun résultat et le chronomètre interne de l'Arduino est interrompu et donc la fonction delay() inaccessible. L'interruption se contente donc de faire passer la vairable globale choc de 0 à 1 via la fonction obstacle(). Cette variable est ensuite lue dans la boucle principale et appelle la fonction bump() si nécessaire qui engage la manoeuve d'évitement : marche arrière et demi-tour.

Y'a quelqu'un ?

antichute.JPG

Autre souci corrigé, la détection de la présence du sol. C'est le rôle des capteurs situés de part et d'autre du bumper et de la fonction antichute().

J'ai commencé avec de simples émetteurs/récepteurs infra-rouges QRD1114. Une diode IR éclaire le sol, un capteur détecte l'intensité de l'émission en question et la retourne en analogique. Si l'intensité tombe sous un certain seuil, c'est qu'il n'y a plus de sol pour réfléchir l'émission et on engage l'action qui s'impose : stop, marche arrière, demi-tour.

Ce capteur s'est révélé particulièrement inadapté à l'usage. Il est très facilement trompé par tout ce qui absorbe l'infrarouge comme le joint entre les carreaux du sol, le robot ne faisait donc pas 30 centimètres avant de faire demi-tour.

J'ai avantageusement remplacé les capteurs par des télémètres infrarouges à très courte portée. Ils fonctionnent exactement comme les télémètres principaux situés sur le dessus du robot à ceci près qu'ils ont une portée de 5cm seulement et renvoient un signal en tout ou rien : LOW si un objet (le sol) est dans le champ, HIGH sinon. Ils sont à peu près insensibles à la réflectivité de la cible.

On pourrait là encore les brancher sur une interruption, mais la carte Arduino n'en a que deux et il y en a déjà une prise par le bumper, et j'ai deux capteurs... Ils sont branchés sur les pins numériques 12 et 13 et sont lus en boucle, déclenchant le même comportement que les capteurs qu'ils remplacent. On pourrait utiliser une interruption logicielle sur n'importe quel pin mais c'est plus compliqué.

Si un capteur détecte une absence de sol, une deuxième mesure est prise 100ms plus tard pour éviter les faux positifs qui arrivent assez souvent sur les surfaces brillantes et inclinées comme un rebord de carrelage. Si les deux mesures sont positives l'action d'évitement s'engage. C'est très fiable.

Silence, on tourne

capteurs_v2.JPG

Dernière et principale amélioration, le comportement en cas de détection d'obstacle par les télémètres dans la fonction conduit().

Premièrement, au lieu d'être montés en étoile autour du servo, les télémètres sont montés à l'arrière de celui-ci. Ainsi, l'essentiel de la "zone morte" des capteurs se trouve au-dessus du châssis lui-même, empêchant les obstacles de s'y trouver.

Le comportement du char a été amélioré : il tourne proportionnellement à l'angle d'approche de l'obstacle, c'est à dire un tout petit peu s'il est quasi parallèle à l'obstacle et beaucoup s'il arrive droit dessus.

Pour cela, j'ai récupéré une donnée précieuse et disponible, à savoir l'orientation du servo et donc des télémètres au moment de la mesure de distance. Suivant le télémètre qui voit l'obstacle et son orientation, je peux en déduire la direction dans laquelle se trouve l’obstacle bien plus finement qu'avec mon approche précédente et agir en conséquence. L'angle du servo détermine l'ampleur du tournant de façon continue via la fonction map() et transmet le résultat à la fonction tourne(). A noter, l'ampleur mini du tournant ne doit pas être zéro sous peine de voir le robot se "garer" à côté d'un mur qui serait détecté par un capteur avec l'angle maxi.

Les premiers essais n'ont guère été concluants, le robot continuant à avoir un comportement pour le moins erratique. Une petite séance de communication avec le port série m'a permis de mettre la main sur le problème : entre le moment où la position du servo est lue et celui où le télémètre renvoie sa valeur, le servo a le temps de tourner de 15°! Suivant que le servo est en train de balayer de la droite vers la gauche ou de la gauche vers la droite, un même obstacle vu par le même capteur pouvait être détecté avec un décalage de 30°, engendrant une manœuvre d'évitement complètement différente.

Pour y remédier, j'ai inséré une valeur de trim qui est ajoutée ou retranchée à la position du servo suivant son sens de balayage avant d'être envoyée à la fonction de pilotage.

La fonction conduit() accepte donc désormais un argument, pos_servo, qui est la position du servo corrigée du décalage du télémètre.

Au final, le comportement du robot est bien plus doux et il se sort beaucoup mieux des "nasses" comme les couloirs, les coins ou les dessous de chaises.

Dernières retouches

carte_electronique.JPG

Une fois le robot au point, j'ai refondu la partie connexion et électronique afin de faciliter l'accès aux différentes parties. Une carte fixée à la partie inférieure permet de déconnecter la partie supérieure via des broches. Elle porte aussi deux régulateurs de tension : un en 5v pour alimenter le servo sans charger l'Arduino ainsi que le ventilateur pour la carte de commande des moteurs qui chauffe beaucoup, et un en 3v pour rajouter un truc plus tard. Il y a aussi deux interrupteurs, un pour allumer la partie logique/servo et un pour connecter ou non les moteurs, ce qui simplifie le déboguage et permet de recharger l'accu 7,2v sans avoir à le sortir.

Les connexions à l'Arduino ont également été soudées à des fiches afin de faciliter le montage/démontage de la carte.

Le code

#include <Servo.h> // bibliotheque servo

// variable de l'interruption du bumper

volatile int choc = 0;

// Initialisation du servo

Servo myservo;  // création de l'objet servo
const int tempo = 3; // temps d'attente rotation (règle la vitesse de balayage)
const int trim = 6; // trim pour alignement servo
const int balayage = 30; // largeur du balayage
const int centre = (90-trim); // point central
int pos = (90-trim);    // variable de position du servo, initialisée droit devant
const int correction_pos = 15; // correction du lag du télémètre par rapport à la rotation du servo

// initialisation des télémètres

const int IRpinG = A0;   // pin du télémètre IR gauche
const int IRpinC = A1;   // pin du télémètre IR centre
const int IRpinD = A2;   // pin du télémètre IR droite
const int iteration = 3; // nombre d'iterations pour la moyenne de mesures de distance
int proche[] = {35, 60, 80}; // distances de déclenchement des actions (en cm)

// initialisation des capteurs de chute

const int pinAntichuteGauche = 12;
const int pinAntichuteDroite = 13;

// initialisation des moteurs

const int moteurDroite   = 6; // PWM Pin Moteur droite
const int PoMD = 7;   // Pin polarité moteur droite
const int moteurGauche   = 5; // PWM Pin Moteur gauche
const int PoMG = 4;   // Pin polarité moteur gauche
const int vitesseAfond = 200; // valeurs vitesses
const int vitesseLente = 180;
const int ralenti = 180;
const int rotation = 200; // vitesse moteur quand on tourne
const int min_turn = 100; // temps min pour tourner, ne pas mettre à zéro pour que le char ne se bloque pas à coté d'un obstacle tangent
const int max_turn = 1200; // temps max pour tourner
const int centre_turn = 800; // temps de rotation quand capteur central
const int tempsAntichute = 1000; // temps de rotation quand capteur antichute
int temps = 0; // temps de rotation d'une chenille quand on tourne du fait d'un télémètre
int vitesseEncours = vitesseAfond; // stocke la vitesse en cours

void setup()
{
  // servo
 
  myservo.attach(9);  // servo sur le pin 9
 
  // interruption par le bumper (choc détecté quand l'interrupt 0 sur le pin 2 passe de LOW à HIGH)
 
  // pinMode(2, INPUT);
  attachInterrupt(0, obstacle, RISING);
  // interrupts();
 
 
  // capteurs antichute
 
  pinMode(pinAntichuteGauche, INPUT);
  pinMode(pinAntichuteDroite, INPUT);
 
  // moteurs
 
  pinMode(moteurDroite,   OUTPUT);
  pinMode(PoMD,   OUTPUT);
  pinMode(moteurGauche, OUTPUT);  
  pinMode(PoMG, OUTPUT);  
  digitalWrite(PoMD, HIGH) ;   // Polarités des moteurs pour marche avant
  digitalWrite(PoMG, HIGH) ;
  analogWrite(moteurDroite, vitesseAfond);   // lance les moteurs
  analogWrite(moteurGauche, vitesseAfond);    
}
 
 
void loop()
{  
 if (choc == 1) { // si un choc a été détecté
   bump();
 }  
 for(pos = (centre-balayage); pos < (centre+balayage); pos += 1)  // balayage servo autour du point central par incréments d'1 degré
  {                                  
    myservo.write(pos);              // met le servo sur la position en cours
    delay(tempo);     // temps d'attente pour que le servo atteigne sa position
    conduit((pos-correction_pos));  // on roule - on transmet la position du servo corrigée du lag du capteur
  }
 for(pos = (centre+balayage); pos>=(centre-balayage); pos-=1)     // idem avec balayage dans l'autre sens
  {                                
    myservo.write(pos);              
    delay(tempo);                      
    conduit((pos+correction_pos));
  }  
}

/* fonction d'évitement d'obstacle */
/* analyse les données des télémètres et */
/* prend les décisions adéquates en fonction */
/* de la position du servo corrigée du lag du capteur */
 
void conduit(int pos_servo) {
 
  // on verifie qu'il y a bien un sol devant
 
  antichute(pinAntichuteGauche);
  antichute(pinAntichuteDroite);
 
  // mesure de distance sur les 3 capteurs
 
  int distanceGauche = calculDistance(IRpinG);
  int distanceCentre = calculDistance(IRpinC);
  int distanceDroite = calculDistance(IRpinD);
 
  // si rien en vue, on met les gaz
 
  if (distanceGauche == 150 && distanceDroite == 150 && distanceCentre == 150 && vitesseEncours != vitesseAfond) {
   roule(vitesseAfond);
  }
 
  // si obstacle à moins du premier seuil de déclenchement, on ralentit
 
   if ((distanceGauche < proche[2] || distanceCentre < proche[2] || distanceDroite < proche[2]) && (distanceGauche > proche[1] || distanceCentre > proche[1] || distanceDroite > proche[1]) && vitesseEncours != vitesseLente) {
    roule(vitesseLente);
    }
   
   // si obstacle à moins du deuxième seuil, on ralentit encore plus
 
   if ((distanceGauche <= proche[1] || distanceCentre <= proche[1] || distanceDroite <= proche[1]) && (distanceGauche > proche[0] || distanceCentre > proche[0] || distanceDroite > proche[0]) && vitesseEncours != ralenti) {
    roule(ralenti);
    }
   
  // on observe si un obstacle est à moins du troisième seuil et quel capteur a l'obstacle le plus proche puis
  // on tourne dans la direction opposée proportionnellement à la direction de l'obstacle.
  // on relance ensuite les moteurs en vitesse lente
 
  if (distanceGauche <= proche[0] || distanceCentre <= proche[0] || distanceDroite <= proche[0]) {
    pos_servo = constrain(pos_servo,(centre-balayage),(centre+balayage)); // on limite la position corrigee du servo aux valeurs valides
   
    if (distanceGauche < distanceCentre && distanceGauche < distanceDroite) {
     
        temps = map(pos_servo, (centre+balayage),(centre-balayage),min_turn,max_turn); // on tourne d'autant plus que l'angle véhicule-obstacle est aigu
        tourne(PoMG, moteurGauche, temps);
        roule(vitesseLente);
     
    }
    if (distanceDroite < distanceCentre && distanceDroite < distanceGauche) {
     
        temps = map(pos_servo,(centre-balayage),(centre+balayage),min_turn,max_turn);
        tourne(PoMD, moteurDroite, temps);
        roule(vitesseLente);
     
    }
    if (distanceCentre < distanceGauche && distanceCentre < distanceDroite) { // si obstacle en face on brauqe à fond dans la direction opposée
      if (pos_servo > centre){
      tourne(PoMG, moteurGauche, centre_turn);
      }
      if (pos_servo <= centre){
       tourne(PoMD, moteurDroite, centre_turn);
      }
      roule(vitesseLente);
    }
  }
}

/* fonctions pour tourner */
/* principe général : on s'arrete (fonction halte()), on tourne, on s'arrete */

void tourne(int polarite, int moteur, int delai) {
   halte();
   digitalWrite(polarite, LOW); // on met la chenille correspondante en marche arrière
   analogWrite(moteur,rotation);
   delay(delai);
   halte();
}


void demiTour (){
   halte();
   digitalWrite(PoMG, LOW); // on met la  chenille gauche en marche arrière
   digitalWrite(PoMD, HIGH); // on met la  chenille gauche en marche avant
   analogWrite(moteurGauche,rotation);
   analogWrite(moteurDroite,rotation);
   delay(max_turn);
   halte();
}

void marcheArriere() {
   halte();
   digitalWrite(PoMD, LOW) ;   // Polarités des moteurs pour marche arrière
   digitalWrite(PoMG, LOW) ;
   analogWrite(moteurDroite,ralenti);
   analogWrite(moteurGauche,ralenti);
   delay(500);
}

void halte() {
   analogWrite(moteurDroite,0); // on arrete
   analogWrite(moteurGauche,0);
   delay(250);
}

void roule(int vitesse) { // fonction pour avancer tout droit à la vitesse indiquée
   digitalWrite(PoMD, HIGH);
   digitalWrite(PoMG, HIGH);
   analogWrite(moteurGauche,vitesse);
   analogWrite(moteurDroite,vitesse);
   vitesseEncours = vitesse; // on note la vitesse
}

void antichute(int capteur) { // fonction antichute
    int vide = digitalRead(capteur);
    if (vide == HIGH){ // si le capteur signale un vide on reprend une mesure 100ms plus tard pour éviter les faux positifs
      delay(100);
       vide = digitalRead(capteur);
       if (vide == HIGH){ // si toujours vide on fait demi tour
         marcheArriere();
         demiTour();
         roule(vitesseLente);
       }
    }
  }

/* calcul de la distance */

int calculDistance(int telemetre) {
  float distance = 0;
  for (int i = 0; i < iteration; i++) { // on fait la moyenne de (iteration) mesures pour lisser les pics
   float volts = analogRead(telemetre)*0.0048828125;   // valeur du télémètre * (5/1024) - si alimentaiton en 3.3.volts changer 5 en 3.3
   distance = distance + 65*pow(volts, -1.10);
  }
  distance = distance/iteration;
  if (distance > 150 or distance < 20) { // si hors limites on met à 150
    distance = 150;
  }
  return(distance);
}

void obstacle() { // en cas d'interrupt du bumper (choc détecté) on met la variable "choc" à 1
    choc = 1;
}

void bump(){
   marcheArriere();
   demiTour();
   choc = 0;
   roule(vitesseLente);
}