查看: 8|回复: 0

[项目] 双ESP32无线协作：打造360°激光扫描避障小车

本帖最后由云天于 2026-3-4 22:25 编辑

   让你的小车拥有“眼睛”，通过旋转扫描感知四周，自主决策避开障碍。

【项目简介】

   大家好！这次我制作了一台基于 “TOF激光测距传感器” 的智能避障小车。它使用两块ESP32开发板——“FireBeetle ESP32-E” 负责采集距离数据，“Romeo ESP32-S3” 负责电机控制和决策。为了让感知范围覆盖360°，我让传感器安装在28BYJ-48步进电机驱动模块上，在行进中不断旋转扫描，实时检测四个方向（前、右、后、左）的障碍距离，然后根据距离阈值自动前进、转向或后退。

【项目的亮点】
   50米高精度TOF激光测距：采用新一代dTOF（直接飞行时间）技术，测距范围 0.05~50米，精度高达 ±3cm，抗环境光干扰能力达100K LUX，为小车提供精准、稳定的障碍物感知，无论室内外都能可靠工作。
   无线通信：采用 **ESP-NOW** 协议，无需Wi-Fi路由器，两块ESP32直接交换数据。
   持续扫描：28BYJ-48步进电机驱动模块步进电机单向旋转，每90°采集一次数据，保证数据实时更新。
   可校准：上电后可通过串口命令校准车头方向，方便安装调试。
   开源代码：Arduino IDE编写，注释清晰，易于二次开发。

【硬件清单】

组件	型号	数量	备注
主控板（接收端）	Romeo ESP32-S3 (DFR0994)	1	四路电机驱动、ESP32-S3核心
传感器板（发送端）	FireBeetle ESP32-E	1	低功耗、I2C接口
TOF激光测距传感器	SEN0648 (50m)	1	采用IIC模式，测距精度±3cm
步进电机驱动模块	28BYJ-48步进电机驱动模块 (DFR1199)	1	STEP/DIR控制，5V供电
直流减速电机	N20 微型电机	4	带轮子，用于四轮驱动
电源	7.4V 锂电池组	2	给Romeo供电，为电机及步进电机供电
车架	亚克力车架		根据车架自行准备

（TOF激光测距传感器与28BYJ-48步进电机驱动模块）

“注意”：TOF传感器需预先通过上位机配置为 “IIC模式”，并记住设置的ID（假设ID=0，则IIC地址为0x08）。

【硬件接线】

FireBeetle ESP32-E ↔ TOF传感器 (SEN0648)

TOF传感器	FireBeetle ESP32-E
VCC (黑线)	5V
GND (红线)	GND
SDA	GPIO21
SCL	GPIO22

（FireBeetle默认I2C引脚为21/22，可根据实际修改）

Romeo ESP32-S3 ↔ 步进电机驱动模块 (DFR1199)

驱动模块	Romeo ESP32-S3
STEP	GPIO4
DIR	GPIO5
5V	5V (可从Romeo板载5V取电)
GND	GND

Romeo ESP32-S3 ↔ 四个N20直流电机
采用PH/EN控制模式，EN引脚输出PWM，PH引脚控制方向。参考下表（可根据实际车轮布局调整）：

电机	EN引脚	PH引脚	对应车轮
M1	GPIO12	GPIO13	右前
M2	GPIO14	GPIO21	左前
M3	GPIO9	GPIO10	右后
M4	GPIO47	GPIO11	左后

   电源连接
   Romeo的 “VIN” 或 “VM” 输入7~12V（我用了7.4V锂电池）。
   步进电机驱动模块的5V可从Romeo的5V输出引脚取电（注意总电流不超过2A）。
   FireBeetle的5V也可从Romeo的5V输出取电，方便共地。
【硬件组装】
   1.组装车底座与供电电池

2.组装主控板Romeo ESP32-S3

3.组装8BYJ-48步进电机驱动模块

4.组装TOF激光测距传感器

5.“雷达”车

