DASE 7503 · 中文逐页精讲

Tutorial 4 · ROS 控制 LED、舵机与电机

tutorial/DASE7503 Tutorial 4 2026-27.pdf · 共28页

没有找到对应内容,换一个词试试。

原课件第1页 / 28页把 ROS 消息变成硬件动作

Tutorial 4 原课件第1页
点击原页可放大查看

上一份教程主要是“设备读取数据,发布给电脑”;这次增加反方向:电脑向主题发命令,ESP32 订阅后控制 LED、蜂鸣器、舵机、步进和直流电机。

基本链是“ROS 命令 → Agent → ESP32 回调 → GPIO 或驱动器 → 执行器”。最后还会读取电机编码器,将计数发回电脑。

先准备 ESP32 套件、Micro-USB 数据线、Type-C 电源线和电源。各节 LED、舵机、步进、DC 电机分别提供独立 Sketch,学习时先完成一个,再上传下一个。

原课件第2页 / 28页蜂鸣器与 RGB LED 的接线不同

Tutorial 4 原课件第2页
点击原页可放大查看

蜂鸣器的 EN 接 GPIO2,控制脚设 HIGH 时响、LOW 时停。VCC 负责供电,GND 接地。

WS2812B 的 DIN 接 GPIO14。它需要按特定时序发送一串 24 位颜色数据,因此要用 NeoPixel 库;单纯把引脚拉高只能得到高电平,无法表达完整颜色。

RGB 各用 8 位,即每色 0~255,共 256³=16777216 种编码组合。例如 (255,0,0) 是红,(0,255,0) 是绿,(0,0,0) 关闭光输出。数据线、供电线和地线要分别接对。

原课件第3页 / 28页步进发相序,舵机发目标角脉冲

Tutorial 4 原课件第3页
点击原页可放大查看

上图的 5 线步进电机通过 ULN2003 驱动:IN1/IN2/IN3/IN4 分别接 GPIO32/33/25/26。ESP32 轮流输出线圈相序,驱动器提供线圈电流;步数决定转多少,步频决定转多快。

下图的舵机信号接 GPIO23。舵机内部会根据控制脉冲宽度调整角度,后面由 ESP32Servo 库生成脉冲。

实际接线时,步进电机用匹配的 5 V 动力电源;舵机电源按型号要求接入,控制器与驱动器共地。图区中的 3.3 V 逻辑标注要与电机动力供电分清,电机电流由电源和驱动器承担。

原课件第4页 / 28页直流电机的驱动链和测量链

Tutorial 4 原课件第4页
点击原页可放大查看

先沿输出链看:GPIO13 接 STB 使能,GPIO5 接 PWMA 控制占空比,GPIO4/15 接 AIN1/AIN2 决定方向;驱动器的 M1/M2 接电机。

再沿反馈链看:编码器 A/B 接 GPIO34/35,ESP32 用两相信号累计转动计数并判断方向。电机转动与编码器读数通过不同线路传递。

下图把逻辑电源 VCC 和电机电源 VM 分开,共用 GND。读真值表时同时看 STBY、方向输入和 PWM:STBY 低时待机;方向输入和 PWM 共同决定正转、反转、制动或输出关闭。后面代码会采用其中的控制组合。

原课件第5页 / 28页导入 LED 库,建立控制对象

Tutorial 4 原课件第5页
点击原页可放大查看

先在库管理器安装 Adafruit_NeoPixel。代码中的 micro-ROS/rcl/rclc 负责通信,Int32 用于蜂鸣命令,ColorRGBA 用于颜色命令,NeoPixel 负责发送 LED 时序。

BUZZER_PIN=2、LED_PIN=14、LED_COUNT=1 对应一个蜂鸣器和一颗 LED。strip(LED_COUNT,LED_PIN,NEO_GRB+NEO_KHZ800) 创建控制对象,GRB 指线上颜色通道顺序,800 指 800 kHz 协议。

tBuzzerStarted 保存开始响的时间,bBuzzerStarted=false 表示目前没有计时。之后收到命令时记起点,主循环再负责到时关闭。

