Lerobot -飞特舵机ST-3215双舵机联动(ESP_NOW)

前言

最近在做一个双舵机同步的小项目，用两块 ESP32-S3 做 MPU 控制器，一块当主机读取本地舵机位置，另一块当从机驱动另一个舵机跟随运动。中间用 ESP-NOW 做无线通信，延迟低、无需路由器、配对简单，非常适合这种点对点控制场景。

踩了几个坑，尤其是 ESP32-S3 获取 MAC 地址 这一块，网上很多老代码在 Core 3.x 上直接返回 00:00:00:00:00:00。这篇文章把完整的思路、代码和避坑点整理出来，方便后来人。

先看下ESP32的ESP_NOW基础代码

发送端：

cpp 复制代码

#include <WiFi.h>
#include <esp_now.h>

// ===== 接收端板子打印出来的MAC =====
uint8_t RECV_MAC[] = {0xE0,0x72,0xA1,0xD5,0x83,0x20};
// =========================================

esp_now_peer_info_t peer;

void setup()
{
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);

  if(esp_now_init() != ESP_OK){
    Serial.println("ESP‑NOW初始化失败");
    return;
  }

  memset(&peer,0,sizeof(peer));
  memcpy(peer.peer_addr, RECV_MAC,6);
  peer.channel = 1;
  peer.encrypt = false;

  esp_now_add_peer(&peer);
}

void loop()
{
  const char msg[] = "hello esp-now";
  esp_now_send(RECV_MAC, (uint8_t*)msg, sizeof(msg));
  delay(500);
}

接收端：

cpp 复制代码

#include <WiFi.h>
#include <esp_now.h>

// 回调函数格式
void onRecv(const esp_now_recv_info *recv_info, const uint8_t *data, int len)
{
  Serial.print("收到：");
  Serial.write(data, len);
  Serial.println();
}

void setup()
{
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);

  if(esp_now_init() != ESP_OK){
    Serial.println("ESP‑NOW初始化失败");
    return;
  }
  esp_now_register_recv_cb(onRecv);

  // 打印本机MAC，复制给发送端
  Serial.print("本机MAC: ");
  Serial.println(WiFi.macAddress());
}

void loop()
{

}

效果：

一、整体方案

主机：周期读取 ID=1 舵机当前位置 → 打包通过 ESP-NOW 发出
接线问题可以参考上篇文章Lerobot -飞特舵机ST-3215的ESP32调试-CSDN博客
从机：收到数据后写入 ID=2 舵机 → 实现位置跟随
通信周期 20ms，舵机运动速度 3400、加速度 50，运动平滑不抖

硬件：

2 × ESP32-S3 开发板
2 × 飞特 STS 系列舵机（ID1、ID2）
舵机串口：GPIO18 (RX)、GPIO19 (TX)，波特率 1000000

二、ESP32-S3 获取 MAC 地址（重点避坑）

踩坑写法

cpp 复制代码

#include <WiFi.h>
void setup(){
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  Serial.println(WiFi.macAddress());   // 输出 00:00:00:00:00:00
}

在 Arduino-ESP32 Core 3.x（我用的是 3.3.8） 上，这行会返回全 0。原因是 Core 3.0 起 WiFi.macAddress() 的行为变了------它返回的是网络接口的 MAC，而接口没启动时读不到。

正确写法：直接读 eFuse

cpp 复制代码

#include <WiFi.h>
#include "esp_mac.h"      // 关键头文件

void setup() {
  Serial.begin(115200);
  delay(1000);

  uint8_t mac[6];
  esp_err_t ret = esp_read_mac(mac, ESP_MAC_WIFI_STA);

  if (ret == ESP_OK) {
    Serial.printf("%02X:%02X:%02X:%02X:%02X:%02X\n",
                  mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  } else {
    Serial.printf("读取失败: %d\n", ret);
  }
}
void loop() {}

这个方式上电即可读，不依赖 WiFi 协议栈启动，最稳定。

编译报错 'ESP_MAC_WIFI_STA' was not declared？十有八九是漏了 #include "esp_mac.h"。

本次实测两块板子的 MAC：

主机：E0:72:A1:D6:B8:18
从机：E0:72:A1:D5:83:20

三、舵机中位校准

飞特舵机上电后需要先把当前物理位置 设为机械零点，否则后续 WritePosEx 的绝对位置没有意义。

cpp 复制代码

/*
将舵机当前物理位置设置为机械零点（中位）。
*/

#include <SCServo.h>

SMS_STS st;

// 舵机通信串口
// 默认引脚：GPIO18‑串口接收(S_RXD)，GPIO19‑串口发送(S_TXD)
#define S_RXD 18
#define S_TXD 19

void setup()
{
  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
  st.pSerial = &Serial1;
  delay(1000);   // 等待舵机上电稳定
}

void loop()
{
  st.CalibrationOfs(1);   // 将ID=1舵机当前的物理位置保存为零点（中位偏移校准）
  while(1);               // 死循环，程序停止在这里，不再往下运行
}

CalibrationOfs(id) 会把当前位置写入舵机的中位偏移。注意两块板子上舵机 ID 要区分开：主机读 ID1，从机写 ID2。校准的时候按实际接线改 ID 即可。

四、从机（接收端）代码

接收端（ID2）：

cpp 复制代码

#include <WiFi.h>
#include <esp_now.h>
#include <SCServo.h>

SMS_STS st;
#define S_RXD 18
#define S_TXD 19

// STS舵机位置最大值
#define SERVO_POS_MAX 4095

typedef struct {
  uint16_t pos;
} ServoData;
ServoData recvData;

void OnDataRecv(const esp_now_recv_info *info, const uint8_t *inData, int len) {
  if(len == sizeof(recvData)){
    memcpy(&recvData, inData, sizeof(recvData));

    // 超出舵机合法范围直接丢弃，过滤所有通信乱码
    if(recvData.pos > SERVO_POS_MAX)
    {
      Serial.print("过滤异常位置：");
      Serial.println(recvData.pos);
      return;
    }

    st.WritePosEx(2, recvData.pos, 3400, 50);
    Serial.print("收到目标位置：");
    Serial.println(recvData.pos);
  }
}

void setup() {
  Serial.begin(115200);
  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
  st.pSerial = &Serial1;
  delay(500);

  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP‑NOW初始化失败");
    return;
  }
  esp_now_register_recv_cb(OnDataRecv);

  Serial.println("接收端就绪，等待ESP‑NOW数据，舵机ID2跟随");
}

