from machine import Pin, PWM
from time import sleep_ms

# ---- CONFIG ----
servo_pin = 15          # GPIO connected to servo signal wire
servo = PWM(Pin(servo_pin))
servo.freq(50)          # 50 Hz standard servo frequency

# ---- FUNCTION ----
def set_angle(angle):
    min_us = 500         # 0°  ≈ 0.5 ms
    max_us = 2500        # 180° ≈ 2.5 ms
    period_us = 20000    # 20 ms period for 50 Hz

    # Convert angle to microseconds pulse width
    us = min_us + (max_us - min_us) * angle / 180

    # Convert microseconds to 16-bit duty value
    duty = int(us / period_us * 65535)
    servo.duty_u16(duty)

# ---- TEST ----
while True:
    for angle in range(0, 181, 1):
        print("Angle:", angle)
        set_angle(angle)
        sleep_ms(30)
    for angle in range(180, -1, -1):
        print("Angle:", angle)
        set_angle(angle)
        sleep_ms(30)