#include <Servo.h>

const int trigPin = 9;     // Pin trigger sensor ultrasonik
const int echoPin = 10;    // Pin echo sensor ultrasonik
Servo servoMotor;           // Objek servo motor

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  servoMotor.attach(3);     // Hubungkan servo motor ke pin 3
  servoMotor.write(90);     // Set posisi awal servo (tegak)
  Serial.begin(9600);       // Inisialisasi komunikasi serial
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  float distance_cm = duration * 0.034 / 2;

  // Jarak 4 cm ke bawah: Servo tegak
  if (distance_cm <= 4) {
    servoMotor.write(0); // Posisi tegak
    Serial.println("Servo tegak.");
  }
  // Jarak 5 cm ke atas: Servo datar
  else if (distance_cm >= 5) {
    servoMotor.write(90); // Posisi datar
    Serial.println("Servo datar.");
  }

  // Tampilkan jarak pada Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  delay(100); // Tunggu sebentar sebelum mengukur kembali jarak
}