【工作原理】

   整体流程如下图所示：

   1. “FireBeetle端”：上电后持续通过IIC读取TOF传感器的距离数据（每秒约5次），并通过ESP-NOW广播。
   2. “Romeo端”：
   上电后等待用户通过串口发送 `c` 命令校准方向（将当前传感器指向设为“前方”）。
   进入**扫描模式**：步进电机以恒定速度单向（顺时针）旋转，每转过90°，等待接收FireBeetle发来的最新距离数据，并存入对应的方向数组（索引0:前, 1:右, 2:后, 3:左）。
   同时小车以低速（20%速度）前进，实现“边前进边扫描”。
   每完成一圈（四个方向都采集一次），调用决策函数：
            若前方距离 > 1.5米 → 继续前进。
            否则，比较左右方向距离，转向较空旷的一侧（原地旋转90°）。
            若左右也受阻，但后方 > 1.5米 → 后退3秒。
            全部受阻 → 随机转向90°。
   动作完成后，回到扫描模式，循环执行。

【软件实现Arduino IDE】

FireBeetle 发送端代码

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

// ==================== TOF 传感器相关 ====================
#define deviceaddress 0x08

typedef struct {
  uint8_t id;
  uint8_t interface_mode;
  uint32_t uart_baudrate;
  uint32_t system_time;
  float dis;
  uint16_t dis_status;
  uint16_t signal_strength;
  uint8_t range_precision;
} tof_parameter;

tof_parameter tof0;

int16_t i2c_readN(uint8_t registerAddress, uint8_t *buf, size_t len) {
  Wire.beginTransmission(deviceaddress);
  Wire.write(registerAddress);
  if (Wire.endTransmission(false) != 0) return -1;
  Wire.requestFrom(deviceaddress, len);
  int i = 0;
  while (Wire.available() && i < len) buf[i++] = Wire.read();
  return i;
}

bool recdData() {
  uint8_t pdata[48];
  if (i2c_readN(0x00, pdata, 32) != 32) return false;
  if (i2c_readN(0x20, &pdata[32], 16) != 16) return false;

  tof0.interface_mode = pdata[0x0c] & 0x07;
  tof0.id = pdata[0x0d];
  tof0.uart_baudrate = (uint32_t)(pdata[0x10] | (pdata[0x11] << 8) | (pdata[0x12] << 16) | (pdata[0x13] << 24));
  tof0.system_time   = (uint32_t)(pdata[0x20] | (pdata[0x21] << 8) | (pdata[0x22] << 16) | (pdata[0x23] << 24));
  tof0.dis           = (float)((uint32_t)(pdata[0x24] | (pdata[0x25] << 8) | (pdata[0x26] << 16) | (pdata[0x27] << 24))) / 1000.0f;
  tof0.dis_status    = (uint16_t)(pdata[0x28] | (pdata[0x29] << 8));
  tof0.signal_strength = (uint16_t)(pdata[0x2a] | (pdata[0x2b] << 8));
  tof0.range_precision = pdata[0x2c];
  return true;
}

// ==================== ESP-NOW ====================
typedef struct {
  float distance;
  uint16_t status;
  uint16_t signal;
} sensor_data_t;

sensor_data_t sendData;

// Romeo ESP32-S3 MAC 地址
uint8_t romeoMac[] = {0xEC, 0xDA, 0x3B, 0x65, 0xD2, 0xA0};

esp_now_peer_info_t peerInfo;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);   // 根据实际接线修改 SDA, SCL

  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init failed");
    return;
  }
  esp_now_register_send_cb(OnDataSent);
  memcpy(peerInfo.peer_addr, romeoMac, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Add peer failed");
    return;
  }
  Serial.println("FireBeetle TOF Sender Ready");
}

void loop() {
  if (recdData()) {
    sendData.distance = tof0.dis;
    sendData.status = tof0.dis_status;
    sendData.signal = tof0.signal_strength;
    esp_now_send(romeoMac, (uint8_t*)&sendData, sizeof(sendData));
    Serial.printf("Sent: %.2f m, status=%d\n", tof0.dis, tof0.dis_status);
  } else {
    Serial.println("Sensor read failed");
  }
  delay(200);
}
复制代码

Romeo 接收端代码

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

// ==================== 引脚定义 ====================
#define STEP_PIN  4
#define DIR_PIN   5

#define M_RF_EN   12
#define M_RF_PH   13
#define M_LF_EN   14
#define M_LF_PH   21
#define M_RR_EN   9
#define M_RR_PH   10
#define M_LR_EN   47
#define M_LR_PH   11