原课件第6页 / 28页错误宏和两个消息缓冲

Tutorial 4 原课件第6页
点击原页可放大查看

RCCHECK 先调用函数,检查返回值是否等于 RCL_RET_OK;出错时进入 error_loop。RCSOFTCHECK 也检查返回值,但错误分支为空,会继续往下执行。宏末尾的反斜杠表示定义还延续到下一行,复制时要保留。

subscriber_led_rgb、subscriber_buzzer_beep 是两个订阅接口;msg_led_rgb、msg_buzzer_beep 是接收消息的存放位置。一个接口对应一种消息类型,颜色消息与整数消息不能混用缓冲。

executor 管理回调,support 提供上下文,allocator 管理内存,node 是节点。代码虽声明 timer,但这段 LED 程序实际使用两个订阅回调。

原课件第7页 / 28页收到蜂鸣和颜色命令后做什么

Tutorial 4 原课件第7页
点击原页可放大查看

error_loop 每 100 ms 翻转蜂鸣器电平。蜂鸣订阅回调把 msgin 转成 Int32 消息指针。data==1 时开启蜂鸣、记录起始毫秒数,再把 bBuzzerStarted 设为 true。第 9 页的主循环在 500 ms 后关掉提示音。

颜色回调先开启提示音,再把三色各放进 32 位整数中的一个字节:

uint32_t new_color = (static_cast<uint32_t>(msg->r) << 16)
                   | (static_cast<uint32_t>(msg->g) << 8)
                   |  static_cast<uint32_t>(msg->b);
strip.setPixelColor(0, new_color);
strip.show();

以 r=128.0, g=64.0, b=32.0 为例。转整数后,十六进制分别是 0x80、0x40、0x20。按字段位置移动:

通道 位运算 移动后
红 0x80 << 16 0x800000
绿 0x40 << 8 0x004000
蓝 保持低 8 位 0x000020

| 把各组位合进同一个数,结果为 0x804020。setPixelColor(0, ...) 改第 0 颗灯的颜色缓存,show() 发给灯。库根据初始化中的 NEO_GRB 排列实际线上字节。此 Sketch 和第 10–11 页终端命令约定传入 0–255 的 RGB,代码未读取 a。

原课件第8页 / 28页setup 建立节点和颜色订阅

Tutorial 4 原课件第8页
点击原页可放大查看

setup 先把蜂鸣器设成 OUTPUT 并置 LOW,避免启动时一直响;再设置 micro-ROS 传输,等待,建立 allocator、support 和节点。

rclc_subscription_init_default 创建颜色订阅:消息类型是 ColorRGBA,主题名是 led_rgb。接下来用同样方式创建 Int32 类型的蜂鸣订阅。

LED 还应在 setup 加上 strip.begin(),并设置初始颜色后 strip.show()。对象创建只是保存配置,begin 才完成库需要的初始化;原页展示的 setup 缺少这一步。空命名空间下这些相对主题名对应 /led_rgb 和 /buzzer_beep。

原课件第9页 / 28页两个订阅回调与半秒蜂鸣

Tutorial 4 原课件第9页
点击原页可放大查看

executor 容量设为 2,因为要加入两个订阅者。每次 add_subscription 都绑定四件事:订阅接口、消息缓冲、回调函数、ON_NEW_DATA 触发条件。新消息到达后,执行器才调用对应回调。

loop 先 spin_some,最多等待 10 ms 处理消息;然后检查蜂鸣状态和经过时间。若正在响且 millis()-tBuzzerStarted>500,就拉低引脚并清除状态。

例如开始时刻为 1000 ms,当前为 1501 ms,差值 501,满足关闭条件。这样等待蜂鸣结束时仍能继续收消息;连续颜色命令会重设起点,让蜂鸣从最近一次命令重新计时。

原课件第10页 / 28页上传后,向蜂鸣器和 LED 发一条命令

Tutorial 4 原课件第10页
点击原页可放大查看

