from machine import Pin
from utime import sleep, sleep_us, ticks_us, ticks_diff

trig = Pin(12, Pin.OUT)
echo = Pin(14, Pin.IN)
r = Pin(27, Pin.OUT)
y = Pin(26, Pin.OUT)
g = Pin(25, Pin.OUT)

def menghitung():
    trig.value(0)
    sleep_us(2)
    
    trig.value(1)
    sleep_us(10)
    trig.value(0)

    while echo.value() == 0:
        timeStart = ticks_us()
    
    while echo.value() == 1:
        timerEnd = ticks_us()

    duration = ticks_diff(timerEnd, timeStart)
    #Rumus menghitung waktu: (duration * kecepatan suara 0.0343) / 2
    distance_cm = (duration * 0.0343) / 2
    return distance_cm

while True:
    jarak = menghitung()
    print("Jarak benda = {:.2f} cm".format(jarak))

    if jarak <= 100:
        r.value(1)
        y.value(0)
        g.value(0)
    elif jarak <= 200:
        r.value(0)
        y.value(1)
        g.value(0)
    else:
        r.value(0)
        y.value(0)
        g.value(1)

    sleep(0.5)