#include <NewPing.h>

int Vitesse1 = 5;    // controle de vitesse du moteur 1
int Vitesse2 = 6;    // controle de vitesse du moteur 2
int Sens1 = 4;    // controle du sens du moteur 1
int Sens2 = 7;    // controle du sens du moteur 2

int Port_Servo_Sonar = 
  
#define TRIGGER_PIN  12
#define ECHO_PIN     11
#define MAX_DISTANCE 200
 
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

#define A_DROITE  1
#define A_GAUCHE  2
#define RECULE    3

void stop(void)                    //Stop
{
  digitalWrite(Vitesse1,LOW);   
  digitalWrite(Vitesse2,LOW);      
}   
void advance(char a,char b)          //Move forward
{
  analogWrite (Vitesse1,a);      //PWM Speed Control
  digitalWrite(Sens1,HIGH);    
  analogWrite (Vitesse2,b);    
  digitalWrite(Sens2,HIGH);
}  
void back_off (char a,char b)          //Move backward
{
  analogWrite (Vitesse1,a);
  digitalWrite(Sens1,LOW);   
  analogWrite (Vitesse2,b);    
  digitalWrite(Sens2,LOW);
}
void turn_L (char a,char b)             //Turn Left
{
  analogWrite (Vitesse1,a);
  digitalWrite(Sens1,LOW);    
  analogWrite (Vitesse2,b);    
  digitalWrite(Sens2,HIGH);
}
void turn_R (char a,char b)             //Turn Right
{
  analogWrite (Vitesse1,a);
  digitalWrite(Sens1,HIGH);    
  analogWrite (Vitesse2,b);    
  digitalWrite(Sens2,LOW);
}
void setup(void) 
{ 
  int i;
  for(i=4;i<=7;i++)
    pinMode(i, OUTPUT);  
 // Serial.begin(19200);      //Set Baud Rate
 
 delay(100);
 advance(50,50);
  
} 
void loop(void) 
{
 delay(50);
  int distance = sonar.ping_cm();
  if (distance < DISTANCE_ALERTE) {
    stop();
    int tourne = choisir_cote(distance);
  }   
}

int choisir_cote(int distance_tout_droit) {
  tourne_sonar_gauche();
  int distance_gauche = sonar.ping_cm();
  tourne_sonar_droite();
  int distance_droite = sonar.ping_cm();
  if ((distance_gauche > distance_tout_droit) && (distance_gauche > DISTANCE_ALERTE)) {
    return A_GAUCHE;
  } else   if ((distance_gauche > distance_tout_droit) && (distance_gauche > DISTANCE_ALERTE)) {
    return A_DROITE;
  } 
  return RECULE;
}

void tourne_sonar_gauche() {
}

void tourne_sonar_droite() {
}