#include <Wire.h>               // Library komunikasi I2C
#include <LiquidCrystal_I2C.h>  // Library LCD I2C
#include <Servo.h>              // Library Servo

// Deklarasi pin dan variabel
#define SERVO_PIN 11            // Pin untuk servo
#define TRIG_PIN 2              // Pin TRIG untuk sensor ultrasonik
#define ECHO_PIN 3              // Pin ECHO untuk sensor ultrasonik

// Objek untuk servo dan LCD
Servo myservo;
LiquidCrystal_I2C lcd(0x27, 16, 2); // LCD I2C dengan alamat 0x27, ukuran 16x2

void setup() {
  // Inisialisasi servo dan sensor jarak
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  myservo.attach(SERVO_PIN);

  // Inisialisasi LCD
  lcd.begin(16, 2);
  lcd.backlight(); // Nyalakan lampu latar LCD

  // Pesan awal pada LCD
  lcd.setCursor(0, 0);
  lcd.print("Servo & Distance");
  lcd.setCursor(0, 1);
  lcd.print("System Starting..");
  delay(2000);
  lcd.clear();
}

void loop() {
  // ===== Pembacaan Sensor Jarak =====
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Hitung waktu pantulan
  long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = duration * 0.034 / 2; // Menghitung jarak dalam cm

  // Validasi jarak
  if (distance > 400 || distance <= 0) {
    distance = 400; // Jika jarak tidak valid, set ke nilai maksimal
  }

  // ===== Kontrol Servo Berdasarkan Jarak =====
  int servoPos = map(distance, 0, 400, 0, 180); // Batasi jarak hingga 400 cm
  myservo.write(servoPos); // Gerakkan servo ke posisi yang dihitung
  delay(15); // Waktu untuk servo bergerak

  // ===== Tampilkan Data di LCD =====
  lcd.setCursor(0, 0);
  lcd.print("Servo Angle: ");
  lcd.print(servoPos); // Tampilkan sudut servo
  lcd.print("  ");

  lcd.setCursor(0, 1);
  lcd.print("Dist: ");
  lcd.print(distance, 1); // Tampilkan jarak dalam cm
  lcd.print(" cm     ");

  delay(500); // Delay untuk stabilitas
}