#include <ESP32Servo.h>

const int ledPin = 5;
const int buzzerPin = 6;
const int servoPin = 1;

const int trigPin = 2;
const int echoPin = 3;

Servo myservo;

void setup() {
  pinMode(ledPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);

  myservo.setPeriodHertz(50); // standard 50 Hz servo
  myservo.attach(servoPin); // attaches the servo on pin with servo min and max limits
  
  myservo.write(0);  // Set the servo to the initial position
  
  Serial.begin(115200);
  Serial.println("Hello, ESP32-C3!");
}

void loop() {
  // Trigger the ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  
  // Measure the duration of the echo pulse
  float duration = pulseIn(echoPin, HIGH);


  // Calculate distance in centimeters (assumes speed of sound is 343 meters/second)
  float jarak = (duration/9)*2.43;
  Serial.print("Distance: ");
  Serial.print(jarak);
  Serial.println(" cm");

  
  // Turn on the LED, and move the servo if distance is lower than 
  
  if (jarak <= 50) {
    digitalWrite(ledPin, HIGH);

    tone (6,500);
    myservo.write(270);  // Adjust the angle as needed
  } else {
    digitalWrite(ledPin, LOW);
    digitalWrite(servoPin, LOW);
    
    noTone(6);
    myservo.write(0);  // Set the servo back to the initial position
  }

  delay(500); // Adjust delay as needed
}