from machine import Pin, PWM
import time

# Setup button
button = Pin(21, Pin.IN, Pin.PULL_UP)

# Setup LEDs
led_red = Pin(3, Pin.OUT)
led_green = Pin(4, Pin.OUT)

# Setup ultrasonic sensor
trigger = Pin(5, Pin.OUT)
echo = Pin(2, Pin.IN)

# Setup servo motor
servo = PWM(Pin(12))
servo.freq(50)

# Function to move servo
def move_servo(angle):
    duty = int((angle / 180 * 2 + 0.5) / 20 * 65535)
    servo.duty_u16(duty)

# Function to measure distance
def get_distance():
    # Trigger the ultrasonic pulse
    trigger.low()
    time.sleep_us(2)
    trigger.high()
    time.sleep_us(10)
    trigger.low()
    
    # Measure the duration of the echo pulse
    while echo.value() == 0:
        start_time = time.ticks_us()
    while echo.value() == 1:
        end_time = time.ticks_us()
    
    # Calculate distance in cm
    duration = time.ticks_diff(end_time, start_time)
    distance = (duration * 0.0343) / 2
    return distance

# Main program
def main():
    print("Press the button to start the program.")
    
    # Wait for button press
    while button.value() == 1:
        time.sleep(0.1)
    
    print("Program started!")
    
    while True:
        distance = get_distance()
        print(f"Distance: {distance} cm")
        
        if distance > 15:  # Adjust the threshold as needed
            move_servo(90)  # Rotate servo to 90 degrees
            led_green.off()
            led_red.on()
            print("Garage door closed")
        else:
            move_servo(0)  # Rotate servo to 0 degrees
            led_red.off()
            led_green.on()
            print("Garage door open")
        
        time.sleep(0.5)  # Delay for stability

# Run the main program
main()