// ==================== 步进电机参数 ====================
const int STEPS_PER_REV = 2048;          // 实测一圈步数
const float STEP_ANGLE = 360.0 / STEPS_PER_REV; // 每步角度 ≈ 0.17578°

// ==================== 扫描参数 ====================
const float SCAN_STEP = 90.0;             // 每次转动角度（90°）
const int NUM_DIRECTIONS = 4;              // 方向数量
float distances[NUM_DIRECTIONS];           // 存储四个方向的距离（索引0:前, 1:右, 2:后, 3:左）
float currentAngle = 0.0;                  // 当前TOF指向的角度（相对于车头前方，0°=前，顺时针增加）
int scanCount = 0;                         // 记录当前圈内已采集的方向数（0~3）
bool scanning = true;                       // 是否正在扫描（true:扫描中, false:执行动作中）
unsigned long actionStartTime = 0;          // 动作开始时间

// 安全阈值（米）
const float SAFE_DIST = 1.5;

// ==================== ESP-NOW ====================
typedef struct {
  float distance;
  uint16_t status;
  uint16_t signal;
} sensor_data_t;

sensor_data_t receivedData;
bool newDataAvailable = false;

// FireBeetle MAC 地址
uint8_t firebeetleMac[] = {0x7C, 0x9E, 0xBD, 0xD3, 0x63, 0x40};

// ==================== 电机 PWM ====================
const int pwmFreq = 2000;
const int pwmResolution = 8;
const int pwmChannels[4] = {0, 1, 2, 3};
const int enPins[4] = {M_RF_EN, M_LF_EN, M_RR_EN, M_LR_EN};
const int phPins[4] = {M_RF_PH, M_LF_PH, M_RR_PH, M_LR_PH};
const int SCAN_SPEED = 10;   // 扫描时的前进速度（0-100）
// ==================== 校准相关 ====================
long headingOffsetSteps = 0;      // 车身正方向对应的绝对步数偏移
long currentStepPosition = 0;     // 当前电机所处的绝对步数（相对于上电时的位置）
bool calibrated = false;

// ==================== 函数声明 ====================
void stepperRotateRelative(float deltaAngle);
void setMotor(int idx, int speedPercent, bool forward);
void moveForward(int speed);
void moveBackward(int speed);
void turnLeft(int speed, bool pivot);
void turnRight(int speed, bool pivot);
void stopMotors();
void checkSerialCommand();
void analyzeAndAct();

// ==================== ESP-NOW 回调 ====================
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
  memcpy(&receivedData, incomingData, sizeof(receivedData));
  newDataAvailable = true;
}

// ==================== 初始化 ====================
void setup() {
  Serial.begin(115200);
  Serial.println("Send 'c' to calibrate forward direction (0°).");

  // 电机引脚初始化
  for (int i = 0; i < 4; i++) {
    pinMode(enPins[i], OUTPUT);
    pinMode(phPins[i], OUTPUT);
    digitalWrite(phPins[i], LOW);
    digitalWrite(enPins[i], LOW);
  }
  for (int i = 0; i < 4; i++) {
    ledcSetup(pwmChannels[i], pwmFreq, pwmResolution);
    ledcAttachPin(enPins[i], pwmChannels[i]);
  }

  // 步进电机引脚
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(STEP_PIN, LOW);
  digitalWrite(DIR_PIN, LOW);

  // Wi-Fi 和 ESP-NOW
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init failed");
    return;
  }
  esp_now_register_recv_cb(OnDataRecv);

  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, firebeetleMac, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Add peer failed (may already exist)");
  }

  Serial.println("Romeo Controller Ready");
  delay(10000);
}

