from machine import Pin, PWM, UART
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

uart = UART(0, baudrate=115200)   # UART0 → USB serial

print("Servo motor test")

# ---- 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:
    if uart.any():
        data = uart.readline()
        angle = int(data)
        print("Angle:", angle)
        set_angle(angle)