from machine import Pin, PWM, ADC
import time

# Sensor Ultrasonik
trigger = Pin(3, Pin.OUT)  # GP3 sebagai trigger
echo = Pin(2, Pin.IN)      # GP2 sebagai echo

# Servo
servo1 = PWM(Pin(28))
servo2 = PWM(Pin(27))
servo1.freq(50)
servo2.freq(50)

# LDR (Photoresistor)
ldr = ADC(Pin(26))  # GP26 = ADC0

def read_distance():
    trigger.low()
    time.sleep_us(2)
    trigger.high()
    time.sleep_us(10)
    trigger.low()

    while echo.value() == 0:
        start = time.ticks_us()
    while echo.value() == 1:
        end = time.ticks_us()
    
    duration = time.ticks_diff(end, start)
    distance_cm = duration / 58.0
    return distance_cm

def move_servo(pwm, angle):
    duty = int(1638 + (angle / 180) * 4912)  # 0°=1638, 180°=6550
    pwm.duty_u16(duty)

while True:
    jarak = read_distance()
    nilai_ldr = ldr.read_u16()

    print("Jarak (cm):", jarak, " | Nilai LDR:", nilai_ldr)

    if jarak < 50 and nilai_ldr < 30000:  # disesuaikan untuk kondisi terang
        print("Gerakkan Servo 1")
        move_servo(servo1, 90)
        time.sleep(1)
        move_servo(servo1, 0)
    else:
        print("Gerakkan Servo 2")
        move_servo(servo2, 90)
        time.sleep(1)
        move_servo(servo2, 0)

    time.sleep(0.5)