#include <NewPing.h>
#include <Servo.h>

#define trigPin 12
#define echoPin 13
#define MAX_DISTANCE 500

NewPing sonar(trigPin, echoPin, MAX_DISTANCE);
const int LEDhijau = 3; 
const int LEDmerah = 2;

Servo myservo;

void setup() {
  Serial.begin(115200); // Kecepatan komunikasi serial.
  pinMode(trigPin, OUTPUT); // Set pin Trigger sebagai output.
  pinMode(echoPin, INPUT);  // Set pin Echo sebagai input.
  pinMode(LEDmerah, OUTPUT); // Set LED merah sebagai output.
  pinMode(LEDhijau, OUTPUT); // Set LED hijau sebagai output.

  myservo.attach(9);        // Set servo pada pin PWM 9.
  myservo.write(90);        // Posisi default servo pada 90 derajat.
}

void loop() {
  int duration, jarak;

  // Mengirimkan sinyal ultrasonik
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Mengukur waktu pantulan
  duration = pulseIn(echoPin, HIGH);
  jarak = (duration / 2) / 29.1; // Menghitung jarak dalam cm.

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

  if (jarak <= 15) {
    // Jika jarak kurang atau sama dengan 15 cm.
    digitalWrite(LEDhijau, LOW);  // LED hijau mati.
    digitalWrite(LEDmerah, HIGH); // LED merah hidup.

    // Gerakan servo dari 0 hingga 180 derajat.
    myservo.write(180);
    delay(450);

    digitalWrite(LEDmerah, LOW);  // LED merah mati.
    myservo.write(90);           // Servo kembali ke posisi tengah.
    delay(450);

    digitalWrite(LEDmerah, HIGH); // LED merah hidup.
    myservo.write(0);            // Servo bergerak ke posisi 0 derajat.
    delay(450);

    digitalWrite(LEDmerah, LOW);  // LED merah mati.
    myservo.write(90);           // Servo kembali ke posisi tengah.
  } else {
    // Jika jarak lebih dari 15 cm.
    digitalWrite(LEDmerah, LOW);  // LED merah mati.
    digitalWrite(LEDhijau, HIGH); // LED hijau hidup.
    myservo.write(90);           // Servo tetap pada posisi tengah.
  }

  delay(450); // Delay untuk menghindari pembacaan terlalu cepat.
}