from machine import Pin, PWM, I2C
import ssd1306
import time

# Inisialisasi pin-pen servo, sensor ultrasonik, dan buzzer
servo = PWM(Pin(21), freq=50)  # Pin 21 untuk servo
trig = Pin(19, Pin.OUT)
echo = Pin(18, Pin.IN)
buzzer = PWM(Pin(26), freq=1000)  # Pin 26 untuk buzzer

# Inisialisasi OLED
i2c = I2C(scl=Pin(22), sda=Pin(21))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

# Rentang duty cycle yang umum digunakan untuk servo
min_duty = 0  # Duty cycle minimum (servo tertutup)
max_duty = 90  # Duty cycle maksimum (servo terbuka)

# Inisialisasi servo dalam posisi tertutup
servo.duty(min_duty)

servo_open = False  # Status awal servo tertutup
last_distance = 0  # Jarak terakhir

while True:
    # Fungsi untuk mengukur jarak dengan sensor ultrasonik
    def measure_distance():
        trig.on()
        time.sleep_us(10)
        trig.off()
        while echo.value() == 0:
            pulse_start = time.ticks_us()
        while echo.value() == 1:
            pulse_end = time.ticks_us()
        pulse_duration = pulse_end - pulse_start
        distance = (pulse_duration / 2) / 29.1
        return distance

    distance = measure_distance()
    
    if distance < 10:
        if not servo_open:  # Jika servo belum terbuka, buka servo
            print("Membuka servo")
            duty = max_duty
            servo.duty(duty)  # Menggerakkan servo (buka tempat sampah)
            servo_open = True
            buzzer.duty(512)  # Mengaktifkan buzzer
        else:
            buzzer.duty(0)  # Matikan buzzer jika servo sudah terbuka
    elif servo_open and distance > 15:  # Jika servo sudah terbuka dan jarak lebih dari 15 cm, tutup servo
        print("Menutup servo")
        duty = min_duty
        servo.duty(duty)  # Menggerakkan servo (tutup tempat sampah)
        servo_open = False
        buzzer.duty(0)  # Matikan buzzer
    
    # Tampilkan informasi pada OLED
    oled.fill(0)
    oled.text("Jarak: {} cm".format(distance), 0, 0)
    if servo_open:
        oled.text("Status: Terbuka", 0, 20)
    else:
        oled.text("Status: Tertutup", 0, 20)
    oled.show()
    
    time.sleep(1)  # Tahan selama 1 detik sebelum mengukur lagi