johnny-five 磁力计日志实战:用 Compass headingchange 驱动连续舵机转向

原创2026-09-22 11:29:081,867 阅读
文章标签:IoT机器人嵌入式

johnny-five 磁力计日志实战:用 Compass headingchange 驱动连续舵机转向

本指南以 johnny-five 仓库中的 docs/magnetometer-log.md 文档为主线,讲解如何利用 five.Magnetometer(Compass 的别名)的 headingchange 事件实时读取航向角,并用带颜色的终端日志可视化 32 方位罗盘读数,同时通过连续旋转舵机实现"自动转向北方"的闭环控制。读完本文,你将掌握 johnny-five 磁力计的属性与事件 API、连续舵机的驱动方式,以及把二者组合成方位感知机器人控制逻辑的完整思路。

示例总览与运行方式

magnetometer-log 是 johnny-five 官方示例之一(对应示例源码 eg/magnetometer-log.js)。它的核心行为是:

  1. 创建一个 I2C 磁力计实例(new five.Magnetometer()),默认挂载 HMC5883L 控制器;
  2. 创建一个 9 号引脚上的连续旋转舵机;
  3. 每当航向角发生变化(headingchange 事件),用 chalk 以不同颜色打印当前方位名与角度;
  4. 当检测到朝向正北(bearing.name === "North")时,反向切换舵机旋转方向,模拟"寻北转向"的机器人行为。

在已安装 johnny-five 依赖、并已连接好硬件(Arduino 等支持 Firmata 的开发板)的前提下,直接在仓库根目录运行:

node eg/magnetometer-log.js

若在 Node.js 项目中独立使用,则需要先安装 johnny-five 与 chalk(chalk 正是本示例控制台彩色输出的依赖,声明于 package.json 的 dependencies 中,版本为 2.1.0):

npm install johnny-five chalk

硬件接线前提

本示例涉及两类硬件设备:

  • I2C 磁力计:five.Magnetometer() 默认对应 HMC5883L 控制器(见 lib/compass.js 中 Controllers.HMC5883L 的实现,默认 I2C 地址 0x1E),通过 I2C 总线与开发板通信;
  • 连续旋转舵机:连接在数字引脚 9,type: "continuous" 表示该舵机是连续旋转模式(非标准角度舵机)。

接线时确保磁力计的 SDA/SCL/VCC/GND 与开发板对应引脚相连。若使用标准角度舵机,只需去掉示例中的 type: "continuous" 即可(该选项注释也说明了这一点)。

完整代码

原文档给出的完整示例代码如下(与仓库 eg/magnetometer-log.js 保持一致):

var chalk = require("chalk"),
  five = require("johnny-five"),
  board, colors, servo, mag, count, dirs, lock;

(board = new five.Board()).on("ready", function() {

  count = -1;
  dirs = ["cw", "ccw"];
  lock = false;

  [
    // Medium Speed Counter Clock Wise
    [92, "ccw"],
    // Medium Speed Clock Wise
    [88, "cw"]

  ].forEach(function(def) {

    // Define a directional method and default speed
    five.Servo.prototype[def[1]] = function(speed) {
      speed = speed || def[0];

      this.move(speed);
    };
  });


  // Create a new `servo` hardware instance.
  servo = new five.Servo({
    pin: 9,
    // `type` defaults to standard servo.
    // For continuous rotation servos, override the default
    // by setting the `type` here
    type: "continuous"
  });


  // Create an I2C `Magnetometer` instance
  mag = new five.Magnetometer();

  // Inject the servo and magnometer into the REPL
  this.repl.inject({
    servo: servo,
    mag: mag
  });

  // set the continuous servo to stopped
  servo.move(90);

  // As the heading changes, log heading value
  mag.on("headingchange", function() {
    var log;
    var color = colors[this.bearing.abbr];

    log = (this.bearing.name + " " + Math.floor(this.heading) + "°");

    console.log(
      chalkcolor
    );


    if (!lock && this.bearing.name === "North") {
      // Set redirection lock
      lock = true;

      // Redirect
      servo[dirs[++count % 2]]();

      // Release redirection lock
      board.wait(2000, function() {
        lock = false;
      });
    }
  });

  this.wait(2000, function() {
    servo[dirs[++count % 2]]();
  });
});

