12 - 舵机控制应用
本章节介绍 Pico-G1 扩展板上的舵机控制应用示例 —— servo_ctrl。该应用演示了如何使用 PWM 接口实现舵机的精确角度控制，支持角度设定、扫描运行、位置归中等功能，并在 TFT 屏幕上显示舵机状态。这是学习 PWM 控制和伺服电机编程的实用示例。
应用源码位于 SDK 目录 source/app/12_servo_ctrl/，提供了完整的 PWM 舵机控制实现。
1 应用概述
1.1 功能特性
- PWM 舵机控制：通过 PWM 接口实现舵机角度精确控制
- 角度范围：支持 0°~180° 标准舵机角度范围
- 高精度定位：角度控制精度 ±1°
- 多种运行模式：支持角度设定、扫描运行、位置归中等模式
- 实时状态显示：在 TFT 屏幕上显示角度、脉宽和运行状态
1.2 技术参数
| 参数 | 值 |
|---|---|
舵机类型 | 标准 9g 微型舵机（兼容 MG996R） |
控制信号 | PWM（50Hz 周期） |
脉宽范围 | 0.5ms~2.5ms（对应 0°~180°） |
工作电压 | 4.8V~6V（推荐 5V） |
控制精度 | ±1° |
刷新频率 | 50Hz（20ms 周期） |
控制接口 | GPIO PWM 输出 |
1.3 测试用例列表
| index | 名称 | 测试指令 | 预期现象（成功） | 失败可能原因 |
|---|---|---|---|---|
| 1 | 角度控制 | ./servo_ctrl | 舵机转动到指定角度，TFT显示角度值 | PWM配置错误、供电不足 |
| 2 | 精度测试 | 设置90度角度 | 舵机精确定位在中间位置 | 脉宽计算错误、机械抖动 |
| 3 | 扫描测试 | 启动扫描模式 | 舵机在0~180度间往复运动 | PWM频率不准确 |
| 4 | 负载测试 | 增加机械负载 | 舵机保持稳定，角度不变 | 供电不足、扭矩不够 |
1.4 目录结构
source/app/12_servo_ctrl/
├── Makefile # 构建脚本
├── main.c # 主程序
├── servo.c # 舵机驱动实现
├── servo.h # 舵机驱动头文件
├── pwm_hal.c # PWM HAL 层实现
├── pwm_hal.h # PWM HAL 层头文件
├── spi_hal.c # SPI HAL 层实现
├── spi_hal.h # SPI HAL 层头文件
├── st7789.c # ST7789 驱动实现
├── st7789.h # ST7789 驱动头文件
├── font8x16.h # 8×16 ASCII 点阵字库
└── README.md # 说明文档2 硬件连接说明
2.1 引脚定义
| 信号 | 板上 GPIO | 说明 |
|---|---|---|
| PWM | GPIO6_0 | PWM 控制信号输出 |
| VCC | 5V | 舵机供电（4.8V~6V） |
| GND | GND | 地线 |
2.2 硬件电路
舵机接线图：
Pico-G1 Servo Motor
┌───────────┐ ┌──────────────┐
│ │ │ │
│ GPIO6_0 ──┼────── PWM ───┤ ORANGE │
│ │ │ │
│ 5V ────┼─────────────┤ RED │
│ │ │ │
│ GND ───┼─────────────┤ BROWN │
└───────────┘ └──────────────┘舵机供电
舵机需要较大电流，建议使用外部 5V 电源供电，避免从板端取电导致电压不稳定。
2.3 PWM 控制原理
舵机角度与脉宽关系：
0° → 0.5ms 高电平
90° → 1.5ms 高电平（中位）
180° → 2.5ms 高电平
PWM 周期：20ms（50Hz）
占空比 = (脉宽 / 20ms) × 100%3 编译与部署
3.1 编译应用
export PATH=$PATH:<SDK>/tools/linux/toolchains/arm-gcc12.2.0-linux-uclibceabi/bin
cd <SDK>/source/app/12_servo_ctrl
make3.2 运行应用
scp servo_ctrl root@<板端IP>:/usr/bin/
ssh root@<板端IP} '/usr/bin/servo_ctrl'3.3 预期输出
控制台输出
/mnt # ./servo_ctrl
[pca] pad 复用：I2C3(4_1/4_2)->func2，GPIO4_5(OE)->func5
[pca] pad 0x100C0010 -> 0x00001002
[pca] pad 0x100C0014 -> 0x00001002
[pca] pad 0x100C0020 -> 0x00001005
[pca] init PCA9685 (PWM 50Hz)...
[pca] MODE1=0x00 (reset OK)
[pca] @ /dev/i2c-3 addr 0x41 init OK, PWM 50 Hz
[pca] OE 已使能，开始接受命令
命令:
s <ch 0..15> <us> a <us> c o q h
脉宽: 500us(0°) / 1500us(90°) / 2500us(180°)
> ch 0 1500
[pca] 全部居中 (1500 us)4 舵机控制原理
4.1 PWM 时序
标准舵机 PWM 时序：
- 周期：20ms（50Hz）
- 脉宽范围：0.5ms ~ 2.5ms
- 更新频率：建议 50Hz，最高不超过 100Hz
4.2 角度计算
角度到脉宽转换：
int angle_to_pulse_width(int angle)
{
// 角度范围：0~180度
// 脉宽范围：500~2500微秒
return 500 + (angle * 2000 / 180);
}脉宽到占空比转换：
float pulse_width_to_duty_cycle(int pulse_width_us)
{
// PWM 周期：20000微秒
return (pulse_width_us / 20000.0f) * 100.0f;
}4.3 PWM 生成
int servo_set_pwm(int angle)
{
// 计算脉宽
int pulse_width_us = angle_to_pulse_width(angle);
// 计算占空比（周期20000微秒）
int period_ns = 20000000; // 20ms
int duty_ns = pulse_width_us * 1000;
// 设置 PWM
pwm_set_config(GPIO6_0, period_ns, duty_ns);
return pulse_width_us;
}5 舵机控制模式
5.1 基础控制函数
// 设置角度（0~180°）
void servo_set_angle(int angle)
{
if (angle < 0) angle = 0;
if (angle > 180) angle = 180;
int pulse_width = servo_set_pwm(angle);
printf("[Servo] 角度: %d° 脉宽: %.2fms\n", angle, pulse_width / 1000.0f);
}
// 舵机归中（90°）
void servo_center(void)
{
servo_set_angle(90);
}
// 设置脉宽（微秒）
void servo_set_pulse_width(int pulse_width_us)
{
int period_ns = 20000000; // 20ms
int duty_ns = pulse_width_us * 1000;
pwm_set_config(GPIO6_0, period_ns, duty_ns);
}5.2 扫描运行
void servo_sweep(int start_angle, int end_angle, int step, int delay_ms)
{
int direction = (start_angle < end_angle) ? 1 : -1;
int current_angle = start_angle;
while (1) {
servo_set_angle(current_angle);
current_angle += direction * step;
// 边界检测
if (current_angle >= end_angle || current_angle <= start_angle) {
direction *= -1; // 反向
current_angle = (direction > 0) ? start_angle : end_angle;
}
usleep(delay_ms * 1000);
}
}5.3 角度限制
typedef struct {
int min_angle;
int max_angle;
} servo_limits_t;
void servo_set_angle_limited(int angle, servo_limits_t *limits)
{
if (angle < limits->min_angle) {
angle = limits->min_angle;
}
if (angle > limits->max_angle) {
angle = limits->max_angle;
}
servo_set_angle(angle);
}6 关键编程要点
6.1 PWM 初始化
int pwm_init_for_servo(void)
{
// 导出 PWM
pwm_export(GPIO6_0);
// 设置周期（20ms = 20000000ns）
int period_ns = 20000000;
pwm_set_period(GPIO6_0, period_ns);
// 初始化到中间位置（1.5ms）
pwm_set_duty_cycle(GPIO6_0, 1500000); // 1.5ms
// 启用 PWM
pwm_enable(GPIO6_0);
return 0;
}6.2 角度精度控制
// 高精度角度控制（支持小数）
void servo_set_angle_precise(float angle)
{
if (angle < 0.0f) angle = 0.0f;
if (angle > 180.0f) angle = 180.0f;
// 精确计算脉宽
int pulse_width_us = (int)(500 + angle * 2000.0f / 180.0f);
servo_set_pulse_width(pulse_width_us);
}6.3 速度控制
void servo_move_with_speed(int target_angle, int step_delay_us)
{
int current_angle = get_current_angle();
int direction = (target_angle > current_angle) ? 1 : -1;
while (current_angle != target_angle) {
current_angle += direction;
servo_set_angle(current_angle);
usleep(step_delay_us);
}
}7 常见问题排查
| 问题 | 可能原因 | 解决方案 |
|---|---|---|
| 舵机抖动 | PWM 频率不准确 | 校准 PWM 频率到 50Hz |
| 角度偏差 | 脉宽计算错误 | 重新校准脉宽-角度关系 |
| 舵机无响应 | PWM 连接错误 | 检查 GPIO6_0 输出 |
| 舵机发热 | 供电电压过高 | 检查供电电压（推荐 5V） |
| 转动范围不够 | 机械限位或脉宽范围设置不当 | 检查机械结构、调整脉宽范围 |
| 舵机无力 | 供电电流不足 | 使用外部电源供电 |
舵机使用建议
- 电源容量：确保电源能提供足够的电流（至少500mA per 舵机）
- 机械设计：避免舵机超出机械限位
- PWM 精度：使用硬件 PWM 以获得更稳定的控制
- 定期校准：每6个月校准一次中位位置
8 进阶功能
8.1 平滑运动
void servo_smooth_move(int start_angle, int end_angle, int total_time)
{
int angle_diff = abs(end_angle - start_angle);
int steps = angle_diff / 2; // 每2度一步
int delay = total_time * 1000 / steps;
int direction = (end_angle > start_angle) ? 1 : -1;
int current_angle = start_angle;
for (int i = 0; i <= steps; i++) {
servo_set_angle(current_angle);
current_angle += direction * 2;
usleep(delay);
}
servo_set_angle(end_angle); // 确保到达目标角度
}8.2 多舵机控制
typedef struct {
int pwm_pin;
int current_angle;
} servo_channel_t;
servo_channel_t servos[3] = {
{GPIO6_0, 90}, // 舵机1
{GPIO6_1, 90}, // 舵机2
{GPIO6_2, 90}, // 舵机3
};
void multi_servo_set_angles(int *angles, int num_servos)
{
for (int i = 0; i < num_servos; i++) {
int pulse_width = angle_to_pulse_width(angles[i]);
pwm_set_duty_cycle(servos[i].pwm_pin, pulse_width * 1000);
servos[i].current_angle = angles[i];
}
}8.3 位置反馈
// 读取舵机当前角度（通过电位器）
int servo_read_feedback(int adc_channel)
{
int adc_value = read_adc(adc_channel);
// 将 ADC 值转换为角度（假设0~180度对应0~4095 ADC）
int angle = adc_value * 180 / 4095;
return angle;
}
// 闭环位置控制
void servo_goto_position(int target_angle, int adc_channel)
{
int current_angle = servo_read_feedback(adc_channel);
int error = target_angle - current_angle;
while (abs(error) > 2) { // 2度容差
int correction = error > 0 ? 1 : -1;
servo_set_angle(current_angle + correction);
usleep(10000); // 等待舵机响应
current_angle = servo_read_feedback(adc_channel);
error = target_angle - current_angle;
}
}8.4 舵机校准
typedef struct {
int min_pulse_width;
int max_pulse_width;
int min_angle;
int max_angle;
} servo_calibration_t;
servo_calibration_t servo_calibration = {
.min_pulse_width = 500, // 0.5ms
.max_pulse_width = 2500, // 2.5ms
.min_angle = 0,
.max_angle = 180,
};
int servo_calibrated_angle(int angle)
{
// 根据校准数据计算脉宽
int pulse_range = servo_calibration.max_pulse_width - servo_calibration.min_pulse_width;
int angle_range = servo_calibration.max_angle - servo_calibration.min_angle;
int pulse_width = servo_calibration.min_pulse_width +
(angle - servo_calibration.min_angle) * pulse_range / angle_range;
return pulse_width;
}