from machine import Pin, PWM
from time import sleep, sleep_us, ticks_us, ticks_diff

trig = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

servo = PWM(Pin(15))
servo.freq(50)

merah = Pin(12, Pin.OUT)
hijau = Pin(13, Pin.OUT)

def baca_jarak():
    trig.value(0)
    sleep_us(3)
    trig.value(1)
    sleep_us(10)
    trig.value(0)

    timeout = 30000

    mulai = ticks_us()

    while echo.value() == 0:
        if ticks_diff(ticks_us(), mulai) > timeout:
            return None

    awal = ticks_us()

    while echo.value() == 1:
        if ticks_diff(ticks_us(), awal) > timeout:
            return None

    durasi = ticks_diff(ticks_us(), awal)
    return durasi * 0.0343 / 2

def gerak_servo(sudut):
    duty = int(1638 + (sudut / 180) * 6554)
    servo.duty_u16(duty)

gerak_servo(0)
merah.value(1)
hijau.value(0)

while True:
    jarak = baca_jarak()

    if jarak is not None:
        print("jarak:", round(jarak, 1), "cm")

        if jarak < 15:
            print("Kendaraan terdeteksi - palang terbuka")
            gerak_servo(90)
            merah.value(0)
            hijau.value(1)
        else:
            print("Jalur kosong - palang tertutup")
            gerak_servo(0)
            merah.value(1)
            hijau.value(0)

    sleep(0.2)