#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

long bacaJarak (int triggerPin, int echoPin){
  pinMode(triggerPin, OUTPUT);
  digitalWrite(triggerPin, LOW);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(19);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  return pulseIn(echoPin, HIGH);
}

  int red = 29;
  int yellow = 31;
  int green = 33;

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

  lcd.init();
  lcd.backlight();

  pinMode(red, OUTPUT);
  pinMode(yellow, OUTPUT);
  pinMode(green, OUTPUT);
}

void loop() {
  int cm = 0;
  cm = (0.03446 * bacaJarak(7, 8))/2;
  int mm = cm*100;

  Serial.print(cm);
  Serial.println("cm");
  delay(100);

  // lcd.setCursor(0,0);
  // lcd.print("Jarak: ");
  // lcd.setCursor(6,0);
  // lcd.print(mm);
  // lcd.print("mm");
  // delay(1000);

  if (cm > 0 && cm<10){
    lcd.setCursor(0,0);
    lcd.print("Hijau menyala.");
    digitalWrite(green, 1);
    delay(100);
    lcd.clear();
  } else if (cm>11 && cm <20){
    lcd.setCursor(0,0);
    lcd.print("Kuning menyala.");
    digitalWrite(yellow, 1);
    delay(100);
    lcd.clear();
  } else if (cm>21 && cm<30){
    lcd.setCursor(0,0);
    lcd.print("Merah nyala.");
    digitalWrite(red, 1);
    delay(100);
    lcd.clear();
  } else if ( cm > 31) {

    lcd.setCursor(0,0);
    lcd.print("Lampu berkedip.");

    digitalWrite(red, 1);
    digitalWrite(yellow, 1);
    digitalWrite(green, 1);

    delay(1000);

    digitalWrite(red, 0);
    digitalWrite(yellow, 0);
    digitalWrite(green, 0);

    lcd.clear();
  }
}