#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

long bacaJarak(int TRIG_PIN, int ECHO_PIN)
{
  pinMode(TRIG_PIN, OUTPUT);
  digitalWrite(TRIG_PIN, LOW);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  pinMode(ECHO_PIN, INPUT);
  return pulseIn(ECHO_PIN, HIGH);
}

void setup() {
  lcd.begin(16, 2);
  lcd.backlight();
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
}

void loop() {
  int cm = 0;
  cm = (0.03446 * bacaJarak(7, 8)/2);
  delay(100);
  lcd.setCursor(0, 0);
  lcd.print("Jarak Objek : ");
  lcd.setCursor(0, 1);
  lcd.print(cm);
  lcd.setCursor(4, 1);
  lcd.print("cm");
  if (cm > 15)
  {
    digitalWrite(6, HIGH);
    digitalWrite(5, LOW);
    digitalWrite(3, LOW);
  }
  else if (cm >= 5 && cm <= 15)
  {
    digitalWrite(6, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(3, LOW);
  }
  if (cm > 5)
  {
    digitalWrite(6, LOW);
    digitalWrite(5, LOW);
    digitalWrite(3, HIGH);
  }
  delay(100);
}