from machine import Pin, PWM, time_pulse_us
import time

# Pins
trig = Pin(2, Pin.OUT)
echo = Pin(3, Pin.IN)
servo = PWM(Pin(4))

servo.freq(50)

# Servo control (stable values)
def set_angle(angle):
    # 0° ≈ 0.5ms → duty ~ 1638
    # 90° ≈ 1.5ms → duty ~ 4915
    duty = int(1638 + (angle / 180) * 6553)
    servo.duty_u16(duty)

# Distance function (robust)
def get_distance():
    trig.low()
    time.sleep_us(2)

    trig.high()
    time.sleep_us(10)
    trig.low()

    try:
        duration = time_pulse_us(echo, 1, 30000)  # timeout 30ms
        distance = (duration * 0.0343) / 2
        return distance / 100  # meters
    except:
        return 999  # no reading

# Initial state
set_angle(0)

last_detected_time = time.ticks_ms()
lid_open = False

while True:
    dist = get_distance()
    print("Distance:", dist)

    # OBJECT DETECTED
    if dist < 0.5:
        set_angle(90)
        lid_open = True
        last_detected_time = time.ticks_ms()

    # CHECK RESET (non-blocking)
    if lid_open:
        elapsed = time.ticks_diff(time.ticks_ms(), last_detected_time)

        # Close if no object OR after 60 sec
        if dist >= 0.5 or elapsed > 60000:
            set_angle(0)
            lid_open = False

    time.sleep(0.1)