import machine
import utime

# According to your schematic, the servo is on Pin D6 (P0)
SERVO_PIN = 0  # D6 on XIAO RP2040 corresponds to GPIO 0

# Setup PWM on the servo pin
servo = machine.PWM(machine.Pin(SERVO_PIN))
servo.freq(50)  # Standard servos operate at 50Hz

def set_angle(angle):
    """
    Convert angle (0-180) to duty cycle for RP2040.
    Duty range is 0 to 65535.
    0.5ms (0 deg)   -> approx 1638 duty (50Hz)
    2.5ms (180 deg) -> approx 8192 duty (50Hz)
    """
    # Map 0-180 degrees to 1638-8192 duty cycle
    duty = int((angle / 180) * (8192 - 1638) + 1638)
    servo.duty_u16(duty)

print("Starting Servo Test...")

try:
    while True:
        print("Moving to 0 degrees")
        set_angle(0)
        utime.sleep(1)
        
        print("Moving to 90 degrees")
        set_angle(90)
        utime.sleep(1)
        
        print("Moving to 180 degrees")
        set_angle(180)
        utime.sleep(1)

except KeyboardInterrupt:
    # Disable PWM on exit
    servo.deinit()
    print("Test stopped.")