在 Arduino IDE 选择 ESP32 Dev Module 和实际串口。编译成功后,先关闭仍占用串口的 Agent,再上传。若上传时权限不足,按第 3 份 Tutorial 的串口权限步骤检查当前用户能否访问该设备。下面以 /dev/ttyUSB0 为例。

上传完,在终端 A 启动 Agent:

docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:jazzy serial --dev /dev/ttyUSB0 -v6

-v /dev:/dev 把主机设备映射给容器,serial --dev 选择其中的串口。让 A 保持运行,并在终端 B 发测试消息:

source /opt/ros/jazzy/setup.bash
ros2 topic list
ros2 topic pub --once /buzzer_beep std_msgs/msg/Int32 "{data: 1}"
ros2 topic pub --once /led_rgb std_msgs/msg/ColorRGBA "{r: 255.0, g: 0.0, b: 0.0, a: 1.0}"
ros2 topic pub --once /led_rgb std_msgs/msg/ColorRGBA "{r: 0.0, g: 255.0, b: 0.0, a: 1.0}"

先确认主题列表出现 /buzzer_beep 和 /led_rgb。逐行执行测试,预期先短响,再亮红、亮绿。改灯色也会触发一次提示音。--once 表示只发一条,收到后灯会保持设置的颜色。

原课件第11页 / 28页蓝、白、关灯,再开始舵机

Tutorial 4 原课件第11页
点击原页可放大查看

蓝色给 r=0,g=0,b=255;白色给三色都为 255;关灯给三色都为 0。命令中的 a 始终写 1,本代码不读取它,因此关闭 LED 是通过 RGB 全零实现。

随后进入舵机示例。舵机内部有电机、位置传感器和控制器:接收目标角度,测当前角度,比较差值,再调整电机。

在库管理器安装 ESP32Servo,新建 Sketch,导入 micro-ROS 和 Int32 等头文件。ROS 发送的是目标角度整数;ESP32Servo 库负责将角度转换成舵机控制脉冲。

原课件第12页 / 28页建立舵机对象和接收缓冲

Tutorial 4 原课件第12页
点击原页可放大查看

SERVO_PIN=23 对应控制信号线;Servo servo 创建舵机对象。BUZZER_PIN=2 以及开始时间、计时状态继续用于接收提示。

subscriber_servo_pos 是订阅接口,msg_servo_pos 是 Int32 接收缓冲。错误宏与前例相同:初始化关键步骤失败时,进入持续蜂鸣的 error_loop。

读这段代码先看对象与引脚的关系:对象叫 servo,后面要用 attach 把它连接到 GPIO23,再由 write 设置目标。这里先完成声明,真正的初始化在 setup 中。

原课件第13页 / 28页舵机回调与 attach 初始化

Tutorial 4 原课件第13页
点击原页可放大查看

订阅回调把收到的消息转成 Int32,执行 servo.write(msg->data)。例如 data=90,表示把目标设为 90°。库随后输出相应脉冲,舵机内部控制器驱动转轴靠近这个角度。

回调最后开启蜂鸣并记录时间。它没有读取实际舵机角度,因此这声提示表示命令已经收到。

setup 先初始化蜂鸣器,再 servo.attach(SERVO_PIN) 建立舵机输出,接着配置 micro-ROS 的传输、等待、allocator 和 support。先 attach,后通过回调 write,才能把对象操作连接到实际引脚。

原课件第14页 / 28页将 /servo_pos 连接到回调

Tutorial 4 原课件第14页
点击原页可放大查看

建立节点后,创建 Int32 类型的 servo_pos 订阅。执行器容量为 1,因为这里只加入一个订阅者;add_subscription 将它与 msg_servo_pos、舵机回调和 ON_NEW_DATA 绑定。

loop 反复 spin_some 处理新命令,并检查是否超过 500 ms 来关闭蜂鸣器。这个半秒计时控制的是提示音。

完整消息路径可以顺着读:电脑发 /servo_pos → Agent → 接收缓冲 → 舵机回调 → servo.write → GPIO23 脉冲 → 舵机内部反馈控制。

原课件第15页 / 28页发 0°、90°、180° 目标

