from machine import Pin, time_pulse_us
import time

# Ultrasonic pins
trig = Pin(5, Pin.OUT)
echo = Pin(18, Pin.IN)

# LED pins
green_led = Pin(33, Pin.OUT)
yellow_led = Pin(27, Pin.OUT)
red_led = Pin(26, Pin.OUT)

def measure_distance():
    # Send trigger pulse
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    # Measure echo pulse duration
    try:
        duration = time_pulse_us(echo, 1, 30000)  # 30 ms timeout
    except Exception:
        return None

    if duration < 0:
        return None

    distance = (duration * 0.0343) / 2  # convert to cm
    return distance

def set_leds(green, yellow, red):
    green_led.value(green)
    yellow_led.value(yellow)
    red_led.value(red)

while True:
    dist = measure_distance()
    if dist:
        print("Distance:", dist, "cm")
        if dist > 50:
            set_leds(1, 0, 0)  # Green ON
        elif 20 < dist <= 50:
            set_leds(0, 1, 0)  # Yellow ON
        elif dist <= 20:
            set_leds(0, 0, 1)  # Red ON
    else:
        print("No object detected.")
        set_leds(0, 0, 0)

    time.sleep(0.5)