#include <Servo.h>

Servo servo1;
Servo servo2;

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

void setup() {
  servo1.attach(9);
  servo2.attach(10);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void moveServoToPosition(Servo &servo, int targetPosition) {
  int currentPosition = servo.read();
  if (currentPosition < targetPosition) {
    for (int pos = currentPosition; pos <= targetPosition; pos += 1) {
      servo.write(pos);
      delay(5);
    }
  } else if (currentPosition > targetPosition) {
    for (int pos = currentPosition; pos >= targetPosition; pos -= 1) {
      servo.write(pos);
      delay(5);
    }
  }
}

void loop() {
  // Ultrasonic sensor code
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;

  if (distance >= 7 && distance <= 17) {
    // Object detected in the specified range for Servo 1
    moveServoToPosition(servo1, 180);

    // Wait for 3 seconds
    delay(3000);

    // Wait for 5 seconds without detecting an object
    unsigned long startMillis = millis();
    while (millis() - startMillis < 5000) {
      // Continue to check for objects
      digitalWrite(trigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      long duration = pulseIn(echoPin, HIGH);
      int distance = duration * 0.034 / 2;

      if (distance >= 7 && distance <= 17) {
        // Object detected again, exit the loop
        break;
      }
    }

    // Move Servo 1 back to its original position
    moveServoToPosition(servo1, 0);

    // If an object is detected again, move Servo 2
    if (distance >= 7 && distance <= 17) {
      moveServoToPosition(servo2, 180);

      // Wait for 5 seconds
      delay(5000);

      // Move Servo 2 back to its original position
      moveServoToPosition(servo2, 0);
    }
  }

  // Add a delay to avoid constant checking
  delay(100);
}