from machine import Pin, PWM
import time

# Define GPIO pins
SERVO_PIN = 26  # Servo connected to GPIO 26
SWITCH_PIN = 4   # Switch connected to GPIO 4

# Initialize the servo (50Hz PWM)
servo = PWM(Pin(SERVO_PIN), freq=50)

# Initialize the switch (pull-up resistor enabled)
switch = Pin(SWITCH_PIN, Pin.IN, Pin.PULL_UP)

# Function to move the servo to a specific angle
def set_servo_angle(angle):
    # Convert angle (0-180) to duty cycle (calibrated for SG90)
    min_duty = 2000  # ~0 degrees
    max_duty = 8000  # ~180 degrees
    duty = min_duty + (angle / 180) * (max_duty - min_duty)
    servo.duty_u16(int(duty))

try:
    while True:
        if switch.value() == 0:  # If the switch is pressed (active low)
            print("Switch pressed, exiting...")
            break

        set_servo_angle(50)  # Move to 60 degrees
        time.sleep(4)  # Hold for 4 seconds

        if switch.value() == 0:
            break

        set_servo_angle(0)  # Move back to 0 degrees
        time.sleep(3)  # Hold for 3 seconds

except KeyboardInterrupt:
    print("Interrupted!")

finally:
    servo.duty_u16(0)  # Stop the servo
    servo.deinit()  # Deinitialize PWM
    print("Servo stopped.")