#include <Servo.h>

const int trigPin = 9;    // Trig Pin of SOR4 ultrasonic sensor
const int echoPin = 10;   // Echo Pin of SOR4 ultrasonic sensor
const int servoPin = 3;   // PWM Pin for SG90 servo motor

Servo servoMotor;

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

void loop() {
  long duration, distance;
  
  // Clear the trigger pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Set the trigger pin to high for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echo pin, and calculate the distance
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

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

  // If the distance is less than 5 inches (12.7 cm), rotate the servo motor to 90 degrees
  if (distance < 12.7) {
    servoMotor.write(90); // Rotate the servo motor to 90 degrees
    delay(500); // Wait for the servo to reach its position
  } else {
    // If the object disappears, rotate the servo motor back to 0 degrees
    servoMotor.write(0); // Rotate the servo motor to 0 degrees
    delay(500); // Wait for the servo to reach its position
  }

  delay(100); // Small delay for stability
}