#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
int trigPin = 7;
int echoPin = 8;
int led1 = 2;
int led2 = 3;
int led3 = 4;

long bacaJarak (int trigPin, int echoPin)
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);
  return pulseIn(echoPin, HIGH);
}

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.begin(16, 2);
  Serial.begin(9600);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
}

void loop() {
  
  int cm = 0;
  cm = (0.03446 * bacaJarak (7, 8)/2);
  lcd.setCursor(0,0);
  lcd.print("Jarak: ");
  lcd.setCursor(6,1);
  lcd.print(cm);
  lcd.setCursor(10, 1);
  lcd.print("cm");
  Serial.println(cm);
  delay(200);

  if (cm > 15)
  {
    digitalWrite(led1, HIGH);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
  }
  else if (cm >= 5 && cm <= 15)
  {
    digitalWrite(led1, LOW);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, LOW);
  }
  else if (cm < 5)
  {
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, HIGH);
  }
}