2025 AI训练营——舵机驱动

tobot

# 舵机控制参数
SERVO_PIN = 0         # 舵机控制引脚（PIO0_0）
PWM_FREQUENCY = 50   # 50Hz (周期20ms)
MIN_PULSE = 1000      # 最小脉冲宽度1ms (0度)
MAX_PULSE = 2000     # 最大脉冲宽度2ms (180度)
SYSTEM_CLOCK = 12000000  # 12MHz系统时钟

# 模拟硬件寄存器
class LPC_SYSCON:
    SYSAHBCLKCTRL = 0

class LPC_IOCON:
    PIO0_0 = 0

class LPC_PWM1:
    CTCR = 0
    PR = 0
    MR0 = 0
    PCR = 0
    TCR = 0
    MR1 = 0
    LER = 0

# PWM匹配寄存器配置
pwm_mr0 = 0

def servo_init():
    global pwm_mr0
    # 使能PWM时钟（SYSAHBCLKCTRL第8位）
    LPC_SYSCON.SYSAHBCLKCTRL |= (1 << 8)

    # 配置引脚为PWM功能（清除功能选择位）
    LPC_IOCON.PIO0_0 &= ~0x07

    # 配置PWM定时器
    LPC_PWM1.CTCR = 0x00  # 定时器模式
    LPC_PWM1.PR = 0        # 预分频=0

    # 计算匹配值（周期=20ms）
    pwm_mr0 = SYSTEM_CLOCK // PWM_FREQUENCY
    LPC_PWM1.MR0 = pwm_mr0

    # 使能PWM输出
    LPC_PWM1.PCR |= (1 << SERVO_PIN)

    # 启动PWM定时器
    LPC_PWM1.TCR = (1 << 0)

def servo_set_angle(angle):
    # 限制角度范围（0-180度）
    angle = max(0, min(angle, 180))

    # 计算脉冲宽度（线性映射）
    pulse = MIN_PULSE + int((MAX_PULSE - MIN_PULSE) * (angle / 180.0))

    # 计算匹配值（占空比）
    duty = (pwm_mr0 * pulse) // 20000  # 20ms=20000us

    # 更新PWM占空比
    LPC_PWM1.MR1 = duty
    LPC_PWM1.LER |= (1 << 1)  # 更新匹配寄存器

# 初始化并测试舵机
servo_init()
servo_set_angle(0)    # 测试0度
servo_set_angle(90)   # 测试90度
servo_set_angle(180)  # 测试180度

阅读全文