#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

Servo myServo;
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;

long duration1, distance1;
long duration2, distance2;
const int threshold = 10;

void setup() {
  myServo.attach(9);
  myServo.write(0);
  Serial.begin(9600);

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Sistem Siap...");
  Serial.println("sistem siap");
  delay(2000);
  lcd.clear();
  lcd.print("Palang Tertutup");
}

void loop() {
  distance1 = getDistance(trigPin1, echoPin1);

  if (distance1 < threshold) {
    myServo.write(90);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Palang Terbuka");
    Serial.println("Palang Terbuka");

    while (getDistance(trigPin1, echoPin1) < threshold) {
      delay(100);
    }
  }

  distance2 = getDistance(trigPin2, echoPin2);
  if (distance2 < threshold) {
    delay(1000);
    myServo.write(0);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Palang Tertutup");
    Serial.println("Palang Tertutup");
  }
}

// Fungsi untuk membaca jarak dari sensor ultrasonik
long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}