from machine import Pin,PWM
import time

# pwm输出引脚
pwm = PWM(Pin(14), freq=50)

def set_angle(servo, angle, min_pulse=0.5, max_pulse=2.5):
    # 1. 限制角度范围，保护舵机硬件
    angle = max(0, min(180, angle))
    print("angle:", angle)
    
    # 2. 计算目标角度对应的脉冲宽度 (ms)
    # 0度时为 min_pulse, 180度时为 max_pulse
    pulse_width = min_pulse + (angle / 180.0) * (max_pulse - min_pulse)
    print("pulse width:", pulse_width)
    
    # 3. 计算 10 位占空比数值 (0-1023)
    # 公式：(目标脉宽 / 周期20ms) * 1023
    # 使用浮点数运算确保精度，最后使用 int() 转换
    duty = int((pulse_width / 20.0) * 1023)
    print("duty: ", duty)
    
    # 4. 输出 PWM 信号
    servo.duty(duty)

def set_angle_fine(servo, angle, min_pulse=0.5, max_pulse=2.5):
    # 限制角度在 0-180 之间，防止过冲损坏舵机
    angle = max(0, min(180, angle))
    print("angle:", angle)
    # 计算对应脉宽 (ms)
    pulse_width = min_pulse + (angle / 180) * (max_pulse - min_pulse)
    print("pulse width:", pulse_width)
    # 计算 16 位占空比
    duty = int((pulse_width / 20) * 65535)
    print("duty: ", duty)
    servo.duty_u16(duty)

while True:
    set_angle(pwm, 0)
    time.sleep(1) 
    set_angle(pwm, 45)
    time.sleep(1)
    set_angle(pwm, 90)
    time.sleep(1)
    set_angle(pwm, 135)
    time.sleep(1)
    set_angle(pwm, 180)
    time.sleep(1)
    
    set_angle_fine(pwm,0)
    time.sleep(1)
    set_angle_fine(pwm,45)
    time.sleep(1)
    set_angle_fine(pwm,90)
    time.sleep(1)
    set_angle_fine(pwm,135)
    time.sleep(1)
    # 这个 180 度在 16 位 pwm 无法达到
    set_angle_fine(pwm, 179)
    time.sleep(1)