//Anggota Kelompok 1. Elang Mahendrawan, 2. Fian Adi Wicaksono, 3, Nadhifa Putri Himawan
#include <Servo.h>
#define pinecho 8
#define pintrig 9
#define buzzer 3
#define oren 10
#define biru 11
#define pinky 12
#define yellow 5

Servo servo;
int pos=0;

void setup() {
  Serial.begin(115200);

  pinMode(pinecho, INPUT);
  pinMode(pintrig, OUTPUT);
  pinMode(oren, OUTPUT);
  pinMode(biru, OUTPUT);
  pinMode(pinky, OUTPUT);
  pinMode(yellow, OUTPUT);
  servo.attach(7);
}

void loop() {
  digitalWrite(pintrig, LOW);
  delayMicroseconds(2);
  digitalWrite(pintrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pintrig, LOW);
  int duration=pulseIn(pinecho, HIGH);
  float distance=duration*0.034/2;
  
  if(distance<10)
  {
    digitalWrite(oren, HIGH);
    delay(100);
    tone(buzzer, 500);
    delay(100);
    digitalWrite(oren, LOW);
    delay(100);
    digitalWrite(biru, HIGH);
    delay(100);
    tone(buzzer, 700);
    delay(100);
    digitalWrite(biru, LOW);
    delay(100);
    digitalWrite(pinky, HIGH);
    delay(100);
    tone(buzzer, 900);
    delay(100);
    digitalWrite(pinky, LOW);
    delay(100);
    digitalWrite(yellow, HIGH);
    delay(100);
    tone(buzzer, 1000);
    delay(100);
    digitalWrite(yellow, LOW);
    delay(100);
    servo.write(0);
  delay(300);
  servo.write(90);
  delay(300);
  
  }
  else{
    digitalWrite(oren, HIGH);
    delay(100);
    digitalWrite(biru, HIGH);
    delay(100);
    digitalWrite(pinky, HIGH);
    delay(100);
    digitalWrite(yellow, HIGH);
    delay(100);
    noTone(buzzer);
    delay(100);
     servo.write(0);
  delay(300);
  servo.write(180);
  delay(300);
  }
  
  Serial.print(distance);
  Serial.println("cm");
  delay(500);
}