# RC Servo control (Raspberry Pi Pico / Pico W, MicroPython)
# Signal wire -> GP15
# Power servo from external 5V (or 6V) and CONNECT GROUNDS (servo GND <-> Pico GND)

import machine
import utime

# -----------------------------
# Servo-specific pulse widths (ms)
# Adjust these if your servo's range is different
ZERO_MS = 0.5        # pulse width for ~0°
ONE_EIGHTY_MS = 2.4  # pulse width for ~180°

# Servo period: 50 Hz -> 20 ms
PERIOD_MS = 20.0

# Convert pulse width (ms) to 16-bit duty (0..65535)
DUTY_0 = int(ZERO_MS / PERIOD_MS * 65535)
DUTY_180 = int(ONE_EIGHTY_MS / PERIOD_MS * 65535)

# -----------------------------
# PWM setup
servo_pin = machine.Pin(2)
pwm = machine.PWM(servo_pin)
pwm.freq(50)

def set_servo_angle(angle):
    """
    Set servo angle in degrees (0..180).
    """
    if not (0 <= angle <= 180):
        raise ValueError("Angle must be between 0 and 180 degrees")

    # Linear interpolation between DUTY_0 and DUTY_180
    duty = int(DUTY_0 + (angle / 180.0) * (DUTY_180 - DUTY_0))
    pwm.duty_u16(duty)

# -----------------------------
# Main program
try:
    while True:
        print("Moving servo to 0 degrees")
        set_servo_angle(0)
        utime.sleep(2)

        print("Moving servo to 90 degrees")
        set_servo_angle(90)
        utime.sleep(2)

        print("Moving servo to 180 degrees")
        set_servo_angle(180)
        utime.sleep(2)

except KeyboardInterrupt:
    print("Stopping servo...")
    pwm.deinit()