// ==================== 主循环 ====================
void loop() {
  checkSerialCommand();  // 监听串口校准命令

  if (scanning) {
    // 扫描时小车以恒定速度前进
    moveForward(SCAN_SPEED);

    // 1. 相对转动 SCAN_STEP 度（持续顺时针）
    stepperRotateRelative(SCAN_STEP);

    // 2. 等待接收数据（最多 500ms）
    unsigned long startWait = millis();
    while (!newDataAvailable && (millis() - startWait < 500)) {
      delay(10);
    }

    // 3. 根据当前角度计算理论方向索引（0前,1右,2后,3左）
    int rawIndex = (int)(currentAngle / SCAN_STEP + 0.5) % NUM_DIRECTIONS;
    // 由于电机实际转向可能与预期相反，交换左右索引（1和3）
    int dirIndex = rawIndex;
    if (rawIndex == 1) dirIndex = 3;
    else if (rawIndex == 3) dirIndex = 1;
    // 前(0)和后(2)保持不变

    // 4. 记录距离
    if (newDataAvailable) {
      newDataAvailable = false;
      distances[dirIndex] = receivedData.distance;
      Serial.printf("Angle %.0f° -> Dir %d (raw %d): %.2f m\n", currentAngle, dirIndex, rawIndex, receivedData.distance);
    } else {
      // 超时，假设无障碍（设为较大值）
      distances[dirIndex] = 10.0;
      Serial.printf("Angle %.0f° -> Dir %d (raw %d): timeout, assume clear\n", currentAngle, dirIndex, rawIndex);
    }

    scanCount++;
    if (scanCount >= NUM_DIRECTIONS) {
      // 完成一圈扫描
      scanning = false;
      scanCount = 0;
      Serial.println("One full circle completed. Analyzing...");
      analyzeAndAct();
    }
  } else {
    // 非扫描状态：执行动作（移动中），等待动作完成
    if (millis() - actionStartTime > 3000) {
      scanning = true;
      stopMotors(); // 可选，停止电机准备下一轮扫描
    }
  }
}

// ==================== 数据分析与动作执行 ====================
void analyzeAndAct() {
  // 打印四个方向距离
  Serial.print("Front: "); Serial.print(distances[0]); Serial.print(" m, ");
  Serial.print("Right: "); Serial.print(distances[1]); Serial.print(" m, ");
  Serial.print("Back: "); Serial.print(distances[2]); Serial.print(" m, ");
  Serial.print("Left: "); Serial.println(distances[3]); Serial.print(" m");

  // 决策逻辑：
  // 1. 如果前方距离 > 安全阈值，前进
  if (distances[0] > SAFE_DIST) {
    Serial.println("Action: Move Forward");
    moveForward(10);
    actionStartTime = millis();
  }
  // 2. 否则，检查左右哪个更远，就转向哪个方向
  else if (distances[1] > distances[3] && distances[1] > SAFE_DIST) {
    Serial.println("Action: Turn Right");
    turnRight(20, true);   // 原地右转
    delay(2000);
    stopMotors();
    actionStartTime = millis();
  }
  else if (distances[3] > distances[1] && distances[3] > SAFE_DIST) {
    Serial.println("Action: Turn Left");
    turnLeft(20, true);
    delay(2000);
    stopMotors();
    actionStartTime = millis();
  }
  // 3. 如果左右都不行，但后方可行，后退
  else if (distances[2] > SAFE_DIST) {
    Serial.println("Action: Move Backward");
    moveBackward(15);
    actionStartTime = millis();
  }
  // 4. 所有方向都危险，原地旋转随机方向
  else {
    Serial.println("Action: All blocked, spin randomly");
    if (random(0, 2) == 0) turnLeft(20, true);
    else turnRight(20, true);
    delay(2000);
    stopMotors();
    actionStartTime = millis();
  }
}

// ==================== 串口命令处理 ====================
void checkSerialCommand() {
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'c' || c == 'C') {
      headingOffsetSteps = currentStepPosition;
      currentStepPosition = 0;
      currentAngle = 0.0;
      calibrated = true;
      Serial.println("Calibration OK! Current direction is now FORWARD (0°).");
    }
  }
}

// ==================== 步进电机相对转动 ====================
void stepperRotateRelative(float deltaAngle) {
  long deltaSteps = round(deltaAngle / STEP_ANGLE);
  if (deltaSteps == 0) return;

  bool clockwise = (deltaSteps > 0);  // 正角度为顺时针
  long stepsToMove = abs(deltaSteps);

  digitalWrite(DIR_PIN, clockwise ? HIGH : LOW);
  for (long i = 0; i < stepsToMove; i++) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(1000);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(1000);
  }

  // 更新当前位置和角度
  if (clockwise) {
    currentStepPosition += stepsToMove;
    currentAngle += deltaAngle;
  } else {
    currentStepPosition -= stepsToMove;
    currentAngle -= deltaAngle;
  }

  // 归一化到 0~360 度
  while (currentAngle >= 360.0) currentAngle -= 360.0;
  while (currentAngle < 0.0) currentAngle += 360.0;
}

