from machine import Pin, time_pulse_us
import time

# Define pins
PIN_TRIG = Pin(26, Pin.OUT)
PIN_ECHO = Pin(25, Pin.IN)
LOWLED = Pin(18, Pin.OUT)
MIDLED = Pin(19, Pin.OUT)
HIGHLED = Pin(21, Pin.OUT)
MOTOR = Pin(27, Pin.OUT)

# Initialize the pins
LOWLED.value(1)
MIDLED.value(1)
HIGHLED.value(1)
MOTOR.value(0)

def measure_distance():
    # Trigger the ultrasonic pulse
    PIN_TRIG.value(1)
    time.sleep_us(10)
    PIN_TRIG.value(0)

    # Measure the duration of the echo pulse
    duration = time_pulse_us(PIN_ECHO, 1, 30000)  # Timeout after 30ms

    # Convert to distance in cm
    distance_cm = duration / 58
    return distance_cm

while True:
    level = measure_distance()

    print("Distance in CM:", level)
    print("Distance in inches:", level / 2.54)

    if level < 100:
        LOWLED.value(0)
        MOTOR.value(1)
        HIGHLED.value(1)
        MIDLED.value(1)
    elif 200 < level < 400:
        LOWLED.value(1)
        HIGHLED.value(1)
        MIDLED.value(0)
    elif level >= 400:
        HIGHLED.value(0)
        MIDLED.value(1)
        LOWLED.value(1)
        MOTOR.value(0)

    time.sleep(1)