#include <ESP32Servo.h>

const int trigPin = 12;
const int echoPin = 11;
const int servoPin =39;

Servo servoMotor;

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

float getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

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

  return distance;
}

void turnServo() {
  servoMotor.write(90); // Initial position
  delay(500);

  while (true) {
    float dist = getDistance();
    Serial.print("Distance: ");
    Serial.print(dist);
    Serial.println(" cm");

    if (dist <= 5) { // If distance is less than or equal to 5 cm, stop the servo motor
      Serial.println("Obstacle detected. Stopping servo motor.");
      servoMotor.write(0); // Stop servo motor
      break;
    }

    servoMotor.write(180); // Turn servo motor to 180 degrees
    delay(500);

    servoMotor.write(0); // Turn servo motor to 0 degrees
    delay(500);
  }
}

void loop() {
  turnServo();
}