#include <Servo.h>

Servo servo1; // create servo object to control servo 1
Servo servo2; // create servo object to control servo 2
Servo servo3; // create servo object to control servo 3

const int servo1Pin = 2; // GPIO pin connected to servo 1
const int servo2Pin = 4; // GPIO pin connected to servo 2
const int servo3Pin = 5; // GPIO pin connected to servo 3

const int trigPin1 = 12; // GPIO pin connected to Ultrasonic Sensor 1's TRIG pin
const int echoPin1 = 13; // GPIO pin connected to Ultrasonic Sensor 1's ECHO pin

const int trigPin2 = 14; // GPIO pin connected to Ultrasonic Sensor 2's TRIG pin
const int echoPin2 = 15; // GPIO pin connected to Ultrasonic Sensor 2's ECHO pin

const int trigPin3 = 16; // GPIO pin connected to Ultrasonic Sensor 3's TRIG pin
const int echoPin3 = 17; // GPIO pin connected to Ultrasonic Sensor 3's ECHO pin

const int distanceThreshold = 10; // Distance threshold in centimeters

void setup() {
  servo1.attach(servo1Pin); // attaches the servo on pin 2 to servo1 object
  servo2.attach(servo2Pin); // attaches the servo on pin 4 to servo2 object
  servo3.attach(servo3Pin); // attaches the servo on pin 5 to servo3 object

  pinMode(trigPin1, OUTPUT); // set Ultrasonic Sensor 1's TRIG pin as output
  pinMode(echoPin1, INPUT);  // set Ultrasonic Sensor 1's ECHO pin as input

  pinMode(trigPin2, OUTPUT); // set Ultrasonic Sensor 2's TRIG pin as output
  pinMode(echoPin2, INPUT);  // set Ultrasonic Sensor 2's ECHO pin as input

  pinMode(trigPin3, OUTPUT); // set Ultrasonic Sensor 3's TRIG pin as output
  pinMode(echoPin3, INPUT);  // set Ultrasonic Sensor 3's ECHO pin as input
}

void loop() {
  // Check distance for Ultrasonic Sensor 1
  if (checkDistance(trigPin1, echoPin1)) {
    servo1.write(90); // Move servo 1 to 90 degrees
  } else {
    servo1.write(0);  // Move servo 1 to 0 degrees
  }

  // Check distance for Ultrasonic Sensor 2
  if (checkDistance(trigPin2, echoPin2)) {
    servo2.write(90); // Move servo 2 to 90 degrees
  } else {
    servo2.write(0);  // Move servo 2 to 0 degrees
  }

  // Check distance for Ultrasonic Sensor 3
  if (checkDistance(trigPin3, echoPin3)) {
    servo3.write(90); // Move servo 3 to 90 degrees
  } else {
    servo3.write(0);  // Move servo 3 to 0 degrees
  }

  delay(100); // Add a short delay to reduce CPU usage
}

bool checkDistance(int trigPin, int echoPin) {
  // Generate a 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the duration of pulse from ECHO pin
  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance in centimeters
  int distance = duration * 0.034 / 2; // Speed of sound is 0.034 cm/microsecond

  // Check if distance is within threshold
  return distance < distanceThreshold;
}