from machine import Pin, PWM
import time 

# Konfirmasi pin untuk sensor ultrasonik, LED, dan servo
trig = Pin(15, Pin.OUT)
echo = Pin(14, Pin.IN)
led_hijau = Pin(12, Pin.OUT)
led_merah = Pin(11, Pin.OUT)

# Konfirmasi servo menggunakan PWM
servo = PWM(Pin(13))
servo.freq(50) # Frekuensi PWM untuk servo 50Hz

# Fungsi untuk menggerakan servo ke sudut tertentu 
def set_servo_angle(angle):
    duty = int(5000 + (angle / 180) * 5000)
    servo.duty_u16(duty)

# Fungsi untuk mengukur jarak menggunakan HC-SR04
def get_distance():
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()

    while echo.value() == 0:
        start = time.ticks_us()
    while echo.value() == 1:
        end = time.ticks_us()

    distance = (time.ticks_diff(end, start)) * 0.0343 / 2
    return distance

# Setup awal: Tutup portal dan nyalakan LED merah
set_servo_angle(0)  # 90 derajat = posisi tertutup
led_merah.on()
led_hijau.off()

# Loop utama
while True:
    jarak = get_distance()
    print("Jarak:" , jarak, "cm")

    if jarak < 60: # Jika jarak kurang dari 60 cm, buka portal
        set_servo_angle(0) # buka portal (0 derajat)
        led_hijau.on()
        led_merah.off()
        time.sleep(5) # Tunggu 5 detik sebelum menutup kembali 

        # Tutup portal dan nyalakan LED merah kembali
        set_servo_angle(90)
        led_hijau.off()
        led_merah.on()

    time.sleep(0.5) # Jeda setengah detik sebelum pembacaan berikutnya