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

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int trigPin = 7;
const int echoPin = 8;

const int greenLedPin = 9;
const int yellowLedPin = 10;
const int redLedPin = 11;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.begin(9600);

  lcd.begin(16, 2);
  lcd.print("Jarak: ");

  // Inisialisasi pin LED
  pinMode(greenLedPin, OUTPUT);
  pinMode(yellowLedPin, OUTPUT);
  pinMode(redLedPin, OUTPUT);
}

void loop() {
  float distance = measureDistance();

  Serial.print("Jarak: ");
  Serial.print(distance);
  Serial.println(" cm");


  lcd.setCursor(7, 0);
  lcd.print("        "); 
  lcd.setCursor(7, 0);
  lcd.print(distance);
  lcd.print(" cm");

  controlLED(distance);

  delay(500);
}

float measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  float duration = pulseIn(echoPin, HIGH);

  float distance = (duration * 0.0343) / 2;

  return distance;
}

void controlLED(float distance) {
  if (distance > 15) {
    
    digitalWrite(greenLedPin, HIGH);
    digitalWrite(yellowLedPin, LOW);
    digitalWrite(redLedPin, LOW);
  } else if (distance >= 5 && distance <= 15) {
    
    digitalWrite(greenLedPin, LOW);
    digitalWrite(yellowLedPin, HIGH);
    digitalWrite(redLedPin, LOW);
  } else {
    
    digitalWrite(greenLedPin, LOW);
    digitalWrite(yellowLedPin, LOW);
    digitalWrite(redLedPin, HIGH);
  }
}