#include <Servo.h>
#define echo 3
#define trig 2
#define buzz 13
#define l1 12
#define l2 11

int pos = 0;
Servo servo;  

float distance() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  int duration = pulseIn(echo, HIGH);
  return duration * 0.034 / 2;
}

void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(buzz, OUTPUT);
  pinMode(l1, OUTPUT);
  pinMode(l2, OUTPUT);
  servo.attach(9);  
}

void loop() {
  float jarak = distance();
  Serial.print("Jarak: ");
  Serial.println(jarak,0);
  servo.write(pos);

  if(jarak <= 100){
    if(pos == 0){
      for (pos = 0; pos <= 90; pos+=5) { 
      servo.write(pos); 
      tone(buzz, 1000);
      digitalWrite(l1, HIGH);
      digitalWrite(l2, LOW);
      delay(500);     
      tone(buzz, 500);
      digitalWrite(l1, LOW);
      digitalWrite(l2, HIGH);
      delay(500);
      }            
    }
    pos = 90;
    tone(buzz, 1000);
    digitalWrite(l1, HIGH);
    digitalWrite(l2, LOW);
    delay(500);
    tone(buzz, 500);
    digitalWrite(l1, LOW);
    digitalWrite(l2, HIGH);
    delay(500);
  }
  else
  
  if(jarak > 100){
    if(pos == 90){
      for (pos = 90; pos >= 0; pos-=5) { 
      servo.write(pos); 
      tone(buzz, 1000);
      digitalWrite(l1, HIGH);
      digitalWrite(l2, LOW);
      delay(500);     
      tone(buzz, 500);
      digitalWrite(l1, LOW);
      digitalWrite(l2, HIGH);
      delay(500);
      }            
    }
    pos = 0;
    noTone(buzz);
    digitalWrite(l1, LOW);
    digitalWrite(l2, LOW);
  }

}