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

LiquidCrystal_I2C lcd(0x27,16,2);  // Set the LCD I2C address
const int trigPin = 2; // Mengatur pin Trig pada pin 2
const int echoPin = 4; // Mengatur pin Echo pada pin 4
const int pinBuzzer = 9; // mengatur buzzer pada pin 9
int distance, distance_max = 20; // mengatur jarak maksimal 20cm
int ledmerah = 14; // Mengatur LED pada pin 14
int ledhijau = 15; // Mengatur LED pada pin 15
Servo servo1; // Deklarasi servo
Servo servo2;
int pos = 0; // servo1

void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  Serial.begin(115200); 
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(trigPin, OUTPUT); // Mengatur pin Trig sebagai OUTPUT
  pinMode(echoPin, INPUT); // Mengatur pin Echo sebagai INPUT
  pinMode(ledmerah, OUTPUT);
  pinMode(ledhijau, OUTPUT);
  servo1.attach(10);
  servo2.attach(8);
  lcd.begin(16,2);
  lcd.backlight();
  //lcd.clear();
}

// the loop function runs over and over again forever
void loop() {
  digitalWrite(LED_BUILTIN, HIGH);  // turn the LED on (HIGH is the voltage level)
  delay(1000);                      // wait for a second

  digitalWrite(LED_BUILTIN, LOW);   // turn the LED off by making the voltage LOW
  delay(1000);                      // wait for a second
  long duration, distance;
  lcd.clear();

  // Mengirim pulsa ultrasonik
  digitalWrite(trigPin, LOW); // Set Trig LOW untuk memastikan awal yang bersih
  delayMicroseconds(2); // Menunggu sebentar
  digitalWrite(trigPin, HIGH); // Set Trig HIGH untuk mengirimkan pulsa ultrasonik
  delayMicroseconds(10); // Menahan pulsa Trig HIGH untuk beberapa waktu
  digitalWrite(trigPin, LOW); // Set Trig kembali ke LOW

  // Menerima sinyal echo dan mengukur waktu
  duration = pulseIn(echoPin, HIGH); // Mengukur waktu saat Echo HIGH

  // Menghitung jarak dalam centimete
  distance = duration * 0.034 / 2; // Rumus sederhana untuk menghitung jarak
  Serial.print("Jarak: "); // Mencetak teks "Jarak: "
  Serial.print(distance); // Mencetak nilai jarak
  Serial.println(" CM"); // Mencetak teks "cm" dan pindah ke baris berikutnya
  lcd.setCursor(0,0);             // karakter dimulai dari kolom 1 baris 3
  lcd.print("Jarak: ");            // Karakter yang akan ditampilkan pada LCD
  lcd.print(distance);               // Menampilkan data jarak pada LCD
  lcd.print(" CM");
  

  if (distance < distance_max){                // instruksi untuk mengaktifkan LED jika jarak benda kurang dari 20 cm (jarak_max)
    digitalWrite(ledmerah, HIGH);
    digitalWrite(ledhijau, LOW);
    digitalWrite(pinBuzzer, HIGH);
    lcd.setCursor(0,1);
    lcd.print("STOP!!");
    servo1.write(90);
    servo2.write(90);

  }else {
    digitalWrite(ledmerah, LOW);              // instruksi untuk mematikan LED jika jarak benda lebih dari 20 cm (jarak_max)
    digitalWrite(ledhijau, HIGH);
    digitalWrite(pinBuzzer, LOW);
    lcd.setCursor(0,1);
    lcd.print("SHIP!!");
    servo1.write(0);
    servo2.write(0);
  }


  //LED
}

//KODE UNTUK SERVO