#define TRIG_PIN 18 // pin TRIG Sensor Ultrasonik terhubung ke ESP32 pin 18
#define ECHO_PIN 19 // pin ECHO Sensor Ultrasonik terhubung ke ESP32 pin 5
#define LED_HIJAU 21 // pin LED hijau terhubung ke ESP32 pin 21
#define LED_KUNING 22 // pin LED kuning terhubung ke ESP32 pin 22
#define LED_MERAH 23 // pin LED2 merah terhubung ke ESP32 pin 23
#define LED 2 // pin internal LED terhubung ke ESP32 pin 2
float duration_us, distance_cm; //Deklarasi variabel untuk menyimpan durasi dan jarak
float tinggi;

void setup() {
  // Mulai komunikasi serial dengan baudrate 9600bps
  Serial.begin (9600);

  // Konfigurasi pin TRIG sebagai output
  pinMode(TRIG_PIN, OUTPUT);
  // Konfigurasi pin ECHO sebagai input
  pinMode(ECHO_PIN, INPUT);
  // Konfigurasi pin LED hijau sebagai output
  pinMode(LED_HIJAU, OUTPUT);
  // Konfigurasi pin LED kuning sebagai output
  pinMode(LED_KUNING, OUTPUT);
  // Konfigurasi pin LED merah sebagai output
  pinMode(LED_MERAH, OUTPUT);
  // Konfigurasi pin LED internal sebagai output
  pinMode(LED, OUTPUT);
}

void loop() {
  // Mengirimkan pulsa 10 mikrodetik ke pin TRIG
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
 
  // Mengukur durasi pulsa dari pin ECHO
  duration_us = pulseIn(ECHO_PIN, HIGH);
  
  // Menghitung jarak dalam centimeter
  distance_cm = 0.017 * duration_us;
  if (distance_cm >= 200){
    distance_cm = 0; //lock alat pengukur di ketinggian 2 meter 
  }
  tinggi = 200-distance_cm;
 
  // Menyalakan LED jika jarak dalam batas tertentu
  if (tinggi < 170) {
    digitalWrite(LED_MERAH, HIGH);
    digitalWrite(LED_KUNING, LOW);
    digitalWrite(LED_HIJAU, LOW);
  }
  else if (tinggi < 180) {
    digitalWrite(LED_MERAH, LOW);
    digitalWrite(LED_KUNING, HIGH);
    digitalWrite(LED_HIJAU, LOW);
  }
  else{
    digitalWrite(LED_MERAH, LOW);
    digitalWrite(LED_KUNING, LOW);
    digitalWrite(LED_HIJAU, HIGH);
  }
 
  // Menampilkan nilai jarak ke Serial Monitor
  Serial.print("jarak object : ");
  Serial.print(distance_cm);
  Serial.println(" cm      ");

  Serial.print("tinggi badan : ");
  Serial.print(tinggi);
  Serial.println(" cm      ");

  Blink(LED);
  delay(500);
}

// Fungsi untuk mengedipkan LED
void Blink(int x){
  digitalWrite(x, HIGH);
  delay(10);
  digitalWrite(x, LOW);
}