#include <HCSR04.h>

const byte echoPin = 2;
const byte triggerPin = 3;
const byte Led_Merah = 6;
const byte Led_Kuning = 7;
const byte Led_Hijau = 8;

void setup() {
  pinMode(Led_Merah,1);
  pinMode(Led_Kuning,1);
  pinMode(Led_Hijau,1);
  Serial1.begin(115200);
  Serial1.println("TEST ULTRASONIC With RASPBERRY Pi PICO!");
  HCSR04.begin(triggerPin, echoPin);
}

void loop() {
  double* distances = HCSR04.measureDistanceCm();
  Serial1.print(distances[0]); Serial1.println(" cm");

  if(distances[0] <= 100){
    digitalWrite(Led_Merah,1);
    digitalWrite(Led_Kuning,0);
    digitalWrite(Led_Hijau,0);
  }
  else if((distances[0] > 100) && (distances[0] < 200)){
    digitalWrite(Led_Merah,0);
    digitalWrite(Led_Kuning,1);
    digitalWrite(Led_Hijau,0);
  }
  else if(distances[0] >= 200){
    digitalWrite(Led_Merah,0);
    digitalWrite(Led_Kuning,0);
    digitalWrite(Led_Hijau,1);
  }
}