#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

Servo myservo1;
Servo myservo2;

#define trig1 18
#define echo1 5
#define echo2 16
#define trig2 4
#define buzzer 19
#define led1 2
#define led2 27

LiquidCrystal_I2C lcd(0x27,16,2);

void setup() {
  // put your setup code here, to run once:


  myservo1.attach(14);
  myservo2.attach(15);

  pinMode(trig1, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo2, INPUT);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(buzzer, OUTPUT);
  delayMicroseconds(2);

  lcd.init();
  lcd.backlight();
  lcd.clear();
}

void loop() {
  digitalWrite(trig1, LOW);
  delayMicroseconds(2);
  digitalWrite(trig1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig1, LOW);
  long duration1 = pulseIn(echo1, HIGH);
  float distance1 = duration1 /58.0;  

  // Membaca jarak dari sensor ultrasonik 2
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig2, LOW);
  long duration2 = pulseIn(echo2, HIGH);
  float distance2 = duration2 /58.0;  

  if (distance1 <= 10 && distance2 <= 10 && abs(distance1 - distance2) < 2) {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("TERTUTUP");
    digitalWrite(buzzer, HIGH);
    digitalWrite(led1, HIGH);
    digitalWrite(led2, LOW);
    delay(1000);
    digitalWrite(buzzer, HIGH);
    digitalWrite(led1, LOW);
    digitalWrite(led2, HIGH);
    myservo1.write(180); 
    myservo2.write(0); 
  } else if (distance1 > 10 && distance2 > 10) {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("TERBUKA");
    digitalWrite(buzzer, LOW);
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    myservo1.write(90);
    myservo2.write(90);
  }

delay(1000);

}