#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

NewPing sonar(trigPin, echoPin, MAX_DISTANCE);

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(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 jarak = sonar.ping_cm();  // Gunakan ping_cm untuk membaca jarak
  if (jarak == 0) jarak = MAX_DISTANCE;  // Jika tidak terbaca, set ke jarak maksimal
  
  Serial.print(jarak);
  Serial.println(" cm");

  // Logika kontrol 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
    digitalWrite(LEDmerah, HIGH);  // LEDmerah hidup
  } else {  // Jika jarak lebih dari 15 cm
    digitalWrite(LEDhijau, HIGH);  // LEDhijau hidup
    digitalWrite(LEDmerah, LOW);   // LEDmerah mati
    myservo.write(90);             // Posisi servo pada 90 derajat
  }
}