from machine import Pin, PWM
from utime import sleep
from hcsr04 import HCSR04  # Import library HC-SR04

# Definisikan pin untuk servo 1, servo 2, dan servo 3
servo1_pin = 13  # Ganti dengan pin yang sesuai
servo2_pin = 12  # Ganti dengan pin yang sesuai
servo3_pin = 14  # Ganti dengan pin yang sesuai

# Buat objek PWM untuk servo 1, servo 2, dan servo 3
servo1 = PWM(Pin(servo1_pin), freq=50, duty=77)  # Duty cycle untuk posisi tengah servo Anda
servo2 = PWM(Pin(servo2_pin), freq=50, duty=77)
servo3 = PWM(Pin(servo3_pin), freq=50, duty=77)

# Buat objek HC-SR04
sensor = HCSR04(trigger_pin=15, echo_pin=18)  # Ganti dengan pin yang sesuai

# Fungsi untuk menggerakkan servo ke posisi tengah
def reset_servos():
    servo1.duty(77)  # Nilai duty cycle untuk posisi tengah
    servo2.duty(77)
    servo3.duty(77)

# Fungsi untuk menggerakkan servo ke posisi 50%
def rotate_servos():
    servo1.duty(128)
    sleep(1)
    servo1.duty(77)  # Nilai duty cycle sekitar 50%
    sleep(2)  # Tunggu selama 2 detik

    servo2.duty(128)
    sleep(1)
    servo2.duty(77)  # Nilai duty cycle sekitar 50%
    sleep(2)  # Tunggu selama 2 detik

    servo3.duty(128)
    sleep(1)
    servo3.duty(77)  # Nilai duty cycle sekitar 50%
    sleep(2)  # Tunggu selama 2 detik

while True:
    distance = sensor.distance_cm()  # Baca jarak dari sensor HC-SR04 dalam cm
    print("Jarak:", distance, "cm")

    if distance <= 20:  # Jika jarak kurang dari atau sama dengan 5 cm
        rotate_servos()  # Memutar servo 1, kemudian servo 2, kemudian servo 3
    else:
        reset_servos()  # Kembali ke posisi tengah