#include <Servo.h>

Servo siservo;
#define ECHO_PIN 6
#define TRIG_PIN 5

void setup() {
  siservo.attach(3);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // Memulai servo di posisi 0 derajat
  siservo.write(0);
}

float jarak_cm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  // Menghitung jarak dalam cm (duration dalam microseconds)
  return duration * 0.034 / 2;
}

void loop() {
  float cm = jarak_cm();

  // Jika jarak kurang dari 200 cm, gerakkan servo 180 derajat
  if (cm <= 200) {
    siservo.write(180); // Gerakkan servo ke 180 derajat
    delay(1000); // Tunggu 1 detik
    siservo.write(0); // Kembali ke posisi 0 derajat
    delay(1000); // Tunggu 1 detik sebelum pembacaan berikutnya
  } else {
    // Jika tidak ada objek terdeteksi, tetap di posisi 0 derajat
    siservo.write(0); 
  }

  delay(100); // Delay untuk stabilitas pembacaan
}