// ==================== 直流电机控制 ====================
void setMotor(int idx, int speedPercent, bool forward) {
  if (idx < 0 || idx >= 4) return;
  speedPercent = constrain(speedPercent, 0, 100);
  int pwmVal = map(speedPercent, 0, 100, 0, 255);
  digitalWrite(phPins[idx], forward ? HIGH : LOW);
  ledcWrite(pwmChannels[idx], pwmVal);
}

void moveForward(int speed) {
  for (int i = 0; i < 4; i++) setMotor(i, speed, true);
}
void moveBackward(int speed) {
  for (int i = 0; i < 4; i++) setMotor(i, speed, false);
}
void turnLeft(int speed, bool pivot) {
  if (pivot) {
    setMotor(0, speed, true);   // 右前
    setMotor(1, speed, false);  // 左前
    setMotor(2, speed, true);   // 右后
    setMotor(3, speed, false);  // 左后
  } else {
    setMotor(0, speed, true);
    setMotor(1, speed / 2, true);
    setMotor(2, speed, true);
    setMotor(3, speed / 2, true);
  }
}
void turnRight(int speed, bool pivot) {
  if (pivot) {
    setMotor(0, speed, false);  // 右前
    setMotor(1, speed, true);   // 左前
    setMotor(2, speed, false);  // 右后
    setMotor(3, speed, true);   // 左后
  } else {
    setMotor(0, speed / 2, true);
    setMotor(1, speed, true);
    setMotor(2, speed / 2, true);
    setMotor(3, speed, true);
  }
}
void stopMotors() {
  for (int i = 0; i < 4; i++) ledcWrite(pwmChannels[i], 0);
}
复制代码

【调试过程与踩坑记录】

   步进电机一圈步数实测
   按照28BYJ-48的减速比1/64，理论上输出轴一圈需4096步。但实际运行时，发现转动90°只转了180°。经过排查，发现驱动模块工作在**全步/双拍模式**，实际一圈只需2048步。因此代码中将 `STEPS_PER_REV` 设为2048，问题解决。

   左右方向相反
   当传感器转到右侧时，程序却显示“左”方向距离。原因可能是步进电机实际转向与预期相反。通过软件映射：在 `loop` 中计算 `rawIndex` 后，交换索引1和3（即 `if(rawIdx==1) dirIdx=3; else if(rawIdx==3) dirIdx=1;`），轻松修正。

   转向延时确定
   为了精确原地旋转90°，我通过实验调节了 `turnRight` 后的 `delay` 时间。最终发现速度15%下，延时2000ms正好转90°。这个参数与地面摩擦、电池电压有关，可根据实际情况微调。

   前进时扫描
   最初扫描时小车静止，但我想让它在前进中感知环境。只需在 `if(scanning)` 开头加上 `moveForward(SCAN_SPEED);`，即可实现边前进边旋转扫描。

   ESP-NOW 添加对等设备失败
   虽然串口打印“Add peer failed”，但通信正常。这是因为之前可能已经添加过该设备，忽略即可。

【最终效果】

   经过调试，小车能在室内环境中自主导航：
            当前方1米内无障碍时，保持直行；
            遇到障碍时，扫描左右，选择较空旷的方向转向；
            若四面受阻，则随机转向或后退；
            整个过程中，步进电机持续旋转，数据实时更新。

【总结与拓展】

   本项目展示了如何利用两块ESP32通过ESP-NOW协同工作，实现一个功能完整的激光扫描避障小车。你可以在此基础上进行更多拓展：

   “增加扫描分辨率”：将 `SCAN_STEP` 改为45°或30°，获取更精细的环境信息。
   “加入PID速度控制”：让前进和转向更平滑。
   “远程监控”：通过Wi-Fi将距离数据及摄像头图像发送到手机或电脑。
   “路径规划”：结合多个方向数据，实现沿墙走或探索算法。

   希望这个项目能给你带来启发！如有任何问题，欢迎在评论区交流讨论。

使用道具举报