Tutorial 4 原课件第15页
点击原页可放大查看

先编译上传这份舵机 Sketch,上传完成再启动 Agent。源代码的 ROS 环境在 Ubuntu 终端中加载。

使用 ros2 topic pub --once /servo_pos std_msgs/msg/Int32 "{data: 0}" 发 0°;将 data 改成 90 或 180,分别发另外两个目标。主题名决定送到哪条通信通道,Int32 决定数据格式,data 是目标角度。

本例使用 0~180 范围;做动作前确保转臂周围有活动空间。先小范围改变目标,观察实际方向和机械限位,再继续其他角度。

原课件第16页 / 28页用步数描述转动量

Tutorial 4 原课件第16页
点击原页可放大查看

步进电机通过按次序通电的线圈,一小步一小步转动。普通 28BYJ-48 示例按开环步数控制,本页关于“必须有反馈传感器”的描述应按这个实际工作方式理解。

安装 Unistep2,新建 Sketch。IN1/IN2/IN3/IN4 对应 32/33/25/26;stepsPerRevolution=4096 规定程序使用每圈 4096 步的换算;构造 Stepper 对象时最后的 1000 表示最短步间隔 1000 µs = 1 ms。run() 每次只在间隔已到时推进一步,调用偏晚会拉长实际间隔。

按本例换算,每步 360/4096≈0.08789°,2048 步对应半圈。实际运动还取决于相序、减速比、负载和是否失步。

原课件第17页 / 28页为什么给步进单独建任务

Tutorial 4 原课件第17页
点击原页可放大查看

步进程序有两个一直在运行的循环。Arduino loop() 通过 spin_some() 等待和处理 ROS 消息;TaskStepper 则要频繁调用 Stepper.run()。假如把它们串在同一个循环里,一次最长约 10 ms 的等待就已经跨过 10 个 1 ms 步间隔。

TaskHandle_t Task1 保存任务句柄,绑定核心由创建任务时的最后一个参数决定。stepsReq=0 表示暂时没有新请求。正负值分别交给库选择转动方向,下一页跟一次消息经过这两个任务的过程。

原课件第18页 / 28页回调交请求,任务持续 run

Tutorial 4 原课件第18页
点击原页可放大查看

跟一条 data=3 的消息。起初 stepsReq=0,库内剩余步数也为 0。

发生的操作 stepsReq 库内剩余步数
ROS 回调收到消息,写入 stepsReq 3 0
步进任务调用 Stepper.move(3) 并清零请求 0 3
第一次到达可步进时刻,run() 推进一相 0 2
第二、第三次到时继续推进 0 1 → 0

stepsReq 清零只代表请求已取走。电机继续走到库中的剩余步数清零。run() 在间隔未到时直接返回。vTaskDelay(1/portTICK_PERIOD_MS) 用整数计算 tick 数,课件写的 1 ms 依赖系统 tick 配置。

xTaskCreatePinnedToCore(..., &Task1, 1) 把这个任务绑定到 core 1。示例的 stepsReq 是两任务共享的单槽变量。若新请求恰好在旧请求刚读出、尚未清零时写入,会被清零覆盖。库的 move(n) 也会直接重设剩余步数。验证时等上一条走完再发下一条。若要连续接收并依次完成多条命令,应使用任务队列,并等当前动作结束后再取下一条。

Unistep2 的 move 与 run 实现

原课件第19页 / 28页先启动步进任务,再建立 ROS 订阅

Tutorial 4 原课件第19页
点击原页可放大查看

setup 先把蜂鸣器设为输出并关闭,然后调用 createStepperTask。新任务开始循环,但 stepsReq 初值为 0,所以此时等待请求。

接着配置 micro-ROS,建立 allocator、support 和节点,最后创建 Int32 类型的 stepper_steps 订阅。回调收到步数后写入 stepsReq,步进任务再读取它。

本页解释文字中的 servo_pos 和 servo.write 是上一节遗留;这里实际用的是 stepper_steps 与 Stepper.move。阅读代码时以本节真实的对象和函数为准。

