print("Program: To detect motion")
print("Created by: SAN OP")
print("Date:20/11/2024")


# Import Libraries
from machine import Pin, PWM, SoftI2C
from utime import sleep
import ultrasonic_lib  
import Oled_lib

# Pin Declaration
oled_Pin = SoftI2C(scl=Pin(22), sda=Pin(21))  
TRIG = 26  
ECHO = 25  
servo_pin = PWM(Pin(4))  
blue_led_pin = Pin(5, Pin.OUT)  
button_pin = Pin(12, Pin.IN, Pin.PULL_UP)  

# Servo motor setup
servo_pin.freq(50)  # Frekuensi PWM untuk servo motor

# Object Declaration
detect_object_sensor = ultrasonic_lib.HCSR04(trigger_pin=TRIG, echo_pin=ECHO)
oled = Oled_lib.SSD1306_I2C(width=128, height=64, i2c=oled_Pin)

def set_servo_angle(pin, angle):
    duty = int(((angle / 180) * 2000) + 500)
    print(f"Setting servo to angle {angle} (duty: {duty})")  
    pin.duty_u16(int(duty / 20000 * 65535)) 

# Main Program
while True:
    if button_pin.value() == 0:
        print("BUTTON IS PRESSED")
        oled.fill(0)
        oled.text("WASH HAND", 30, 30)
        oled.show()

        set_servo_angle(servo_pin, 90)  
        sleep(5)  
        set_servo_angle(servo_pin, 0)  

        

    distance = detect_object_sensor.distance_cm()  
    print(f"Jarak: {distance} cm")
    print("==================================================\n")

    if distance <= 10:
        print("TANGAN DIKESAN")
        oled.fill(0)
        oled.text("WASH HAND", 30, 30)
        oled.show()

        set_servo_angle(servo_pin, 90)  

    elif 10 < distance <= 20:
        print("Jarak > 10 cm dan <= 20 cm. LED biru berkelip.")
        set_servo_angle(servo_pin, 0)  
        oled.fill(0)
        oled.text("PUT HANDS", 10, 30)
        oled.text("CLOSER", 10, 40)
        oled.show()

        blue_led_pin.on()
        sleep(0.5)
        blue_led_pin.off()
        sleep(0.5)

    else:
        print("Jarak > 20 cm. Tidak ada tangan dikesan.")
        blue_led_pin.off()
        set_servo_angle(servo_pin, 0)  

        oled.fill(0)
        oled.text("NO HANDS", 30, 30)
        oled.text("DETECTED", 30, 40)
        oled.show()

    sleep(0.1)