#include <Servo.h>

// Constants (won't change)
const int TRIG_PIN = 2; // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN = 3; // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int SERVO_PIN1 = 5; // Arduino pin connected to the first Servo Motor
const int SERVO_PIN2 = 6; // Arduino pin connected to the second Servo Motor
const int DISTANCE_THRESHOLD = 50; // Distance threshold in centimeters

Servo servo1; // Create servo object for the first servo motor
Servo servo2; // Create servo object for the second servo motor

void setup() {
  Serial.begin(9600); // Initialize serial communication
  pinMode(TRIG_PIN, OUTPUT); // Set TRIG pin to output mode
  pinMode(ECHO_PIN, INPUT); // Set ECHO pin to input mode
  servo1.attach(SERVO_PIN1); // Attach the first servo to its pin
  servo2.attach(SERVO_PIN2); // Attach the second servo to its pin
  servo1.write(0); // Initialize the first servo to 0 degrees
  servo2.write(0); // Initialize the second servo to 0 degrees
}

void loop() {
  // Generate a 10-microsecond pulse to the TRIG pin
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Measure the duration of the pulse from the ECHO pin
  float duration_us = pulseIn(ECHO_PIN, HIGH); 

  // Calculate the distance in centimeters
  float distance_cm = 0.017 * duration_us;

  // Control the servo motors based on the distance
  if (distance_cm >= 5 && distance_cm <= 70) {
    servo1.write(0); // Rotate the first servo to 90 degrees
    delay(100);
    servo1.write(45); // Rotate the first servo to 90 degrees
    delay(100);
    servo2.write(0); // Rotate the second servo to 90 degrees
    delay(100);
    servo2.write(45); // Rotate the second servo to 90 degrees
  } else {
    servo1.write(0); // Rotate the first servo to 0 degrees
    servo2.write(0); // Rotate the second servo to 0 degrees
  }
}