from machine import Pin
from time import sleep, ticks_us

# Inisialisasi pin GPIO untuk relay
relay1 = Pin(0, Pin.OUT)  # Relay 1 di GP0
relay2 = Pin(1, Pin.OUT)  # Relay 2 di GP1
relay3 = Pin(2, Pin.OUT)  # Relay 3 di GP2
relay4 = Pin(3, Pin.OUT)  # Relay 4 di GP3

# Inisialisasi pin GPIO untuk sensor ultrasonik HC-SR04
trigger = Pin(4, Pin.OUT)  # Trigger di GP4
echo = Pin(5, Pin.IN)      # Echo di GP5

# Fungsi untuk membaca jarak dari HC-SR04
def read_distance():
    # Kirim pulsa trigger
    trigger.value(0)
    sleep(0.002)  # Tunggu sebentar
    trigger.value(1)
    sleep(0.01)
    trigger.value(0)

    # Hitung durasi sinyal echo
    while echo.value() == 0:
        signal_off = ticks_us()
    while echo.value() == 1:
        signal_on = ticks_us()

    # Durasi waktu pulsa
    time_passed = signal_on - signal_off

    # Hitung jarak dalam cm (kecepatan suara = 34300 cm/s)
    distance = (time_passed * 0.0343) / 2
    return distance

# Fungsi untuk mengontrol relay berdasarkan jarak
def control_relay_by_distance(distance):
    # Kondisi jarak yang menentukan relay yang menyala
    if distance < 10:  # Jarak sangat dekat
        relay1.value(1)
        relay2.value(0)
        relay3.value(0)
        relay4.value(0)
    elif 10 <= distance < 20:  # Jarak dekat
        relay1.value(0)
        relay2.value(1)
        relay3.value(0)
        relay4.value(0)
    elif 20 <= distance < 30:  # Jarak sedang
        relay1.value(0)
        relay2.value(0)
        relay3.value(1)
        relay4.value(0)
    elif distance >= 30:  # Jarak jauh
        relay1.value(0)
        relay2.value(0)
        relay3.value(0)
        relay4.value(1)
    else:  # Jika jarak tidak terdeteksi
        relay1.value(0)
        relay2.value(0)
        relay3.value(0)
        relay4.value(0)

# Program utama
while True:
    jarak = read_distance()
    print("Jarak: {:.2f} cm".format(jarak))
    control_relay_by_distance(jarak)
    sleep(0.5)  # Tunggu sebentar sebelum pembacaan berikutnya