#include <ESP32Servo.h>

const int trigPin = 23;
const int echoPin = 22;
const int redLEDPin = 12;
const int greenLEDPin = 13;
const int servoPin1 = 25;
const int servoPin2 = 21;

Servo servo1;
Servo servo2;

long duration;
int distance;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redLEDPin, OUTPUT);
  pinMode(greenLEDPin, OUTPUT);

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  servo1.write(0); // Initially closed
  servo2.write(0); // Initially closed

  Serial.begin(9600);
}

void loop() {
  // Clear the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Set the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);

  // Calculate the distance
  distance = duration * 0.034 / 2;

  // Print the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);

  // If the distance is less than 50 cm, open the garage door
  if (distance < 50) {
    digitalWrite(greenLEDPin, HIGH);
    digitalWrite(redLEDPin, LOW);

    // Open the garage door
    servo1.write(90);
    servo2.write(90);
  } else {
    digitalWrite(greenLEDPin, LOW);
    digitalWrite(redLEDPin, HIGH);

    // Close the garage door
    servo1.write(0);
    servo2.write(0);
  }

  delay(1000); // Check every second
}