#include <Servo.h>
Servo servo;
#define echo1 3
#define trig1 4
#define echo2 5
#define trig2 6
#define echo3 7
#define trig3 8
#define led 2

void setup() {
  // put your setup code here, to run once:
  pinMode(echo1, INPUT);
  pinMode(trig1, OUTPUT);
  pinMode(echo2, INPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo3, INPUT);
  pinMode(trig3, OUTPUT);
  servo.attach(9);
  Serial.begin(9600);
  pinMode(led, OUTPUT);
  digitalWrite(led, LOW);
}
float jrk3(){
  digitalWrite(trig1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig1, LOW);
  int durasi = pulseIn(echo1, HIGH);
  return durasi / 57.95;
}
float jrk2(){
  digitalWrite(trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig2, LOW);
  int durasi = pulseIn(echo2, HIGH);
  return durasi / 57.95;
}
float jrk1(){
  digitalWrite(trig3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig3, LOW);
  int durasi = pulseIn(echo3, HIGH);
  return durasi / 57.95;
}

void loop() {
  // put your main code here, to run repeatedly:
  float sensor1 = jrk1();
  float sensor2 = jrk2();
  float sensor3 = jrk3();
  Serial.print("Sensor1 : ");
  Serial.print(sensor1);
  Serial.print("  |Sensor2 : ");
  Serial.print(sensor2);
  Serial.print("  |sensor3 : ");
  Serial.println(sensor3);

  if(sensor1 < 4 || sensor2 < 4 || sensor3 < 4){
    digitalWrite(led, LOW);
    servo.write(90);
  }else if(sensor1 < 25 && sensor3 < 25){
    digitalWrite(led, HIGH);
    servo.write(90);
  }else  if(sensor2 < 25){
    digitalWrite(led, HIGH);
    servo.write(90);
  }else if(sensor1 < 25){
    servo.write(180);
    digitalWrite(led, HIGH);
  }else if(sensor3 < 25){
    servo.write(0);
    digitalWrite(led, HIGH);
  }else{
    digitalWrite(led, LOW);
    servo.write(90);
  }
  

}