#include <Adafruit_Sensor.h>
#include <ESP32Servo.h>

const int trigPin1 = 16;
const int echoPin1 = 4;
const int trigPin2 = 17;
const int echoPin2 = 5;

long duration1, duration2;
int distance1, distance2;

static const int servoPin5 = 2;
static const int servoPin6 = 15;

Servo servo5;
Servo servo6;

void setup() {
  Serial.begin(9600);

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  servo5.setPeriodHertz(50);  // Standard 50 Hz servo
  servo6.setPeriodHertz(50);  // Standard 50 Hz servo
  servo5.attach(servoPin5, 500, 2400);  // attach servo5 to pin with min and max pulse width
  servo6.attach(servoPin6, 500, 2400);  // attach servo6 to pin with min and max pulse width
  
  servo5.write(30);
  servo6.write(30);
}

void loop() {
  ultrasonic1();
  ultrasonic2();
}

void ultrasonic1() {
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);

  duration1 = pulseIn(echoPin1, HIGH);

  distance1 = duration1 * 0.034 / 2;

  Serial.print("Distance 1: ");
  Serial.println(distance1);
  delay(1000);

  if (distance1 > 30) {
      servo5.write(180);
      delay(10);  // smaller delay for smoother motion
    }
  else {
      servo5.write(60);
      delay(10);  // smaller delay for smoother motion
    }
}

void ultrasonic2() {
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);

  duration2 = pulseIn(echoPin2, HIGH);

  distance2 = duration2 * 0.034 / 2;

  Serial.print("Distance 2: ");
  Serial.println(distance2);
  delay(1000);

  if (distance2 > 30) {
      servo6.write(90);
      delay(10);  // smaller delay for smoother motion
  } else {
      servo6.write(120);
      delay(10);  // smaller delay for smoother motion
    }
}