#include <Servo.h>

Servo myservo;

//ultrasonic sensor  1
const int Trig1 = 13;
const int Echo1 = 12;

//ultrasonic sensor 2
const int Trig2 = 10;
const int Echo2 = 11;

//servo motor pin(can also not declare this here and just go straight myservo.attach(9);)
const int ServoPin = 9;//always attach servo to PWM pins

void setup() {
  Serial.begin(9600);
  myservo.attach(ServoPin);
  
  pinMode(Trig1, OUTPUT);
  pinMode(Echo1, INPUT);
  
  pinMode(Trig2, OUTPUT);
  pinMode(Echo2, INPUT);
}

void loop() {
  //check the first ultrasonic sensor if object is present
  if (isObstaclePresent(Trig1, Echo1)) {
    rotateServo(90); //rotate servo to 90 degrees
    while (isObstaclePresent(Trig1, Echo1)) {
      //wait for the object to leave, then close if there's no object present
      delay(100);
    }
    rotateServo(0);//rotate back to 0 degrees when there's no object near
  }

  //check the second ultrasonic sensor if object is present
  if (isObstaclePresent(Trig2, Echo2)) {
    rotateServo(90); //rotate servo to 90 degrees
    while (isObstaclePresent(Trig2, Echo2)) {
      //wait for the object to leave, then close if there's no object present
      delay(100);
    }
    rotateServo(0);//rotate back to 0 degrees when there's no object near
  }
}

bool isObstaclePresent(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);//trigger the sensor to test it
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);//measure the duration or how long till the pulse reach the object and bounce back to the echo pin
  int distance = duration * 0.034 / 2;//computation for calculating the distrance. duration x speed of sound in air / two

  Serial.print("Distance: ");
  Serial.println(distance);

  return distance <= 20; //distance limit
}

void rotateServo(int angle) {
  myservo.write(angle);
  delay(500); //delay for the servo
}