#include <Servo.h>

// Definisikan pin yang digunakan
const int trigPin = 2;
const int echoPin = 3;
const int ledPin = 4;
const int ledPin1 = 5;
const int servoPin = 9;
const int buzzerPin = 6;

// Variabel untuk menyimpan jarak
long duration;
int distance;

Servo myServo;

void setup() {
  // Set pin trig sebagai OUTPUT
  pinMode(trigPin, OUTPUT);
  // Set pin echo sebagai INPUT
  pinMode(echoPin, INPUT);
  // Set pin led sebagai OUTPUT
  pinMode(ledPin, OUTPUT);
  pinMode(ledPin1, OUTPUT);
  // Attach servo
  myServo.attach(servoPin);
  // Start serial communication
  Serial.begin(9600);
}

void loop() {
  // Kirim sinyal ultrasonik
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Baca pulsa ultrasonik
  duration = pulseIn(echoPin, HIGH);
  
  // Hitung jarak dalam cm
  distance = duration * 0.034 / 2;
  
  // Tampilkan jarak pada Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);
  
  // Buzzer
  if (distance < 30) {
    tone(buzzerPin, 1000); // Nyalakan buzzer jika jarak kurang dari 30cm
  } else {
    noTone(buzzerPin); // Matikan buzzer jika jarak lebih dari 30cm
  }
  
  // Servo
  int angle = map(distance, 0, 200, 0, 180); // Map jarak ke sudut untuk servo
  myServo.write(angle); // Posisikan servo sesuai dengan jarak
  
  // LED
  if (distance < 30) {
    digitalWrite(ledPin, HIGH); // Nyalakan LED jika jarak kurang dari 30cm
    digitalWrite(ledPin1, LOW);
  } else {
    digitalWrite(ledPin, LOW); // Matikan LED jika jarak lebih dari 30cm
    digitalWrite(ledPin1, HIGH);
  }
  
  // Tunggu sebentar sebelum mengukur lagi
  delay(1000);
}