Voici la seconde partie de l’article concernant l’évitement d’obstacle.

Etape 4 : Programme d’évitement d’obstacle

Le code est plus ou moins proche d’une version fonctionnelle et finale. Voici tout d’abord le code source :

motor.c : Ce fichier définit les fonctions permettant de commander les moteurs à travers des fonctions associées à différents types de mouvement.

#include "motor.h"

void move_forward()
{
  analogWrite (E2, SPEED);
  digitalWrite(M2,LOW);
  analogWrite (E1, SPEED);
  digitalWrite(M1,HIGH);
}

void move_backward()
{
  analogWrite (E2, SPEED);
  digitalWrite(M2,HIGH);
  analogWrite (E1, SPEED);
  digitalWrite(M1,LOW);
}

void turn_right ()
{
  analogWrite (E2, 0);
  digitalWrite(M2,LOW);
  analogWrite (E1, SPEED);
  digitalWrite(M1,HIGH);
}

void turn_left ()
{
  analogWrite (E2, SPEED);
  digitalWrite(M2,LOW);
  analogWrite (E1, 0);
  digitalWrite(M1,HIGH);
}

void rotate_right ()
{
  analogWrite (E2, SPEED);
  digitalWrite(M2,HIGH);
  analogWrite (E1, SPEED);
  digitalWrite(M1,HIGH);
}

void rotate_left ()
{
  analogWrite (E2, SPEED);
  digitalWrite(M2,LOW);
  analogWrite (E1, SPEED);
  digitalWrite(M1,LOW);
}

void stop_motor ()
{
  analogWrite (E1, 0);
  analogWrite (E2, 0);
}

sensor.c : Gestion des obstacles.

#include "sensor.h"

int static r = 0;

void handle_obstacle ()
{
   int i;
   int left_avg = 0;
   int right_avg = 0;

   if (r == 1024)
   r = 0;

   for (i = 0; i < 5; i++)
      left_avg += analogRead(LEFT_SENSOR);

   for (i = 0; i < 5; i++)
      right_avg += analogRead(RIGHT_SENSOR);

   left_avg = left_avg / 5;
   right_avg = right_avg / 5;

   // zero obstacle in the both side
   if (left_avg < CONF_SENSOR && right_avg < CONF_SENSOR)   
   {
      move_forward();
      delay (300);
   } 
   // one obstacle on the left side   
   else if (left_avg >= CONF_SENSOR && right_avg < CONF_SENSOR)   
   {     
      turn_right ();  
      delay (300);   
   }   
   // one obstacle on the right side  
   else if (right_avg >= CONF_SENSOR && left_avg < CONF_SENSOR)
   {
      turn_left ();
      delay (300);
   }
   else
   {
      stop_motor();
      move_backward();
      delay(500);
      if (r % 2)
         rotate_left();
      else
         rotate_right();
      r++;
      delay (1500);
   }
}

Enfin le “main” de mon programme :

romeo.c : Fonction main du programme

#include "motor.h"
#include "sensor.h"

void setup(void)
{
  int i;
  for (i = 6; i     pinMode(i, OUTPUT);

  Serial.begin(19200);
  move_forward();
}
void loop(void)
{
  handle_obstacle();
  move_forward();
  delay(15);
}

Ainsi dans le fichier sensor.c, nous retrouvons le code correspondant à la gestion des obstacles. Nous avons deux capteurs actuellement sur la plateforme mobile.

TODO : AJOUTER PHOTO ROBOT + CAPTEURS + AJOUT CONE VISIBILITE

Le code ici reste très simple, nous allons récupérer 5 fois les valeurs sur nos capteurs et nous calculons la moyenne des valeurs obtenues. Cette étape permet d’augmenter la précision puisque les capteurs peuvent parfois renvoyer des valeurs erronées.

Ensuite nous avons 3 cas à gérer :

1. Aucun obstacle détecté par nos capteurs : J’avance tout droit
2. Un obstacle détecté sur l’un des capteurs : Je me dirige dans la direction opposé a l’obstacle détecté
3. Les deux capteurs détectent un obstacle : Je recule, effectue une rotation à 90° sur la gauche ou la droite.

Les résultats sont plutot encourageants comme vu lors d’un article précédent.