原课件第20页 / 28页主循环处理消息,步进任务安排脉冲

Tutorial 4 原课件第20页
点击原页可放大查看

executor 容量为 1,把 stepper_steps 订阅绑定到步数回调。loop 中 spin_some 处理 ROS 命令,再检查 500 ms 蜂鸣超时。

Stepper.run 已放到 TaskStepper,不必在主 loop 重复安排另一套控制流程。两个循环各有任务:主循环接命令和提示音,步进任务持续推进电机。

本页后半重新选择板卡、串口、编译并配置权限。把跨页代码合到同一 Sketch 时,先确认 setup 和 loop 的括号完整,再编译;编译通过后停止 Agent,上传固件。

原课件第21页 / 28页正反一圈,并转到编码器电机

Tutorial 4 原课件第21页
点击原页可放大查看

上传步进 Sketch,启动第 10 页的 Agent。另开一个已加载 ROS 的终端,逐条执行,等正向动作走完再发反向命令:

ros2 topic pub --once /stepper_steps std_msgs/msg/Int32 "{data: 4096}"
ros2 topic pub --once /stepper_steps std_msgs/msg/Int32 "{data: -4096}"

课件用 4096 步对应输出轴一圈。4096÷4096×360°=360°,因此第一条请求正向一圈,第二条请求反向一圈。发 1024 时,1024÷4096=1/4,目标是相对当前位置转 360°/4=90°。实际角度受齿轮减速比和失步影响。

下一节换成直流编码器电机。请新建 Sketch,接下来一条 ROS 主题传 PWM 命令,另一条把编码器累计计数返回电脑。

原课件第22页 / 28页直流电机的引脚与累计计数

Tutorial 4 原课件第22页
点击原页可放大查看

STB=13 控制驱动使能;PWMA=5 控制 PWM 幅值;AIN1=4、AIN2=15 决定方向;编码器 A=34、B=35;蜂鸣器=2。先把这些常量与第 4 页接线图逐项对上。

encoderCount=0 是累计转动计数,转一方向增加,另一方向减少。interruptTriggered 是中断发生标志,本例后面没有再利用它触发其他操作。

两个变量带 volatile,告诉编译器它们可能被中断异步修改。需要在任务间一致地读取或修改数据时,还要使用合适的同步方式;volatile 本身只解决编译器对变量访问的处理。

原课件第23页 / 28页用中断快速计数,用定时器发送

Tutorial 4 原课件第23页
点击原页可放大查看

第 25 页的 attachInterrupt(..., CHANGE) 让 A 每次变化都触发 encoderISR。进中断后比较 A/B:相同就 encoderCount++,不同就 encoderCount--。以 AB 表示两位电平:

  • 00 → 10 → 11 → 01 → 00 中,只有 10 和 01 对应 A 的变化。当时两位不同,所以两次各减 1,总计 −2。
  • 00 → 01 → 11 → 10 → 00 中,只有 11 和 00 对应 A 的变化。当时两位相同,所以两次各加 1,总计 +2。

这段程序每个 A/B 电气周期记两次,方向符号随 A/B 顺序和机械正方向约定变化。中断更新计数,定时回调把当时的 encoderCount 复制到 Int32 消息的 data,下一页再发布。

原课件第24页 / 28页把正负 PWM 分成方向和幅值

Tutorial 4 原课件第24页
点击原页可放大查看

上面第一行把累计计数发布到 /encoder_count。下面的订阅回调从 /motor_pwm 取 msg->data,先按正负选方向,再将 PWM 幅值限制在 0–255。

if (pwm > 0) {
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  pwm = (pwm > 255) ? 255 : pwm;
  analogWrite(PWMA, pwm);
} else if (pwm < 0) {
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  pwm = (pwm < -255) ? 255 : -pwm;
  analogWrite(PWMA, pwm);
} else {
  analogWrite(PWMA, 0);
}

条件 ? A : B 在条件为真时取 A,否则取 B。逐值跟踪:

