/*
 * This ESP32 code is created by esp32io.com
 *
 * This ESP32 code is released in the public domain
 *
 * For more detail (instruction and wiring diagram), visit https://esp32io.com/tutorials/esp32-servo-motor
 */

#include <ESP32Servo.h>

const int trigPin = 22;  // Ultrasonic sensor trigger pin
const int echoPin = 23;  // Ultrasonic sensor echo pin
const int servoPin = 2; // Servo motor control pin

Servo myServo;

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

void loop() {
  long duration, distance;

  // Trigger the ultrasonic sensor to measure distance
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

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

  // Calculate the distance in centimeters
  distance = duration * 0.034 / 2;

  // Display the distance on the serial monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Rotate the servo based on distance conditions
  if (distance >= 10 && distance <= 50) {
    rotateClockwise();
  } else if (distance > 50 && distance <= 100) {
    rotateAntiClockwise();
  }

  delay(500);  // Add a delay to control the rate of measurements and servo movement
}

void rotateClockwise() {
  Serial.println("Rotating +360° (clockwise)");
  myServo.write(0);  // Adjust the angle as needed
  delay(1000);  // Adjust the delay as needed
}

void rotateAntiClockwise() {
  Serial.println("Rotating -360° (anti-clockwise)");
  myServo.write(180);  // Adjust the angle as needed
  delay(1000);  // Adjust the delay as needed
}