colors = {
  N: "red",
  NbE: "red",
  NNE: "red",
  NEbN: "red",
  NE: "yellow",
  NEbE: "yellow",
  ENE: "yellow",
  EbN: "yellow",
  E: "green",
  EbS: "green",
  ESE: "green",
  SEbE: "green",
  SE: "green",
  SEbS: "cyan",
  SSE: "cyan",
  SbE: "cyan",
  S: "cyan",
  SbW: "cyan",
  SSW: "cyan",
  SWbS: "blue",
  SW: "blue",
  SWbW: "blue",
  WSW: "blue",
  WbS: "blue",
  W: "magenta",
  WbN: "magenta",
  WNW: "magenta",
  NWbW: "magenta",
  NW: "magenta",
  NWbN: "magenta",
  NNW: "magenta",
  NbW: "red"
};

逐段解析:从初始化到转向闭环

1. 为 Servo 原型扩展方向方法

示例在 ready 回调开头遍历 [[92, "ccw"], [88, "cw"]],为 five.Servo.prototype 动态添加 ccw()(逆时针)与 cw()(顺时针)两个方法:

five.Servo.prototype[def[1]] = function(speed) {
  speed = speed || def[0];
  this.move(speed);
};

这里的 92 与 88 是连续舵机在"中位 90°"附近的两个偏置速度值:连续舵机的 move(degrees) 中 90 表示静止,略大于 90 与略小于 90 分别驱动两个旋转方向,数值偏离 90 越远转速越快。该方法被挂到原型上,因此后续 servo.ccw()、servo.cw() 均可在所有 Servo 实例上直接调用,未传参时使用默认速度。

2. 创建连续旋转舵机

servo = new five.Servo({
  pin: 9,
  type: "continuous"
});

type: "continuous" 是关键选项。查看 lib/servo.js 构造逻辑可以发现,当 options.type === "continuous" 时,初始化完成后会立即调用 this.stop(),将舵机停在静止位(见 lib/servo.js);而 stop() 对连续舵机的实现是把舵机 to() 到 deadband(死区)的中点(见 lib/servo.js),从而让舵机真正停转。

3. 创建磁力计并注入 REPL

mag = new five.Magnetometer();

this.repl.inject({
  servo: servo,
  mag: mag
});

Magnetometer 是 Compass 的别名,二者完全等价——在 lib/johnny-five.js 中有明确的导出语句 module.exports.Magnetometer = module.exports.Compass;。参考 docs/magnetometer.md 中的说明:

// five.Magnetometer() 等价于:
//   new five.Compass({
//    controller: "HMC5883L",
//    freq: 50,
//    gauss: 1.3
//   });

this.repl.inject() 把 servo 和 mag 注入到 johnny-five 的交互式 REPL 环境(参考 docs/repl.md),程序运行期间可以在终端直接输入 mag.heading、servo.ccw() 等表达式进行实时调试。

4. 航向变化事件与彩色日志

servo.move(90); // 先让连续舵机静止

mag.on("headingchange", function() {
  var log;
  var color = colors[this.bearing.abbr];
  log = (this.bearing.name + " " + Math.floor(this.heading) + "°");
  console.log(chalkcolor);
  // ...
});

headingchange 事件在计算出的航向角发生变化时触发。在事件回调中:

  • this.heading:以度为单位的当前航向(0° 指向磁北,顺时针递增,取值范围 0~360);
  • this.bearing:方位信息对象,包含 name(如 "North"、"East-NorthEast")、abbr(如 "N"、"ENE")、low/high/heading 等字段;
  • Math.floor(this.heading) 用于去掉小数,保证日志输出整洁;
  • colors[this.bearing.abbr] 根据方位缩写查表得到 chalk 颜色名,再用 chalkcolor 输出带颜色的日志,例如北方显示为红色。

headingchange 与 data、change、read 等事件都在 lib/compass.js 中由内部定时采样循环触发:构造器以 opts.freq || 25(默认 25ms)为周期轮询原始数据,计算 heading 并与上一次值比较,发生变化时发出 change 与 headingchange 事件(见 lib/compass.js)。

5. 32 方位罗盘与颜色映射表

colors 映射表覆盖了完整的 32 方位罗盘(32-point compass)缩写,与源码中 lib/compass.js 定义的 Compass.Points 一一对应。Compass.Points 是一个被 Object.freeze 冻结的常量数组,把 0~360° 划分为 33 个区间(正北由两个区间组成:354.38°~360° 与 0°~5.62°),每个区间包含:

  • name:完整方位名(如 "North-East");
  • abbr:标准缩写(如 "NE");
  • low / high:该方位覆盖的角度范围。