输入 data 分支与限幅 AIN1 / AIN2 写入 PWMA 占空比
200 正,200 > 255 为假,保留 200 HIGH / LOW 200 200/255≈78.4%
−150 负,−150 < −255 为假,取 −(−150)=150 LOW / HIGH 150 150/255≈58.8%
−300 负,−300 < −255 为真,取 255 LOW / HIGH 255 255/255=100%
0 直接把 PWMA 归零 保持上次设置 0 0/255=0%

200 控制的是占空比,实际 rpm 要用编码器测量。回调最后开启蜂鸣并记录起点,随后由主循环关闭蜂鸣。

原课件第25页 / 28页初始化为不驱动,再启用编码器中断

Tutorial 4 原课件第25页
点击原页可放大查看

setup 先关闭蜂鸣器,设置 STB、PWMA、AIN1、AIN2 为输出;STB 拉高启用驱动,方向脚先 LOW,PWM 先 0。这样正常启动时先保持无驱动输出。

编码器 A/B 设置为输入。attachInterrupt(digitalPinToInterrupt(ENCODER_A),encoderISR,CHANGE) 把 A 的上升沿和下降沿都交给 encoderISR 处理。CHANGE 因而包括两个边沿;后续每圈计数 N 要与这一设定一致。

然后配置 micro-ROS 的传输、等待、allocator 和 support。方向脚输出、编码器测量输入、ROS 通信初始化三条逻辑按顺序建立。

原课件第26页 / 28页每半秒发送计数,怎样算转速

Tutorial 4 原课件第26页
点击原页可放大查看

节点建好后,设置 timer_timeout=500 ms,所以发送频率 f=1/0.5=2 Hz。创建 /encoder_count 发布者与 motor_pwm 订阅者。executor 容量为 2,对应一个 timer 和一个 subscription;publisher 是在回调中主动调用的发送接口。

速度计算示例:上次计数 1000,本次 1200,间隔 0.5 s,每圈计数 N=400。先作差 ΔC=1200-1000=200;再算圈数 200/400=0.5;除以时间得 0.5/0.5=1 转/s;乘 60 得 60 rpm。

这里的 2 Hz 是计数消息频率。PWM 本身的开关频率由底层输出设置决定。

原课件第27页 / 28页发一次命令后会维持多久

Tutorial 4 原课件第27页
点击原页可放大查看

本页完成订阅注册,然后 loop 持续 spin 并检查蜂鸣超时。收到一次非零 PWM 后,输出会保持这个设置,直到收到新命令、复位或其他程序改变它。

蜂鸣器的 500 ms 关闭只控制提示音。它不会顺便把电机 PWM 清零。希望电机停下时,要向 motor_pwm 明确发送 0。

如果用于需要失联自动停止的控制,还应记录最后命令时刻,在超过允许间隔时主动清零输出。课件当前 error_loop 也只控制提示音,设计错误处理时应另外安排电机停止动作。

之后完成选板、串口访问、编译与上传,准备最后一页的正反停练习。

原课件第28页 / 28页同时观察命令、转轴和计数

Tutorial 4 原课件第28页
点击原页可放大查看

上传直流电机 Sketch,启动第 10 页的 Agent,再打开终端 B 观察编码器:

source /opt/ros/jazzy/setup.bash
ros2 topic echo /encoder_count

在终端 C 逐条发命令,观察每一步的转轴方向和累计计数:

source /opt/ros/jazzy/setup.bash
ros2 topic pub --once /motor_pwm std_msgs/msg/Int32 "{data: 200}"
ros2 topic pub --once /motor_pwm std_msgs/msg/Int32 "{data: -150}"
ros2 topic pub --once /motor_pwm std_msgs/msg/Int32 "{data: 0}"

200 走代码正向分支,-150 换成反向、幅值 150。计数随转动变化,若正向示例中是 1000 → 1200,反向时增量应改为相反符号,具体正负取决于接线与软件符号约定。发 0 后,驱动命令归零,轴停稳后累计计数会保持在最后一个值。

--once 只发送一条消息。ESP32 会保留最后的 PWM 设置,练习结束发送 0。用第 26 页的方法从计数变化算 rpm。