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

// Alamat I2C dari LCD
int i2c_address = 0x27; // Anda mungkin perlu mengganti ini sesuai dengan alamat LCD Anda

// Inisialisasi objek LCD
LiquidCrystal_I2C lcd(i2c_address, 16, 2); // 16 kolom, 2 baris

// Deklarasi PIN
int trig = 6;
int echo = 7;
long durasi, jarak;
int merah = 4;
int kuning = 3;
int hijau = 2;
int buzzer = 5;

// Deklarasi servo
Servo servoMotor;
int servoPin = 9; // Pin PWM untuk servo

void setup() {
  // Mengatur pin sebagai OUTPUT atau INPUT
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(merah, OUTPUT);
  pinMode(kuning, OUTPUT);
  pinMode(hijau, OUTPUT);
  pinMode(buzzer, OUTPUT);
  Serial.begin(9600);
  
  // Menginisialisasi LCD
  lcd.init();
  lcd.backlight(); // Menyalakan backlight LCD
  lcd.setCursor(0, 0); // Posisi awal cursor
  lcd.print("Jarak :"); // Menampilkan judul
  
  // Menginisialisasi servo
  servoMotor.attach(servoPin); // Menghubungkan servo ke pin PWM
  servoMotor.write(0); // Posisi awal servo (tertutup)
}

void loop() {
  // Mengukur jarak menggunakan sensor ultrasonik
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  durasi = pulseIn(echo, HIGH);
  jarak = (durasi / 2 / 29.1);
  
  // Menampilkan jarak di LCD
  lcd.setCursor(0, 2); // Posisi cursor LCD
  lcd.print("        "); // Membersihkan tampilan jarak sebelumnya
  lcd.setCursor(0, 1); // Posisi cursor LCD
  lcd.print(jarak); // Menampilkan jarak

  if (jarak >= 150) {
    lcd.print(" 'palang terbuka' "); // Menampilkan status hijau jika jarak >= 150
  } else if ((jarak >= 100) && (jarak < 150)) {
    lcd.print(" 'palang tertutup' "); // Menampilkan status kuning jika 100 < jarak < 150
  } else {
    lcd.print(" 'palang diam' "); // Menampilkan status merah jika jarak < 100
  }

  // Memutuskan tindakan berdasarkan jarak
  if (jarak >= 150) {
    digitalWrite(hijau, HIGH);
    digitalWrite(merah, LOW);
    digitalWrite(kuning, LOW); 
    digitalWrite(buzzer, LOW);
    servoMotor.write(0); // Servo kembali ke posisi awal (tertutup)
  } else if ((jarak >= 100) && (jarak < 150)) {
    digitalWrite(hijau, LOW);
    digitalWrite(merah, LOW);
    digitalWrite(kuning, HIGH); 
    digitalWrite(buzzer, LOW); 
    servoMotor.write(90); // Servo diam ketika kuning menyala
  } else {
    digitalWrite(hijau, LOW);
    digitalWrite(merah, HIGH);
    digitalWrite(kuning, LOW); 
    digitalWrite(buzzer, HIGH);
    servoMotor.write(90); // Servo terbuka ketika hijau menyala
  }

  delay(500); // Memberi sedikit jeda
}