📚 课程目录第4章 PWM舵机控制

4. PWM舵机控制

任务目标

本案例实现一个micro-ROS节点，订阅/servo_angle话题（数据类型std_msgs/msg/Int16），将接收到的角度值（0～180度）映射为PWM占空比信号，通过定时器通道输出，驱动舵机转动至对应角度。

硬件与依赖

主控：ESP32（或任意支持micro-ROS的MCU，需具备LEDC/PWM外设）
舵机：SG90（工作频率50Hz，脉宽0.5ms～2.5ms对应0°～180°）
micro-ROS客户端已生成并配置好std_msgs接口
接线：舵机信号线接GPIO5，VCC接5V，GND共地

核心实现

代码使用ESP32 LEDC外设生成50Hz PWM信号。将订阅到的角度值线性映射到占空比：duty = 角度/180 × (2.5-0.5) + 0.5（单位ms），再换算为LEDC占空比寄存器值。主循环中调用rclc_executor_spin_some处理订阅回调。

#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int16.h>

#define LEDC_TIMER      LEDC_TIMER_0
#define LEDC_MODE       LEDC_LOW_SPEED_MODE
#define LEDC_CHANNEL    LEDC_CHANNEL_0
#define LEDC_GPIO       5
#define PWM_FREQ        50          // 舵机工作频率 50Hz
#define PWM_RESOLUTION  14          // 14位分辨率，占空比范围 0~16383

rcl_publisher_t publisher;
rcl_subscription_t subscriber;
std_msgs__msg__Int16 msg_in;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

// 角度(0~180) → 占空比寄存器值
uint32_t angle_to_duty(int16_t angle) {
    if (angle < 0) angle = 0;
    if (angle > 180) angle = 180;
    // 脉宽 0.5ms~2.5ms，周期20ms
    float pulse_ms = 0.5f + (angle / 180.0f) * 2.0f;
    // 占空比 = 脉宽/周期 × 2^分辨率
    return (uint32_t)((pulse_ms / 20.0f) * (1 << PWM_RESOLUTION));
}

void servo_callback(const void *msgin) {
    const std_msgs__msg__Int16 *angle_msg = (const std_msgs__msg__Int16 *)msgin;
    uint32_t duty = angle_to_duty(angle_msg->data);
    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, duty);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
}

void setup() {
    // 初始化LEDC
    ledc_timer_config_t timer_conf = {
        .speed_mode = LEDC_MODE,
        .duty_resolution = (ledc_timer_bit_t)PWM_RESOLUTION,
        .timer_num = LEDC_TIMER,
        .freq_hz = PWM_FREQ,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&timer_conf);

    ledc_channel_config_t channel_conf = {
        .gpio_num = LEDC_GPIO,
        .speed_mode = LEDC_MODE,
        .channel = LEDC_CHANNEL,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER,
        .duty = 0,
        .hpoint = 0
    };
    ledc_channel_config(&channel_conf);

    // micro-ROS初始化
    set_microros_wifi_transports("WiFi_SSID", "WiFi_PASSWORD", "192.168.1.100", 8888);
    allocator = rcl_get_default_allocator();
    rclc_support_init(&support, 0, NULL, &allocator);
    rclc_node_init_default(&node, "servo_pwm_node", "", &support);

    // 订阅 /servo_angle
    rclc_subscription_init_default(
        &subscriber, &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
        "/servo_angle");
    rclc_executor_init(&executor, &support.context, 1, &allocator);
    rclc_executor_add_subscription(&executor, &subscriber, &msg_in, &servo_callback, ON_NEW_DATA);
}

void loop() {
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
    delay(10);
}

运行与验证

在platformio.ini中配置micro-ROS环境（agent IP与端口需与代码一致），编译烧录至ESP32。
启动micro-ROS Agent：docker run -it --rm -p 8888:8888 microros/micro-ros-agent:humble udp4 --port 8888
在PC终端发布角度指令：
ros2 topic pub /servo_angle std_msgs/msg/Int16 "{data: 90}" --once
通过条件：舵机转动至约90°位置（中间位置）。依次发布0、90、180，舵机应分别转至两端极限和中间位置，且无抖动或卡死。

常见问题

舵机不转：检查GPIO5是否与舵机信号线正确连接，确认电源5V供电电流足够（SG90堵转时约500mA）。
角度反向或范围不对：不同舵机脉宽范围略有差异，若0°或180°不到位，微调angle_to_duty中0.5和2.5的边界值。
WiFi连接失败：确认SSID/密码正确，Agent所在主机IP与端口可达。