#include <LiquidCrystal_I2C.h>
#include <Servo.h>
LiquidCrystal_I2C lcd(0x27,16,2); //Memanggil LCD
int trigPin = 11; //pin trigger
int echoPin = 10; //pin echo
long waktu; //Menghitung Waktu
int jarak; //Menghitung Jarak
Servo myservo1;
Servo myservo2;
int buzz = 8;
int relay1 = 7;
int relay2 = 6;
void setup()
{
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);
  myservo1.attach(9);
  myservo1.write(90);
   myservo2.attach(5);
  myservo2.write(90);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("FULL ARMOR");
  lcd.setCursor(0,1);
  lcd.print("KEL 10 P5"); 
  delay(3000);  
}
void loop(){
lcd.clear();
digitalWrite(trigPin, LOW); //Mematikan trigger
delayMicroseconds(2);
digitalWrite(trigPin, HIGH); //Menyalakan trigger
delayMicroseconds(10);
digitalWrite(trigPin, LOW); //Mematikan trigger
waktu = pulseIn(echoPin,HIGH); //membaca waktu yang diterima echopin
jarak = waktu * 0.034/2; //Menghitung jarak
lcd.setCursor(0,0);
lcd.print("JARAK OBYEK");
lcd.setCursor(0,1);
lcd.print(jarak);
lcd.setCursor(4,1);
lcd.print(" cm");
digitalWrite(buzz, HIGH);
tone(buzz, 10000, 10000000);
delay(1000);

if(jarak <100) {
myservo1.write(30) ;
myservo2.write(150) ;
 digitalWrite(buzz, HIGH);
 digitalWrite(relay1, HIGH);
 digitalWrite(relay2, LOW);
 tone(buzz, 500, 10);
}

else if( 100<= jarak && jarak <200 ){
myservo1.write(60) ;
myservo2.write(120) ;
 digitalWrite(buzz, HIGH);
 digitalWrite(relay1, LOW);
 digitalWrite(relay2, HIGH);
 tone(buzz, 900, 100);
}

else if(200 <= jarak && jarak <300){
myservo1.write(120) ;
myservo2.write(60) ;
 digitalWrite(buzz, HIGH);
 digitalWrite(relay1, HIGH);
 digitalWrite(relay2, HIGH);
 tone(buzz, 1200, 1000);
}

else{
myservo1.write(180) ;
myservo2.write(0) ;
 digitalWrite(buzz, HIGH);
 digitalWrite(relay1, LOW);
 digitalWrite(relay2, LOW);
 tone(buzz, 1400, 2000);
}

}