#define trigPin 4  // Pin Trig sensor ultrasonik
#define echoPin 2 // Pin Echo sensor ultrasonik
#define lampu1 25   // Pin lampu 1
#define lampu2 26   // Pin lampu 2
#define lampu3 27   // Pin lampu 3

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(lampu1, OUTPUT);
  pinMode(lampu2, OUTPUT);
  pinMode(lampu3, OUTPUT);
  digitalWrite(lampu1, LOW);
  digitalWrite(lampu2, LOW);
  digitalWrite(lampu3, LOW);
  Serial.begin(9600);
}

void loop() {
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;

  if (distance < 40) {
    digitalWrite(lampu1, HIGH);
    digitalWrite(lampu2, LOW);
    digitalWrite(lampu3, LOW);
  } else if (distance < 70) {
    digitalWrite(lampu1, HIGH);
    digitalWrite(lampu2, HIGH);
    digitalWrite(lampu3, LOW);
  } else if (distance >= 70 && distance <= 100) {
    digitalWrite(lampu1, HIGH);
    digitalWrite(lampu2, HIGH);
    digitalWrite(lampu3, HIGH);
  } else {
    digitalWrite(lampu1, LOW);
    digitalWrite(lampu2, LOW);
    digitalWrite(lampu3, LOW);
  }

  delay(500); // Delay untuk mengurangi pembacaan sensor yang berlebihan
}