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

// Inisialisasi servo dan LCD
Servo myServo;
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Sesuaikan alamat I2C jika berbeda

// Pin sensor ultrasonik
const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;

// Variabel untuk jarak
long duration1, distance1;
long duration2, distance2;

// Jarak minimal untuk membuka gerbang
const int threshold = 10;

void setup() {
  // Setup servo dan posisi awal pada 0 derajat (tertutup)
  myServo.attach(9);
  myServo.write(0);
  Serial.begin(9600);

  // Inisialisasi pin sensor ultrasonik
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  // Inisialisasi LCD
  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() {

  // Baca jarak dari sensor ultrasonik 1
  distance1 = getDistance(trigPin1, echoPin1);

  // Jika objek terdeteksi di bawah threshold 5 cm di ultrasonik 1, buka gerbang
  if (distance1 < threshold) {
    myServo.write(90);  // Buka gerbang
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Palang Terbuka");
    Serial.println(distance1);
    Serial.println("Palang Terbuka");

    while (getDistance(trigPin1, echoPin1) < threshold) {
      delay(100);  // Tunggu sebentar sebelum cek lagi
    }
  }


  // Cek jika ada objek di bawah 5 cm di ultrasonik 2 untuk menutup gerbang
  distance2 = getDistance(trigPin2, echoPin2);
  if (distance2 < threshold) {
    delay(1000);  // Tunggu 5 detik sebelum menutup

    myServo.write(0);  // Tutup gerbang
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Palang Tertutup");
    
    Serial.println(distance2);
    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);

  // Hitung waktu balik suara
  long duration = pulseIn(echoPin, HIGH);

  // Konversi waktu ke jarak dalam cm
  long distance = duration * 0.034 / 2;

  return distance;
}