#include <NewPing.h>  // Library untuk Sensor Ultrasonic
#include <Servo.h>    // Library untuk Servo

#define trigPin 8    // Set Trigger HCSR04 di Pin digital 12
#define echoPin 7    // Set Echo HCSR04 di Pin digital 13
#define MAX_DISTANCE 500  // Set jarak maksimal

int LEDhijau = 2, LEDmerah = 3; // Set Pin LEDhijau dan LEDmerah
Servo myservo; // Buat object 1 buah motor servo

void setup() {
  Serial.begin(9600);  // Kecepatan komunikasi serial
  pinMode(trigPin, OUTPUT);  // Set pin Trigger sebagai output
  pinMode(echoPin, INPUT);   // Set pin Echo sebagai input
  pinMode(LEDmerah, OUTPUT); // Set LEDmerah sebagai output
  pinMode(LEDhijau, OUTPUT); // Set LEDhijau sebagai output
  myservo.attach(9);  // Set servo pada pin PWM 9
}

void loop() {
  int duration, jarak;

  // Hanya sekali eksekusi trigger dan durasi untuk satu pengukuran
  digitalWrite(trigPin, LOW);  // Mengatur Trigger ke LOW
  delayMicroseconds(2);        
  digitalWrite(trigPin, HIGH); // Mengatur Trigger ke HIGH
  delayMicroseconds(10);       
  digitalWrite(trigPin, LOW);  // Mengatur Trigger ke LOW kembali

  duration = pulseIn(echoPin, HIGH);  // Mengukur durasi pulse
  jarak = (duration / 2) / 29.1;     // Menghitung jarak (dalam cm)

  Serial.print(jarak);
  Serial.println(" cm");

  // Mengurangi pemanggilan pengaturan LED dan servo
  if (jarak > 0 && jarak <= 15) {  // Jika jarak kurang dari atau sama dengan 15 cm
    if (digitalRead(LEDhijau) == HIGH) {
      digitalWrite(LEDhijau, LOW);   // LEDhijau mati
      digitalWrite(LEDmerah, HIGH);  // LEDmerah hidup
      myservo.write(180);            // Posisi servo pada 180 derajat
      delay(4000);                    // Delay selama 450 ms
    }

    digitalWrite(LEDmerah, LOW);   // LEDmerah mati
    myservo.write(90);             // Posisi servo pada 90 derajat
    // delay(2000);                    // Delay selama 450 ms
    digitalWrite(LEDmerah, HIGH);  // LEDmerah hidup
  }
  else {
    // Jika jarak lebih dari 15 cm, reset LED dan servo
    digitalWrite(LEDhijau, HIGH);  // LEDhijau hidup
    digitalWrite(LEDmerah, LOW);   // LEDmerah mati
    myservo.write(90);             // Posisi servo pada 90 derajat
  }
}