from machine import Pin, time_pulse_us
import utime

# Pin untuk sensor ultrasonik
TRIGGER_PIN = Pin(2, Pin.OUT)
ECHO_PIN = Pin(15, Pin.IN)

# Pin untuk LED
RED_LED_PIN = Pin(14, Pin.OUT)
YELLOW_LED_PIN = Pin(12, Pin.OUT)
GREEN_LED_PIN = Pin(13, Pin.OUT)

# Jarak maksimum parkir mundur (dalam cm)
MAX_DISTANCE = 200

def measure_distance():
    # Mengirimkan sinyal ultrasonik untuk mengukur jarak
    TRIGGER_PIN.on()
    utime.sleep_us(10)
    TRIGGER_PIN.off()
    
    # Menerima sinyal pantulan dan menghitung jarak
    pulse_time = time_pulse_us(ECHO_PIN, True, 100000)
    distance = pulse_time / 58.8 
    
    return distance

def control_led(distance):
    # Mengendalikan LED berdasarkan jarak
    if distance <= 100:
        # Jika jarak mobil dan tembok kurang dari atau sama dengan 1 meter, LED merah menyala
        RED_LED_PIN.on()
        YELLOW_LED_PIN.off()
        GREEN_LED_PIN.off()
    elif distance <= 200:
        # Jika jarak mobil dan tembok kurang dari atau sama dengan 2 meter, LED kuning menyala
        YELLOW_LED_PIN.on()
        RED_LED_PIN.off()
        GREEN_LED_PIN.off()
    elif distance > 200:
        # Jika jarak mobil dan tembok lebih dari 2 meter, LED hijau menyala
        GREEN_LED_PIN.on()
        RED_LED_PIN.off()
        YELLOW_LED_PIN.off()

# Inisialisasi pin
TRIGGER_PIN.off()
RED_LED_PIN.off()
YELLOW_LED_PIN.off()
GREEN_LED_PIN.off()

while True:
    # Mengukur jarak menggunakan sensor ultrasonik
    distance = measure_distance()
    
    # Menampilkan jarak pada Serial Monitor
    print("Jarak:", distance, "cm")
    
    # Mengendalikan LED berdasarkan jarak
    control_led(distance)
    
    # Jeda 1 detik sebelum mengukur jarak lagi
    utime.sleep(1)