void loop() {
  delay(50);
}

关键点：

结构体两端必须完全一致，用 len == sizeof(recvData) 判长度
加一层 pos > 4095 过滤，避免偶发乱码把舵机推到极限
WritePosEx(id, pos, speed, acc)：速度 3400，加速度 50，运动柔和

五、主机（发送端）代码

发送端（ID1）：

cpp 复制代码

#include <WiFi.h>
#include <esp_now.h>
#include <SCServo.h>

SMS_STS st;
#define S_RXD 18
#define S_TXD 19

// ========== 接收端MAC地址 ==========
uint8_t RECV_MAC[] = {0xE0,0x72,0xA1,0xD5,0x83,0x20};
esp_now_peer_info_t peerInfo;

// 数据包结构体，传输舵机位置
typedef struct {
  uint16_t pos;
} ServoData;
ServoData sendData;

// 发送回调函数
void OnDataSent(const wifi_tx_info_t *tx_info, esp_now_send_status_t status)
{
  // 如需MAC：tx_info->mac_addr
  // Serial.print("Send status: ");
  // Serial.println(status == ESP_NOW_SEND_SUCCESS ? "OK" : "FAIL");
}

void setup() {
  Serial.begin(115200);
  // 初始化飞特舵机串口
  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
  st.pSerial = &Serial1;
  delay(500);

  // ESP-NOW初始化
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW 初始化失败");
    return;
  }
  esp_now_register_send_cb(OnDataSent);

  // 添加接收端设备
  memcpy(peerInfo.peer_addr, RECV_MAC, 6);
  peerInfo.channel = 1;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("添加peer失败");
    return;
  }
  Serial.println("发送端就绪，读取ID1舵机位置");
}

void loop() {
  // 读取ID=1舵机当前位置
  uint16_t curPos = st.ReadPos(1);
  sendData.pos = curPos;

  // 通过ESP-NOW发送给接收板
  esp_now_send(RECV_MAC, (uint8_t *)&sendData, sizeof(sendData));

  Serial.print("ID1舵机位置：");
  Serial.println(curPos);
  delay(20); // 20ms刷新
}

关键点：

peer.channel 两端必须一致（这里都是 1）
发送回调新 API 里第一个参数是 const wifi_tx_info_t *，不是老的 const uint8_t *mac_addr。网上老代码会编译不过，注意改。
接收回调新 API 里第一个参数是 const esp_now_recv_info *，不是老的 const uint8_t *mac_addr。同样注意。

六、实测效果

主机串口输出：

从机串口输出：

实机效果GIF:

手动转动主机上的 ID1 舵机，从机的 ID2 舵机基本同步跟随，延迟体感在几十毫秒内，满足一般演示和联动需求。

七、踩坑总结

坑	现象	解决
`WiFi.macAddress()` 全 0	Core 3.x 下读不到 MAC	用 `esp_read_mac(mac, ESP_MAC_WIFI_STA)`
编译报错 `ESP_MAC_WIFI_STA` 未声明	缺头文件	`#include "esp_mac.h"`
回调函数签名不匹配	旧代码编译不过	换成 `esp_now_recv_info` / `wifi_tx_info_t`
舵机乱动	通信乱码被写入	加 `pos > 4095` 过滤
舵机不同步	通道设置不一致	两端 `peer.channel` 保持一致
舵机上电位置不对	没做中位校准	先跑 `CalibrationOfs(id)`

八、小结

ESP-NOW 做点对点舵机同步，代码量小、延迟低，非常适合双板协作的场景。ESP32-S3 上获取 MAC 地址一定要用 esp_read_mac 而不是老教程里的 WiFi.macAddress()，否则会白白浪费半小时找问题。

完整工程可以直接参考上面的代码，把两端的 MAC 和舵机 ID 按实际情况改一下就能跑。如果后续要做多从机广播，把 esp_now_send 的第一个参数换成 NULL（广播地址），再在从机过滤来源 MAC 即可。

有问题欢迎评论区交流 👍