from machine import Pin, PWM
import time

# Setup PWM on GP0
servo = PWM(Pin(15))
servo.freq(50)

# Function to convert angle → duty
def set_angle(angle):
    min_duty = 1638
    max_duty = 8192
    duty = int(min_duty + (angle / 180) * (max_duty - min_duty))
    servo.duty_u16(duty)

# Define initial position
INITIAL_ANGLE = 0

# Move to initial position at start
set_angle(INITIAL_ANGLE)
time.sleep(1)

# Main loop
while True:
    try:
        user_input = input("Enter angle (0-180): ")
        angle = int(user_input)

        if 0 <= angle <= 180:
            # Step 1: Reset to initial position
            set_angle(INITIAL_ANGLE)
            time.sleep(1)   # give time to reach initial position
            
            # Step 2: Move to desired angle
            set_angle(angle)
            print("Moved from", INITIAL_ANGLE, "to", angle)

        else:
            print("Enter value between 0 and 180")

    except:
        print("Invalid input!")