#include <Servo.h>
#include <LiquidCrystal_I2C.h>
// Configuration des broches pour le premier capteur à ultrasons
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4
const int TRIG_PIN1 = 3;
const int ECHO_PIN1 = 2;
const int SERVO_PIN1 = 9;
const int LED_PIN1 = 10;
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);
// Configuration des broches pour le deuxième capteur à ultrasons
const int TRIG_PIN2 = 5;
const int ECHO_PIN2 = 4;
const int SERVO_PIN2 = 6;
const int LED_PIN2 = 7;
const int SERVO_PIN3 = 11;
Servo servo1, servo2, servo3;





void setup() {
  servo1.attach(SERVO_PIN1);
  servo2.attach(SERVO_PIN2);
  servo3.attach(SERVO_PIN3);
  pinMode(LED_PIN1, OUTPUT);
  pinMode(LED_PIN2, OUTPUT);
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();
  lcd.begin(16, 2);
  lcd.setCursor(3, 0);
  lcd.clear();
  loop();
  

}

float readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int duration = pulseIn(echoPin, HIGH);
  float distance = duration * 0.034 / 2;
  return distance;
  
}

void loop() {
  // Contrôle du premier capteur à ultrasons et de la première LED
  float distance2 = readDistance(TRIG_PIN2, ECHO_PIN2);
  float distance1 = readDistance(TRIG_PIN1, ECHO_PIN1);
  
  
  if (distance1 < 50 | distance2 < 100) {
    servo3.write(90);
    delay(500);
    servo1.write(0);
    servo2.write(0); // Tourne le servo-moteur à gauche
    digitalWrite(LED_PIN1, HIGH);
    digitalWrite(LED_PIN2, LOW); // Allume la LED
    
    
  } else {
    servo3.write(0);
    delay(500);
    servo1.write(90);
    servo2.write(90); // Tourne le servo-moteur à droite
    digitalWrite(LED_PIN1, LOW); 
    digitalWrite(LED_PIN2, HIGH);
  }// Éteint la LED
  if (distance1 < 50 | distance2 < 100) {
    lcd.clear();
    lcd.print("Avancez tranquille");
    
  }
  else{
    lcd.clear();
    lcd.print("IL Y A LES ANDEK");
  }
  
}