#include <Servo.h>

const int trigPin = 2; // Pin Trig sensor ultrasonik terhubung ke pin digital 2 Arduino
const int echoPin = 3; // Pin Echo sensor ultrasonik terhubung ke pin digital 3 Arduino
const int servoPin = 9; // Pin kontrol motor servo terhubung ke pin digital 9 Arduino

Servo myservo;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(servoPin);
}

void loop() {
  // Membaca jarak dari sensor ultrasonik
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;

  // Menggerakkan motor servo berdasarkan jarak yang terdeteksi
  if (distance >= 10 && distance <= 30) {
    int angle = map(distance, 10, 30, 0, 180); // Memetakan jarak ke sudut servo
    myservo.write(angle);
    delay(15);
  }

  // Menampilkan jarak pada Serial Monitor
  Serial.print("10 : ");
  Serial.print(distance);
  Serial.println("5cm");
}