[arduino] détection d'obstacle

[invite3a248b26]

Bonjour,
Actuellement en terminale SI je dois faire un projet pour permettre le déplacement d'une poubelle (le socle). Le socle est composé de 4 roues et de trois capteurs à ultrasons ( module grove de chez arduino :Télémètre à ultrasons Grove 101020010) à l'avant pour éviter les obstacles. Mon rôle est de faire déplacer le socle de la poubelle sans avoir de collision. J'ai donc déjà fais un programme qui fonctionne.

#include "Ultrasonic.h"

long RangeInCentimeters;  //déclaration d'une variable de type long

Ultrasonic capteurA(8);
Ultrasonic capteurB(3);
Ultrasonic capteurC(2);
void setup()
{
    Serial.begin(9600);

  //Configuration moteur A
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);


  //Configuration moteur B
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
}

void loop()
{
    Serial.print("La distance avec l'obstacle est :  ");
    RangeInCentimeters = capteurA.MeasureInCentimeters();
    Serial.print(RangeInCentimeters);
    Serial.println(" cm");
    delay(1000);
if (RangeInCentimeters <= 50)
{
  arrete1();
}
else
{
  avance();
}


Serial.print("La distance avec l'obstacle est :  ");
    RangeInCentimeters = capteurB.MeasureInCentimeters();
    Serial.print(RangeInCentimeters);
    Serial.println(" cm");
    delay(1000);
if (RangeInCentimeters <= 50)
{
  arrete1();
}
else
{
  avance();
}

Serial.print("La distance avec l'obstacle est :  ");
    RangeInCentimeters = capteurC.MeasureInCentimeters();
    Serial.print(RangeInCentimeters);
    Serial.println(" cm");
    delay(1000);
if (RangeInCentimeters <= 50)
{
  arrete3();
}
else
{
  avance();
}


}
void avance()
{
   
  //Roue droite avance 

  digitalWrite(4, HIGH);
 
 
  //Roue gauche avance
  digitalWrite(6, HIGH);
  

 
  }
void arrete1()
{
  
   //Roue droite avance

  digitalWrite(4, HIGH);


  //Roue gauche stop
  digitalWrite(6, LOW);

  
  
 delay(1000);
 
  //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);
  
  
delay(3000);

  //Roue droite stop 

  digitalWrite(4, LOW);
 

  //Roue gauche avance
  digitalWrite(6, HIGH);

  
delay(1000);

   //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);

delay(3000);

  //Roue droite stop 

  digitalWrite(4, LOW);


  //Roue gauche avance
  digitalWrite(6, HIGH);

  
delay(1000);

   //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);

delay(3000);

 //Roue droite avance

  digitalWrite(4, HIGH);


  //Roue gauche stop
  digitalWrite(6, LOW);

  
  
 delay(1000);
 

  //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);
 
}
 






void arrete3()
{
    //Roue droite stop 

  digitalWrite(4, LOW);


  //Roue gauche avance
  digitalWrite(6, HIGH);

  
delay(1000);

   //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);

delay(3000);

 //Roue droite avance

  digitalWrite(4, HIGH);


  //Roue gauche stop
  digitalWrite(6, LOW);

  
 delay(1000);
 
    //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);
  

delay(3000);

 //Roue droite avance

  digitalWrite(4, HIGH);


  //Roue gauche stop
  digitalWrite(6, LOW);

 
  
 delay(1000);
 
     //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);
  

delay(3000);

    //Roue droite stop 

  digitalWrite(4, LOW);


  //Roue gauche avance
  digitalWrite(6, HIGH);

  
delay(1000);

  //Roue droite avance 

  digitalWrite(4, HIGH);
  

  //Roue gauche avance
  digitalWrite(6, HIGH);





 



  
  
  }

Le problème de ce programme c'est qu'une fois un objet détecté il le détourne sans savoir si il y a un autre objet ou pas sur son passage. Donc j'ai essayer de modifier le programme sans jamais réussir. J'ai donc besoin de votre aide si possible .
Merci

PS : matériel utilisé :
-Télémètre à ultrasons Grove 101020010
-Module relais shield V3.0 103030009
-Carte Arduino UNO
-----