#include <HCSR04.h>
#include <Servo.h>

Servo monServomoteur;
const int servoPin = 2;
const int trig1Pin = 3;
const int echo1Pin = 4;
const int trig2Pin = 5;
const int echo2Pin = 6;
const int LEDrouge = 7;
const int LEDvert = 8;

UltraSonicDistanceSensor distanceSensor1(trig1Pin, echo1Pin);
UltraSonicDistanceSensor distanceSensor2(trig2Pin,echo2Pin);

void setup() {
  monServomoteur.attach(servoPin);
  pinMode(LEDrouge, OUTPUT);
  pinMode(LEDvert, OUTPUT);

}

void loop() {
  bool detect1 = false;
  bool detect2 = false;

  if ((distanceSensor1.measureDistanceCm() + 1 < 150) && (distanceSensor1.measureDistanceCm() + 1 > 0)) {
    detect1 = true;
  }
  if ((distanceSensor2.measureDistanceCm() + 1 < 150) && (distanceSensor2.measureDistanceCm() + 1 > 0)) {
    detect2 = true;
  }

  if (detect1 || detect2) {
    monServomoteur.write(180);
    digitalWrite(LEDrouge, LOW); //la LED s'allume en rouge
    digitalWrite(LEDvert, HIGH); //la LED s'allume en vert

    if (detect2) {
      delay(5000);
    } else {
      while ((distanceSensor1.measureDistanceCm() + 1 < 150) && (distanceSenso1.measureDistanceCm() + 1 > 0)) {
        delay(100);
      }
      monServomoteur.write(180);
      digitalWrite(LEDrouge, HIGH); // la LED s'allume en rouge
      digitalWrite(LEDvert, LOW); 
    }
  } else {
    monServomoteur.write(90);
    digitalWrite(LEDrouge, HIGH);
    digitalWrite(LEDvert, LOW); // la LED s'allume en vert
  }
  delay(100);
}