bearing 属性的 getter 遍历 Compass.Points,找到 heading 落入的区间并返回包含 name、abbr、low、high、heading 的对象(见 lib/compass.js)。颜色分配逻辑大致为:北侧区域为红色、东北/东侧为黄绿、南侧为青蓝、西侧为品红,方位越接近正北颜色越"警示",方便一眼判断朝向。

6. 寻北重定向逻辑与锁

if (!lock && this.bearing.name === "North") {
  lock = true;
  servo[dirs[++count % 2]]();   // 交替执行 cw / ccw
  board.wait(2000, function() {
    lock = false;
  });
}

当方位为北时,示例通过 lock 布尔锁避免在连续触发期间重复转向:一旦进入北向,就锁定并调用 servo.cw() 或 servo.ccw() 切换旋转方向,2 秒后由 board.wait(2000, ...) 解除锁定。dirs[++count % 2] 让两次北向触发分别选择 cw 与 ccw,形成来回转向的演示效果。程序启动时 this.wait(2000, ...) 还会先让舵机主动转一次,确保闭环立即进入运动状态。

源码级原理:heading 与 bearing 是如何算出来的

要真正用好这个日志示例,有必要理解 lib/compass.js 中的两个核心计算:

航向角计算 ToHeading(x, y)(lib/compass.js):通过 Math.atan2(y, x) 计算反正切,将结果归一化到 0~2π,再乘以 RAD_TO_DEG 得到 0~360 的角度。以 HMC5883L 为例,toScaledHeading 会先把原始 x/y 乘以量程系数 state.scale 再交给 ToHeading(见 lib/compass.js)。

量程系数 Compass.Scale(lib/compass.js):根据 opts.gauss 选择寄存器值与换算系数,支持的档位包括 0.88、1.3、1.9、2.5、4.0、4.7、5.6、8.1 高斯,默认取 0.88(系数 0.73)。gauss 需与传感器量程匹配,否则读数可能失真。

控制器实现:Controllers 对象中内置了 HMC5883L、HMC6352、BNO055、MAG3110、LSM303C 等控制器的初始化与读数逻辑(见 lib/compass.js)。其中 HMC5883L 通过 i2cWrite 配置 CRA(0x70,8 次平均/15Hz 默认测量)、CRB(0x40)与 MODE(0x00,连续测量)三个寄存器后,以 6 字节连续读取 x/y/z 原始值(见 lib/compass.js)。MAG3110 控制器还内置了基于 20 次测量的自动校准流程,校准完成会触发 calibrated 事件(见 lib/compass.js)。

属性一览(来自 docs/magnetometer.md 与 lib/compass.js 的只读属性定义):

属性 含义
mag.raw 原始三轴读数对象 { x, y, z }
mag.heading 以度为单位的航向角(0~360,磁北校准)
mag.bearing 方位数据对象 { name, abbr, low, high, heading }

扩展与相关示例

围绕磁力计,仓库还提供了多个可直接对照学习的示例:

运行注意事项

  • 磁力计校准:heading 是基于磁北计算的,周围环境的铁磁性物体、电机磁场都会引入偏差,导致航向不准;需要校准或为设备选择无干扰的安装位置。MAG3110 控制器的自动校准(calibrate(measurements))可参考 lib/compass.js。
  • I2C 地址冲突:HMC5883L 默认地址 0x1E(见 lib/compass.js),若总线上已有同地址设备需通过 address 选项指定;HMC6352 为 0x21,MAG3110 固定为 0x0E。
  • 连续舵机中位:90 附近存在 deadband 死区,示例以 move(90) 实现静止;不同舵机的死区范围与转速-脉宽关系可能不同,[92, "ccw"]、[88, "cw"] 这类偏置值需要根据实际舵机微调,避免过慢或抖动。
  • REPL 交互:程序运行后,可交互式输入 mag.heading、mag.bearing、servo.cw() 等命令即时观察磁力计与舵机的实时状态,便于调试转向阈值与速度参数。

综合来看,magnetometer-log 示例是 johnny-five 中"传感器事件驱动执行器"这一经典模式的浓缩体现:磁力计通过 I2C 持续采样,框架层的 Compass 类负责把原始三轴数据换算成可读的航向与方位,再通过 headingchange 事件将变化通知给业务代码,而业务代码只需一行颜色查表与一次舵机转向调用,就能构建出完整的方位感知控制闭环。理解这条链路后,你完全可以将其中的颜色日志替换为 WebSocket 推送、数据库记录或更复杂的 PID 转向控制,实现自己的自动导航小车。

登录后查看全文
johnny-five