//Make by Rahul Kurniawan

#include <Servo.h>
#define echoPin 8
#define trigPin 7
int ledYellow = 6;
Servo myservo;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("Kontrol Servo dengan Sensor Jarak");
  myservo.attach(5);
  pinMode(ledYellow, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  long jarak, Tinggi, duration;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  jarak = (duration/2) / 28.5;
  Tinggi = 18.5 - jarak;
  Serial.println(Tinggi);
  if(Tinggi<5){
    digitalWrite(ledYellow, HIGH);
    myservo.write(45);
    delay(1000);
    myservo.write(0);
    delay(1000);
    myservo.write(90);
    delay(1000);
    Serial.println("Servo bergerak ke kiri");
  }else if(Tinggi>5){
    digitalWrite(ledYellow, LOW);
    myservo.write(135);
    delay(1000);
    myservo.write(180);
    delay(1000);
    myservo.write(90);
    delay(1000);
    Serial.println("Servo bergerak ke kanan");
  }
  delay(2000);
}