#include <HCSR04.h>
#include <ESP32Servo.h>

const int servoPin = 17;
const int R=2;
const int Y=4;
const int G=5;

Servo servo;
// Initialize sensor that uses digital pins 13 and 12.

UltraSonicDistanceSensor s1(14, 12);
UltraSonicDistanceSensor s2(25, 26);

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<15){
digitalWrite(G,LOW);
digitalWrite(Y,HIGH);
digitalWrite(R,LOW); 
}

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

}