from machine import Pin
from time import sleep_us, sleep, ticks_us, ticks_diff
 
# Define the velocity of sound
soundVelocity=340
 
# Define led, Trigger and echo pins
triggerPin = Pin(14,Pin.OUT)
echoPin = Pin(17,Pin.IN)
red_led = Pin(21, Pin.OUT)
yellow_led = Pin(22, Pin.OUT)
green_led = Pin(23, Pin.OUT)

print("Start Measuring distance")
# Start an endless loop
while True:
# Set the Trigger pin to LOW
    triggerPin.off()
    sleep_us(5)
# Make a 10 microSeconds HIGH pulse
    triggerPin.on()
    sleep_us(10)
    triggerPin.off()
# Wait for the echo pin to get to ON status
    while (echoPin.value() == 0):
        pass
# Start measuring the time
    startTime = ticks_us()
# Wait for the echo pin to get to OFF status
    while (echoPin.value() == 1):
        pass
# Stop measuring the time
    stopTime = ticks_us()
# Calculate the time for the echo to arrive
    echoTime = ticks_diff(stopTime,startTime)
# Calculate the distance 
    distance = echoTime*soundVelocity/2/10000
# Print the distance
    print("Distance: %5.1f" % (distance),"cm")

# if distance less then 10cm, turn on red. Turn off others
    if distance < 10 :
        red_led.on()
        yellow_led.off()
        green_led.off()

# if distance between 10cm to 20cm, turn on yellow. Turn off others
    if distance >= 10 and distance <20 :
        red_led.off()
        yellow_led.on()
        green_led.off()

# if distance more then 20cm, turn on green. Turn off others
    if distance >= 20 :
        red_led.off()
        yellow_led.off()
        green_led.on()
'''
# if distance less then 10cm, turn on red. Turn off others
    if distance < 10 :
        red_led.on()
        yellow_led.off()
        green_led.off()

# if distance between 10cm to 20cm, turn on yellow. Turn off others
    elif distance <20:
        red_led.off()
        yellow_led.on()
        green_led.off()

# if distance more then 20cm, turn on green. Turn off others
    else:
        red_led.off()
        yellow_led.off()
        green_led.on()
'''                
    sleep(0.2)