#include <ESP32Servo.h>
int trigpin = 4;
int echopin = 5;
int servopin1 = 2;
int servopin2 = 16;

Servo servo1st;
Servo servo2nd;
float duration, distance;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
  servo1st.attach(servopin1);
  servo2nd.attach(servopin2);
}

void loop() {
  digitalWrite(trigpin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin, LOW);
  duration = pulseIn(echopin, HIGH);
  distance = 0.017 * duration;

  if (distance <= 10) {
    servo1st.write(180);
    servo2nd.write(0);
  }
  else if (distance > 10, distance <=20){
    servo1st.write(0);
    servo2nd.write(180);
  }
  else{
    servo1st.write(0);
    servo2nd.write(0);
  }

  Serial.print("Distance in CM: ");
  Serial.println(distance);
  delay(1000); 


}

/* Jika jarak <= 1, maka servo1 bergerak 180 dan servo2 0
Jarak > 10 sampai <= 20, maka servo1 0 dan servo2 180
Selain itu 0 */