#define pinTrig 3
#define pinEcho 2
#include <Servo.h>
Servo servodhimas;

long jarak, waktu;

void setup (){
  Serial.begin(9600);
  servodhimas.attach(11);
  pinMode(1, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(pinTrig, OUTPUT);
  pinMode(pinEcho, INPUT);
}
float ultrasonik(){
  digitalWrite(pinTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(pinTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrig, LOW);

  waktu = pulseIn(pinEcho, HIGH);
  jarak = waktu * 0.034 / 2;
}

void loop(){
  digitalWrite(pinTrig,LOW);

  delayMicroseconds(2);

  digitalWrite(pinTrig,HIGH);

  delayMicroseconds(10);

  digitalWrite(pinTrig,LOW);


  waktu=pulseIn (pinEcho,HIGH);

  jarak=0.034/2*waktu;

  if (jarak>5){

  servodhimas.write(90);

  delay(1000);

  }

  if (jarak<5){

  servodhimas.write(0);

  delay(1000);

aman();
hatihati();
bahaya();
}
void aman(){
  float jarak = ultrasonik();
  if (jarak >50){
    digitalWrite(6, HIGH);
    delay(500);
    digitalWrite(6, LOW);
    delay(500);
  }
}
void hatihati(){
  float jarak = ultrasonik();
  if (jarak > 50 && jarak >100){
  digitalWrite(5, HIGH);
  delay(250);
  digitalWrite(5, LOW);
  delay(250);
}
}
void bahaya(){
  float jarak = ultrasonik();
  if (jarak < 25){
  digitalWrite(1, HIGH);
  delay(50);
  digitalWrite(1, LOW);
  delay(50);
}
}