#include <HCSR04.h>
#include <ESP32Servo.h>
const int servoPin = 19;
const int R=34;
const int Y=35;
const int G=32;

Servo servo;

UltraSonicDistanceSensor  s1(23, 22); 
UltraSonicDistanceSensor s2(18, 5);

void setup () {
    Serial.begin(9600);  // We initialize serial connection so that we could print values from sensor.
    servo.attach(servoPin, 500, 2400);
    pinMode(R,OUTPUT);
    pinMode(Y,OUTPUT);
    pinMode(G,OUTPUT);
}

void loop () {
    // Every 500 miliseconds, do a measurement using the sensor and print the distance in centimeters.
    int jarak1 = s1.measureDistanceCm();
    int jarak2 = s2.measureDistanceCm();
    Serial.print(jarak1);
    Serial.print("--");
    Serial.println(jarak2);

    if(jarak1<20 && jarak2>5){
      servo.write(90);
      delay(2000);
      } else {servo.write(0);}

     if(jarak2>30){
      digitalWrite(G,HIGH);
      digitalWrite(Y,LOW);
      digitalWrite(R,LOW);
     }

     if(jarak2>5 && jarak2<20){
      digitalWrite(G,LOW);
      digitalWrite(Y,HIGH);
      digitalWrite(R,LOW); 
     }

     if(jarak2<5){
      digitalWrite(G,LOW);
      digitalWrite(Y,LOW);
      digitalWrite(R,HIGH); 
     }
 delay(500);
}