from machine import Pin, PWM
import time

# Pins
TRIG = Pin(3, Pin.OUT)
ECHO = Pin(2, Pin.IN)
led = Pin(7, Pin.OUT)

# Servo setup
servo1 = PWM(Pin(17))
servo1.freq(50)
servo2 = PWM(Pin(19))
servo2.freq(50)

# Function to set servo angle
def set_angle(servo, angle):
    min_us = 1000
    max_us = 2000
    us = min_us + (angle / 180) * (max_us - min_us)
    duty = int((us / 20000) * 65535)
    servo.duty_u16(duty)

# Ultrasonic distance measurement
def measure_distance():
    TRIG.low()
    time.sleep_us(2)
    TRIG.high()
    time.sleep_us(10)
    TRIG.low()

    timeout = 30000  # 30 ms max

    start = time.ticks_us()
    while ECHO.value() == 0:
        if time.ticks_diff(time.ticks_us(), start) > timeout:
            return -1
        signaloff = time.ticks_us()

    start = time.ticks_us()
    while ECHO.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout:
            return -1
        signalon = time.ticks_us()

    duration = time.ticks_diff(signalon, signaloff)
    distance_cm = (duration * 0.0343) / 2
    return distance_cm

while True:
    distance = measure_distance()  # Get distance
    print("Distance:", distance, "cm")

    if distance == -1:
        led.value(0)
        set_angle(servo1, 0)
        set_angle(servo2, 0)
    elif 90 <= distance <= 100:
        led.value(1)
        set_angle(servo1, 90)
        set_angle(servo2, 0)
    elif 150 <= distance <= 180:
        led.value(1)
        set_angle(servo1, 0)
        set_angle(servo2, 180)
    else:
        led.value(0)
        set_angle(servo1, 0)
        set_angle(servo2, 0)

    time.sleep(1)  # Sleep inside the loop