Arduino BLDC 基于串口指令的远程控制工业巡检机器人

Arduino BLDC 基于串口指令的远程控制工业巡检机器人

基于 Arduino 的 BLDC 串口指令远程控制工业巡检机器人，是一种将嵌入式控制、高效驱动与可靠通信技术深度融合的工业自动化解决方案。该系统以 Arduino 为核心控制器，驱动 BLDC 电机实现高机动性移动，通过串口通信链路接收上位机或远程终端的指令，实现对机器人的精确操控与状态监控。

1. 主要特点

高可靠性的串口通信架构 串口通信（UART）作为工业控制领域的基石，提供了稳定、低延迟的指令传输通道。

• 协议灵活性：系统可定义自定义的二进制或 ASCII 协议。例如，通过发送字符指令（如 "F" 前进，"B" 后退，"L" 左转，"R" 右转）或结构化数据包（包含速度、方向、任务 ID 等字段），实现复杂的控制逻辑。
• 硬件接口多样性：物理层可采用标准 TTL 电平、RS232 或 RS485。其中，RS485 支持多点、长距离（可达 1200 米）差分传输，具有极强的抗共模干扰能力，非常适合工业现场复杂的电磁环境。
• 低延迟实时控制：相较于网络协议栈，串口通信的协议开销极小，数据传输延迟通常在微秒级。这使得操作员能够实时控制机器人的运动状态，快速响应突发情况。

BLDC 驱动的高机动性与长续航 BLDC 电机为巡检机器人提供了适应复杂工业环境的动力保障。

• 高能效比：BLDC 电机效率通常高达 85%-90%，配合电子调速器（ESC），在频繁启停的巡检任务中能显著降低能耗，延长单次充电的巡检时长。
• 宽调速范围与高动态响应：通过 PWM 信号精确控制 BLDC 电机，机器人可实现从极低速（用于精细检查设备仪表）到高速（用于快速转移阵地）的平滑调速。电机的快速扭矩响应特性确保了机器人在接收到急停或变向指令时能迅速执行。
• 低维护与高可靠性：无电刷设计消除了机械磨损和火花干扰，使驱动系统能在粉尘、油污等恶劣工业环境中长期稳定运行，减少了停机维护成本。

模块化与可扩展性 Arduino 的开源生态为系统功能的扩展提供了极大便利。

• 多传感器集成：通过串口、I2C 或 SPI 总线，可轻松集成红外热像仪、气体传感器、高清摄像头等多种巡检传感器。Arduino 负责协调这些传感器的数据采集，并通过串口回传给上位机。
• 分级控制架构：通常采用'上位机规划 + Arduino 执行'的模式。上位机负责复杂的路径规划、图像处理和数据分析，Arduino 则专注于底层的电机控制、传感器数据读取和紧急避障，实现了计算资源的合理分配。

2. 应用场景

• 电力与石化设施巡检：在变电站、炼油厂等高危环境中，机器人代替人工进入现场，通过串口接收远程指令，对变压器、管道、阀门等设备进行巡检。操作员可在控制室实时查看机器人回传的红外热图、气体浓度数据和视频画面，及时发现漏电、漏油、过热等隐患。
• 轨道交通与隧道检测：在地铁隧道、铁路涵洞等狭长、封闭的空间内，机器人沿轨道或预设路径行进，利用串口链路与基站保持通信。通过远程控制，操作员可指挥机器人对轨道状态、隧道壁结构进行详细检查，解决了人工巡检视野受限、效率低下的问题。
• 大型仓储与数据中心：在面积广阔的仓库或数据中心机房，机器人执行环境监测任务。通过串口指令，系统可调度机器人前往指定区域检测温湿度、烟雾浓度，或对服务器机柜的指示灯状态进行视觉识别，实现无人化、全天候的智能巡检。
• 应急救援与排爆：在发生事故或存在爆炸风险的现场，机器人作为侦查先锋，通过有线或无线串口（如数传电台）接收远程指令。其强大的越障能力和灵活的运动性能，使其能够深入复杂废墟，回传现场音视频信息，为救援决策提供依据。

3. 注意事项

通信稳定性与抗干扰

• 硬件隔离与保护：工业现场存在强烈的电磁干扰和浪涌电压。必须在串口通信线路中加入光耦隔离、磁耦隔离或 RS485 收发器（如 MAX485）的保护电路，防止干扰信号或高压尖峰损坏 Arduino 和上位机。
• 数据校验与重传机制：在软件层面，必须设计完善的数据包结构，包含帧头、地址、指令、数据、校验和（如 CRC16）及帧尾。若接收方校验失败，应丢弃数据包并请求重传，确保指令的准确无误。

电源管理与系统分区

• 强弱电分离：BLDC 电机驱动属于强电回路，其启动和换向会产生巨大的电流冲击和电压波动。必须使用独立的 DC-DC 模块为 Arduino 及其逻辑电路供电，严禁与电机共用电源路径，防止电机噪声导致单片机复位。
• 电源滤波：在电源输入端和逻辑电路端并联大容量电解电容和高频陶瓷电容，以吸收电压尖峰，平滑电源噪声。

安全机制与故障处理

• 看门狗与超时保护：必须启用 Arduino 的硬件看门狗定时器。同时，在程序中设置通信超时机制，若在设定时间内未接收到上位机的'心跳包'或有效指令，系统应自动进入安全模式（如停止电机、启动声光报警）。
• 急停与限位保护：物理急停按钮是必须的硬件安全措施。此外，还应设置软件限位，当传感器检测到障碍物过近或电机电流异常（可能卡死）时，立即切断电机动力。

协议设计与状态同步

• 指令冲突处理：需设计合理的协议优先级。例如，急停指令应具有最高优先级，任何时刻接收到急停指令都必须立即响应，而速度调节指令则应平滑过渡。
• 双向通信与状态反馈：远程控制不应是单向的。Arduino 应定期或在事件触发时向上位机回传机器人的状态信息（如当前坐标、电池电压、电机温度、传感器读数），以便操作员全面掌握现场情况，实现闭环控制。

4. 代码示例

4.1 基础串口速度控制（PWM 信号）

#include <Servo.h>

Servo esc; // 电调控制对象
String inputCommand;
int currentSpeed = 1000; // 默认低脉冲（停止）

void setup() {
  Serial.begin(115200);
  esc.attach(9, 1000, 2000); // 信号线接 9 号引脚
  esc.writeMicroseconds(currentSpeed); // 初始化电调
  Serial.println("BLDC Control Ready (PWM Mode)");
  Serial.println("Commands: SPEED:<1000-2000> | STOP");
}

void loop() {
  if (Serial.available() > 0) {
    inputCommand = Serial.readStringUntil('\n');
    inputCommand.trim();
    if (inputCommand.startsWith("SPEED:")) {
      currentSpeed = inputCommand.substring(6).toInt();
      currentSpeed = constrain(currentSpeed, 1000, 2000); // 限制范围
      esc.writeMicroseconds(currentSpeed);
      Serial.print("Speed set to: ");
      Serial.println(currentSpeed);
    } else if (inputCommand == "STOP") {
      currentSpeed = 1000;
      esc.writeMicroseconds(currentSpeed);
      Serial.println("Emergency Stop");
    }
  }
}

应用场景：通过上位机（如 Python 脚本）发送串口指令控制机器人电机启停和调速。

4.2 多电机协同控制（差速转向）

#include <Servo.h>

Servo escLeft, escRight; // 左右电机电调
String command;

void setup() {
  Serial.begin(115200);
  escLeft.attach(9, 1000, 2000);
  escRight.attach(10, 1000, 2000);
  Serial.println("Differential Drive Ready");
  Serial.println("Commands: FORWARD:<speed> | TURN:<left_speed>:<right_speed> | STOP");
}

void loop() {
  if (Serial.available()) {
    command = Serial.readStringUntil('\n');
    command.trim();
    if (command.startsWith("FORWARD:")) {
      int speed = command.substring(8).toInt();
      speed = constrain(speed, 1000, 2000);
      escLeft.writeMicroseconds(speed);
      escRight.writeMicroseconds(speed);
      Serial.print("Moving forward at: ");
      Serial.println(speed);
    } else if (command.startsWith("TURN:")) {
      int colonIndex = command.indexOf(':', 5);
      int leftSpeed = command.substring(5, colonIndex).toInt();
      int rightSpeed = command.substring(colonIndex + 1).toInt();
      escLeft.writeMicroseconds(constrain(leftSpeed, 1000, 2000));
      escRight.writeMicroseconds(constrain(rightSpeed, 1000, 2000));
      Serial.print("Turning - Left: ");
      Serial.print(leftSpeed);
      Serial.print(" | Right: ");
      Serial.println(rightSpeed);
    } else if (command == "STOP") {
      escLeft.writeMicroseconds(1000);
      escRight.writeMicroseconds(1000);
      Serial.println("Stopped");
    }
  }
}

应用场景：差速驱动机器人（如巡检小车），通过串口指令实现直行、转向和停止。

4.3 带反馈的闭环控制（PID 调速）

#include <Servo.h>
#include <PID_v1.h> // 需要安装 PID 库

Servo esc;
int encoderPin = 2; // 编码器信号输入（需中断引脚）
volatile long pulseCount = 0;
double targetRPM = 100, actualRPM = 0;
double outputPWM = 1000;
PID myPID(&actualRPM, &outputPWM, &targetRPM, 0.5, 0.1, 0.01, DIRECT); // PID 参数需调试

void countPulses() { pulseCount++; } // 编码器中断函数

void setup() {
  Serial.begin(115200);
  esc.attach(9, 1000, 2000);
  pinMode(encoderPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoderPin), countPulses, RISING);
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(1000, 2000); // 限制 PWM 范围
  Serial.println("Closed-Loop Control Ready");
}

void loop() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= 100) { // 每 100ms 计算一次转速
    noInterrupts();
    long currentPulses = pulseCount;
    pulseCount = 0;
    interrupts();
    actualRPM = (currentPulses / 20.0) * 60; // 假设编码器每转 20 脉冲
    myPID.Compute(); // 更新 PID 输出
    esc.writeMicroseconds(outputPWM);
    Serial.print("Target: ");
    Serial.print(targetRPM);
    Serial.print(" | Actual: ");
    Serial.print(actualRPM);
    Serial.print(" | PWM: ");
    Serial.println(outputPWM);
    lastTime = millis();
  }
  // 串口接收新目标转速
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    if (cmd.startsWith("RPM:")) {
      targetRPM = cmd.substring(4).toDouble();
      Serial.print("New RPM Target: ");
      Serial.println(targetRPM);
    }
  }
}

应用场景：需要精确控制轮速的巡检机器人（如爬坡时保持恒定速度），通过编码器反馈实现 PID 闭环控制。

4.4 基础串口指令控制巡检机器人

场景：室内简单巡检，通过串口接收控制指令。核心逻辑：ASCII 指令解析 + 差速控制。

#include <SimpleFOC.h>

// BLDC 电机对象
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);

// 编码器
Encoder encoderL(2, 3, 2048);
Encoder encoderR(18, 19, 2048);

void doA_L() { encoderL.handleA(); }
void doB_L() { encoderL.handleB(); }
void doA_R() { encoderR.handleA(); }
void doB_R() { encoderR.handleB(); }

// 控制参数
float linearSpeed = 0.0; // 线速度 m/s
float angularSpeed = 0.0; // 角速度 rad/s
float maxSpeed = 1.0; // 最大速度 1m/s
float wheelSeparation = 0.3; // 轮距 0.3m
float wheelRadius = 0.05; // 轮半径 0.05m

// 指令结构
struct Command {
  char type; // 指令类型：M=移动，S=停止，C=配置，R=读取
  float param1; // 参数 1
  float param2; // 参数 2
  unsigned long timestamp; // 时间戳
};
Command currentCmd = {'S', 0, 0, 0};
bool newCommand = false;
String serialBuffer = "";

// 机器人状态
struct RobotState {
  float posX = 0.0, posY = 0.0; // 位置
  float heading = 0.0; // 航向
  float battery = 12.6; // 电池电压
  float temp = 25.0; // 温度
  unsigned long uptime = 0; // 运行时间
};
RobotState state;

// 超声波传感器
#define SONAR_TRIG 9
#define SONAR_ECHO 10
float obstacleDistance = 0.0;

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200); // 用于扩展通信
  // 初始化编码器
  encoderL.init();
  encoderR.init();
  encoderL.enableInterrupts(doA_L, doB_L);
  encoderR.enableInterrupts(doA_R, doB_R);
  // 链接传感器
  motorL.linkSensor(&encoderL);
  motorR.linkSensor(&encoderR);
  // 初始化电机
  motorL.init();
  motorR.init();
  motorL.initFOC();
  motorR.initFOC();
  // 初始化超声波
  pinMode(SONAR_TRIG, OUTPUT);
  pinMode(SONAR_ECHO, INPUT);
  // 发送启动消息
  Serial.println("INSPECT> Robot Ready");
  Serial.println("INSPECT> Commands: F-forward B-back L-left R-right S-stop");
  Serial.println("INSPECT> V0.5-speed0.5 T0.2-turn0.2 C-config");
}

void loop() {
  // 1. 读取串口指令
  readSerialCommands();
  // 2. 执行当前指令
  if (newCommand) {
    executeCommand(currentCmd);
    newCommand = false;
  }
  // 3. 运动控制
  motionControl();
  // 4. 更新状态
  updateRobotState();
  // 5. 障碍物检测
  checkObstacles();
  // 6. 执行 FOC 控制
  motorL.loopFOC();
  motorR.loopFOC();
  // 7. 定期状态报告
  static unsigned long lastReport = 0;
  if (millis() - lastReport > 1000) { // 1Hz 报告
    sendStatusReport();
    lastReport = millis();
  }
}

void readSerialCommands() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (serialBuffer.length() > 0) {
        parseCommand(serialBuffer);
        serialBuffer = "";
      }
    } else {
      serialBuffer += c;
    }
  }
}

void parseCommand(String buffer) {
  buffer.trim();
  buffer.toUpperCase();
  Command cmd = {'S', 0, 0, millis()};
  if (buffer.length() == 1) {
    // 单字符指令
    cmd.type = buffer[0];
    switch (buffer[0]) {
      case 'F': // 前进
        cmd.param1 = 0.3; // 默认速度 0.3m/s
        break;
      case 'B': // 后退
        cmd.param1 = -0.3;
        break;
      case 'L': // 左转
        cmd.type = 'T';
        cmd.param1 = 0.5; // 0.5rad/s
        break;
      case 'R': // 右转
        cmd.type = 'T';
        cmd.param1 = -0.5;
        break;
      case 'S': // 停止
        cmd.type = 'S';
        break;
      case '?': // 状态查询
        cmd.type = 'R';
        break;
    }
  } else if (buffer.startsWith("V")) {
    // 速度设置 V0.5
    cmd.type = 'M';
    buffer.remove(0, 1);
    cmd.param1 = buffer.toFloat();
  } else if (buffer.startsWith("T")) {
    // 转向设置 T0.2
    cmd.type = 'T';
    buffer.remove(0, 1);
    cmd.param1 = buffer.toFloat();
  } else if (buffer.startsWith("C")) {
    // 配置命令
    parseConfigCommand(buffer, cmd);
  } else if (buffer.startsWith("G")) {
    // 前往坐标 G1.5,2.0
    cmd.type = 'G';
    parseCoordinates(buffer, cmd);
  }
  currentCmd = cmd;
  newCommand = true; // 回显接收的指令
  Serial.print("INSPECT> Received: ");
  Serial.println(buffer);
}

void executeCommand(Command cmd) {
  switch (cmd.type) {
    case 'M': // 移动指令
      linearSpeed = constrain(cmd.param1, -maxSpeed, maxSpeed);
      angularSpeed = 0;
      Serial.print("INSPECT> Set speed: ");
      Serial.println(linearSpeed, 2);
      break;
    case 'T': // 转向指令
      angularSpeed = constrain(cmd.param1, -1.0, 1.0);
      linearSpeed = 0;
      Serial.print("INSPECT> Set turn: ");
      Serial.println(angularSpeed, 2);
      break;
    case 'S': // 停止
      linearSpeed = 0;
      angularSpeed = 0;
      Serial.println("INSPECT> Stopped");
      break;
    case 'R': // 读取状态
      sendDetailedStatus();
      break;
    case 'G': // 前往坐标
      navigateTo(cmd.param1, cmd.param2);
      break;
    case 'C': // 配置
      applyConfiguration(cmd);
      break;
  }
}

void motionControl() {
  // 差速运动学
  float leftSpeed = linearSpeed - (angularSpeed * wheelSeparation / 2.0);
  float rightSpeed = linearSpeed + (angularSpeed * wheelSeparation / 2.0);
  // 转换为电机速度 (rad/s)
  float leftMotorSpeed = leftSpeed / wheelRadius;
  float rightMotorSpeed = rightSpeed / wheelRadius;
  // 限制最大速度
  leftMotorSpeed = constrain(leftMotorSpeed, -20.0, 20.0);
  rightMotorSpeed = constrain(rightMotorSpeed, -20.0, 20.0);
  // 发送到电机
  motorL.move(leftMotorSpeed);
  motorR.move(rightMotorSpeed);
}

void updateRobotState() {
  // 更新里程计
  static float lastLeftPos = 0, lastRightPos = 0;
  float leftPos = motorL.shaft_angle;
  float rightPos = motorR.shaft_angle;
  float deltaLeft = leftPos - lastLeftPos;
  float deltaRight = rightPos - lastRightPos;
  // 计算位置变化
  float linearDelta = (deltaLeft + deltaRight) * wheelRadius / 2.0;
  float angularDelta = (deltaRight - deltaLeft) * wheelRadius / wheelSeparation;
  // 更新位置
  state.heading += angularDelta;
  state.posX += linearDelta * cos(state.heading);
  state.posY += linearDelta * sin(state.heading);
  lastLeftPos = leftPos;
  lastRightPos = rightPos;
  // 更新运行时间
  state.uptime = millis() / 1000;
  // 模拟读取电池和温度
  state.battery = 12.0 + random(0, 20) / 10.0;
  state.temp = 25.0 + random(0, 20) / 10.0;
}

void checkObstacles() {
  // 超声波测距
  digitalWrite(SONAR_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(SONAR_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(SONAR_TRIG, LOW);
  long duration = pulseIn(SONAR_ECHO, HIGH, 30000);
  obstacleDistance = duration * 0.034 / 2.0;
  // 障碍物检测
  if (obstacleDistance > 0 && obstacleDistance < 0.3) { // 30cm 内有障碍
    if (linearSpeed > 0) { // 前进时检测
      linearSpeed = 0;
      angularSpeed = 0.5; // 自动右转避开
      Serial.println("INSPECT> Obstacle detected! Auto avoiding");
    }
  }
}

void sendStatusReport() {
  Serial.print("INSPECT> STATUS X:");
  Serial.print(state.posX, 2);
  Serial.print(" Y:");
  Serial.print(state.posY, 2);
  Serial.print(" H:");
  Serial.print(state.heading, 2);
  Serial.print(" Bat:");
  Serial.print(state.battery, 1);
  Serial.print("V Temp:");
  Serial.print(state.temp, 1);
  Serial.print("C Obst:");
  Serial.print(obstacleDistance, 2);
  Serial.println("m");
}

4.5 工业 Modbus RTU 串口控制

场景：工业环境，需要标准 Modbus 协议与 PLC/DCS 集成。核心逻辑：Modbus RTU 协议 + 寄存器映射。

#include <SimpleFOC.h>
#include <ModbusRTU.h>

// Modbus 库
// BLDC 电机
BLDCMotor motorL, motorR;
// Modbus 从站
ModbusRTU mb;
#define MODBUS_SLAVE_ID 1
#define MODBUS_BAUDRATE 9600
#define MODBUS_SERIAL Serial2 // 硬件串口 2

// Modbus 寄存器映射
enum ModbusRegisters {
  // 保持寄存器 (可读可写)
  REG_SET_SPEED = 0, // 设置速度
  REG_SET_ANGLE,     // 设置角度
  REG_MAX_SPEED,     // 最大速度
  REG_ACCELERATION,  // 加速度
  REG_CONTROL_MODE,  // 控制模式
  REG_TARGET_X,      // 目标 X 坐标
  REG_TARGET_Y,      // 目标 Y 坐标
  // 输入寄存器 (只读)
  REG_ACTUAL_SPEED = 100, // 实际速度
  REG_ACTUAL_ANGLE,       // 实际角度
  REG_POSITION_X,         // 当前位置 X
  REG_POSITION_Y,         // 当前位置 Y
  REG_BATTERY_VOLTAGE,    // 电池电压
  REG_MOTOR_TEMP,         // 电机温度
  REG_OBSTACLE_DIST,      // 障碍物距离
  REG_ERROR_CODE,         // 错误代码
  REG_STATUS_WORD,        // 状态字
  REG_COUNT = 150
};

// 寄存器值
uint16_t mbRegisters[REG_COUNT] = {0};
uint16_t mbInputs[50] = {0};

// 机器人状态
struct IndustrialState {
  float speedActual = 0.0;
  float angleActual = 0.0;
  float posX = 0.0, posY = 0.0;
  float battery = 24.0;
  float temperature[2] = {25.0, 25.0};
  uint16_t errorCode = 0;
  uint16_t statusWord = 0x0001; // 初始状态：就绪
  bool emergencyStop = false;
};
IndustrialState iState;

// 控制模式
enum ControlMode { SPEED_CTRL, POSITION_CTRL, TRAJECTORY_CTRL };
ControlMode ctrlMode = SPEED_CTRL;

void setup() {
  Serial.begin(115200); // 调试串口
  MODBUS_SERIAL.begin(MODBUS_BAUDRATE, SERIAL_8N1);
  // 初始化 Modbus
  mb.begin(&MODBUS_SERIAL);
  mb.setBaudrate(MODBUS_BAUDRATE);
  mb.slave(MODBUS_SLAVE_ID);
  // 配置 Modbus 回调
  mb.addHreg(REG_SET_SPEED, 0, REG_COUNT);
  mb.onSet(HREG(0), writeRegisterCallback, REG_COUNT);
  mb.onGet(HREG(0), readRegisterCallback, REG_COUNT);
  // 初始化寄存器默认值
  initModbusRegisters();
  // 初始化电机
  initBLDCMotors();
  Serial.println("INSPECT> Modbus RTU Industrial Robot Ready");
  Serial.println("INSPECT> Slave ID: 1, Baud: 9600, 8N1");
}

void loop() {
  // 1. 处理 Modbus 请求
  mb.task();
  // 2. 读取 Modbus 命令
  processModbusCommands();
  // 3. 根据控制模式执行
  switch (ctrlMode) {
    case SPEED_CTRL:
      speedControl();
      break;
    case POSITION_CTRL:
      positionControl();
      break;
    case TRAJECTORY_CTRL:
      trajectoryControl();
      break;
  }
  // 4. 更新输入寄存器
  updateInputRegisters();
  // 5. 安全检查
  safetyMonitor();
  // 6. 执行电机控制
  motorL.loopFOC();
  motorR.loopFOC();
  // 7. 诊断输出
  static unsigned long lastDiag = 0;
  if (millis() - lastDiag > 1000) {
    sendDiagnosticInfo();
    lastDiag = millis();
  }
}

void initModbusRegisters() {
  // 初始化保持寄存器默认值
  // 速度相关 (单位：0.01 m/s)
  mb.Hreg(REG_SET_SPEED, 0); // 0 m/s
  mb.Hreg(REG_MAX_SPEED, 100); // 1.0 m/s
  mb.Hreg(REG_ACCELERATION, 50); // 0.5 m/s²
  // 位置相关 (单位：0.01 m)
  mb.Hreg(REG_TARGET_X, 0);
  mb.Hreg(REG_TARGET_Y, 0);
  // 控制模式
  mb.Hreg(REG_CONTROL_MODE, SPEED_CTRL);
  // 初始化输入寄存器
  for (int i = 0; i < 50; i++) {
    mb.Ireg(i, 0);
  }
}

bool writeRegisterCallback(Modbus::ResultCode event, uint16_t address, uint16_t val) {
  // Modbus 写寄存器回调
  if (event == Modbus::EX_SUCCESS) {
    Serial.print("MODBUS> Write reg ");
    Serial.print(address);
    Serial.print(" = ");
    Serial.println(val);
    // 根据寄存器地址处理
    switch (address) {
      case REG_SET_SPEED:
        handleSpeedCommand(val);
        break;
      case REG_CONTROL_MODE:
        ctrlMode = (ControlMode)constrain(val, 0, 2);
        Serial.print("MODBUS> Control mode: ");
        Serial.println(ctrlMode);
        break;
      case REG_TARGET_X:
      case REG_TARGET_Y:
        if (ctrlMode == POSITION_CTRL || ctrlMode == TRAJECTORY_CTRL) {
          handlePositionCommand();
        }
        break;
      default:
        // 直接更新寄存器值
        mb.Hreg(address, val);
    }
    return true;
  }
  return false;
}

void handleSpeedCommand(uint16_t speedReg) {
  // 处理速度命令
  float speedSetpoint = speedReg / 100.0; // 转换为 m/s
  // 应用加速度限制
  static float currentSpeed = 0.0;
  float maxAccel = mb.Hreg(REG_ACCELERATION) / 100.0;
  float maxSpeed = mb.Hreg(REG_MAX_SPEED) / 100.0;
  // 加速度限制
  if (speedSetpoint > currentSpeed) {
    currentSpeed = min(currentSpeed + maxAccel * 0.1, speedSetpoint);
  } else {
    currentSpeed = max(currentSpeed - maxAccel * 0.1, speedSetpoint);
  }
  // 速度限制
  currentSpeed = constrain(currentSpeed, -maxSpeed, maxSpeed);
  // 更新状态
  iState.speedActual = currentSpeed;
  // 设置电机速度
  float motorSpeed = currentSpeed / 0.05; // 轮半径 0.05m
  motorL.move(motorSpeed);
  motorR.move(motorSpeed);
}

void handlePositionCommand() {
  // 处理位置命令
  float targetX = mb.Hreg(REG_TARGET_X) / 100.0;
  float targetY = mb.Hreg(REG_TARGET_Y) / 100.0;
  // 计算位置误差
  float dx = targetX - iState.posX;
  float dy = targetY - iState.posY;
  float distance = sqrt(dx * dx + dy * dy);
  if (distance > 0.01) { // 1cm 阈值
    // 计算目标角度
    float targetAngle = atan2(dy, dx);
    // 角度控制
    float angleError = targetAngle - iState.angleActual;
    // 归一化到 [-π, π]
    while (angleError > PI) angleError -= 2 * PI;
    while (angleError < -PI) angleError += 2 * PI;
    // 角度 PID
    float angularSpeed = angleError * 2.0; // 简单 P 控制
    // 距离控制
    float linearSpeed = min(distance * 0.5, 0.5); // 最大 0.5m/s
    // 差速控制
    float wheelSep = 0.3;
    float leftSpeed = linearSpeed - (angularSpeed * wheelSep / 2.0);
    float rightSpeed = linearSpeed + (angularSpeed * wheelSep / 2.0);
    motorL.move(leftSpeed / 0.05);
    motorR.move(rightSpeed / 0.05);
  }
}

void updateInputRegisters() {
  // 更新输入寄存器
  // 实际速度 (0.01 m/s)
  mb.Ireg(REG_ACTUAL_SPEED - 100, (uint16_t)(iState.speedActual * 100));
  // 实际位置 (0.01 m)
  mb.Ireg(REG_POSITION_X - 100, (uint16_t)(iState.posX * 100));
  mb.Ireg(REG_POSITION_Y - 100, (uint16_t)(iState.posY * 100));
  // 电池电压 (0.1 V)
  mb.Ireg(REG_BATTERY_VOLTAGE - 100, (uint16_t)(iState.battery * 10));
  // 温度 (0.1 °C)
  mb.Ireg(REG_MOTOR_TEMP - 100, (uint16_t)(iState.temperature[0] * 10));
  // 状态字
  updateStatusWord();
  mb.Ireg(REG_STATUS_WORD - 100, iState.statusWord);
  // 错误代码
  mb.Ireg(REG_ERROR_CODE - 100, iState.errorCode);
}

void updateStatusWord() {
  // 更新 Modbus 状态字
  iState.statusWord = 0;
  // 位 0: 设备就绪
  iState.statusWord |= 0x0001;
  // 位 1: 电机使能
  iState.statusWord |= 0x0002;
  // 位 2: 错误状态
  if (iState.errorCode != 0) {
    iState.statusWord |= 0x0004;
  }
  // 位 3: 急停状态
  if (iState.emergencyStop) {
    iState.statusWord |= 0x0008;
  }
  // 位 4: 电池低
  if (iState.battery < 20.0) {
    iState.statusWord |= 0x0010;
  }
  // 位 5: 通讯正常
  iState.statusWord |= 0x0020;
  // 位 6: 目标到达
  if (abs(iState.speedActual) < 0.01) {
    iState.statusWord |= 0x0040;
  }
}

void safetyMonitor() {
  // 工业安全监控
  // 1. 电池监控
  if (iState.battery < 18.0) { // 欠压保护
    iState.errorCode = 0x1001;
    emergencyStop();
  }
  // 2. 温度监控
  if (iState.temperature[0] > 75.0 || iState.temperature[1] > 75.0) {
    iState.errorCode = 0x1002;
    deratePower(0.5); // 降额 50%
  }
  // 3. 通讯超时
  static unsigned long lastComm = 0;
  if (millis() - lastComm > 5000) { // 5 秒超时
    iState.errorCode = 0x1003;
    emergencyStop();
  }
}

4.6 ROS 串行协议集成

场景：与 ROS 系统集成，实现高级导航和感知。核心逻辑：ROS Serial 协议 + Twist 消息。

#include <SimpleFOC.h>
#include <ros.h>

// ROS 库
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/BatteryState.h>
#include <std_msgs/Bool.h>

// ROS 节点句柄
ros::NodeHandle nh;

// BLDC 电机
BLDCMotor motorL, motorR;

// 里程计参数
float wheel_radius = 0.05; // 轮半径
float wheel_base = 0.30;   // 轮距
float encoder_resolution = 2048 * 4; // 编码器分辨率
float left_ticks = 0, right_ticks = 0;
float last_left_ticks = 0, last_right_ticks = 0;

// ROS 消息
geometry_msgs::Twist cmd_vel;
nav_msgs::Odometry odom_msg;
sensor_msgs::BatteryState battery_msg;
std_msgs::Bool emergency_msg;

// ROS 发布者
ros::Publisher odom_pub("odom", &odom_msg);
ros::Publisher battery_pub("battery", &battery_msg);
ros::Publisher emergency_pub("emergency_stop", &emergency_msg);

// ROS 订阅者
void cmdVelCallback(const geometry_msgs::Twist& msg) {
  cmd_vel = msg;
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &cmdVelCallback);

// 紧急停止服务
void emergencyStopCallback(const std_msgs::Bool& msg) {
  if (msg.data) {
    emergencyStop();
  } else {
    resumeOperation();
  }
}
ros::Subscriber<std_msgs::Bool> emergency_sub("emergency_stop", &emergencyStopCallback);

// 控制参数
float max_linear_vel = 1.0; // 最大线速度 1m/s
float max_angular_vel = 1.0; // 最大角速度 1rad/s
float linear_vel = 0.0;
float angular_vel = 0.0;
bool emergency_stop = false;

// 安全区域
float safe_zone_x_min = 0.0, safe_zone_x_max = 10.0;
float safe_zone_y_min = 0.0, safe_zone_y_max = 10.0;

void setup() {
  // 初始化串口
  Serial.begin(57600); // ROS 默认波特率
  // 初始化 ROS
  nh.initNode();
  nh.advertise(odom_pub);
  nh.advertise(battery_pub);
  nh.advertise(emergency_pub);
  nh.subscribe(cmd_vel_sub);
  nh.subscribe(emergency_sub);
  // 初始化电机
  initMotorsROS();
  // 初始化里程计
  initOdometry();
  // 初始化安全系统
  initSafetySystem();
  // 发布启动消息
  nh.loginfo("ROS Industrial Inspection Robot Ready");
  // 设置 TF 帧
  odom_msg.header.frame_id = "odom";
  odom_msg.child_frame_id = "base_link";
}

void loop() {
  // 1. 处理 ROS 回调
  nh.spinOnce();
  // 2. 检查安全区域
  checkSafetyZone();
  // 3. 速度控制
  if (!emergency_stop) {
    velocityControl();
  } else {
    // 紧急停止状态
    motorL.move(0);
    motorR.move(0);
  }
  // 4. 更新里程计
  updateOdometry();
  // 5. 发布 ROS 消息
  publishROSData();
  // 6. 执行电机控制
  motorL.loopFOC();
  motorR.loopFOC();
  // 控制循环延迟
  delay(10); // 100Hz
}

void velocityControl() {
  // 应用速度限制
  linear_vel = constrain(cmd_vel.linear.x, -max_linear_vel, max_linear_vel);
  angular_vel = constrain(cmd_vel.angular.z, -max_angular_vel, max_angular_vel);
  // 差速运动学
  float left_speed = (linear_vel - angular_vel * wheel_base / 2.0) / wheel_radius;
  float right_speed = (linear_vel + angular_vel * wheel_base / 2.0) / wheel_radius;
  // 限制电机速度
  left_speed = constrain(left_speed, -20.0, 20.0);
  right_speed = constrain(right_speed, -20.0, 20.0);
  // 发送到电机
  motorL.move(left_speed);
  motorR.move(right_speed);
}

void updateOdometry() {
  // 读取编码器计数
  float current_left = motorL.shaft_angle * encoder_resolution / (2 * PI);
  float current_right = motorR.shaft_angle * encoder_resolution / (2 * PI);
  // 计算增量
  float delta_left = current_left - last_left_ticks;
  float delta_right = current_right - last_right_ticks;
  // 保存当前值
  last_left_ticks = current_left;
  last_right_ticks = current_right;
  left_ticks += delta_left;
  right_ticks += delta_right;
  // 计算里程
  float left_distance = delta_left * 2 * PI * wheel_radius / encoder_resolution;
  float right_distance = delta_right * 2 * PI * wheel_radius / encoder_resolution;
  static float x = 0.0, y = 0.0, theta = 0.0;
  // 计算增量
  float linear = (left_distance + right_distance) / 2.0;
  float angular = (right_distance - left_distance) / wheel_base;
  // 更新位姿
  x += linear * cos(theta + angular / 2.0);
  y += linear * sin(theta + angular / 2.0);
  theta += angular;
  // 归一化角度
  while (theta > PI) theta -= 2 * PI;
  while (theta < -PI) theta += 2 * PI;
  // 填充里程计消息
  odom_msg.header.stamp = nh.now();
  odom_msg.pose.pose.position.x = x;
  odom_msg.pose.pose.position.y = y;
  odom_msg.pose.pose.position.z = 0.0;
  // 四元数方向
  odom_msg.pose.pose.orientation.x = 0.0;
  odom_msg.pose.pose.orientation.y = 0.0;
  odom_msg.pose.pose.orientation.z = sin(theta / 2.0);
  odom_msg.pose.pose.orientation.w = cos(theta / 2.0);
  // 速度
  odom_msg.twist.twist.linear.x = linear_vel;
  odom_msg.twist.twist.angular.z = angular_vel;
}

void publishROSData() {
  static unsigned long last_odom_pub = 0;
  static unsigned long last_battery_pub = 0;
  unsigned long now = millis();
  // 发布里程计 (20Hz)
  if (now - last_odom_pub >= 50) {
    odom_pub.publish(&odom_msg);
    last_odom_pub = now;
  }
  // 发布电池状态 (1Hz)
  if (now - last_battery_pub >= 1000) {
    updateBatteryMessage();
    battery_pub.publish(&battery_msg);
    last_battery_pub = now;
  }
  // 发布紧急状态
  emergency_msg.data = emergency_stop;
  emergency_pub.publish(&emergency_msg);
}

void updateBatteryMessage() {
  battery_msg.header.stamp = nh.now();
  battery_msg.voltage = readBatteryVoltage();
  battery_msg.current = readBatteryCurrent();
  battery_msg.percentage = calculateBatteryPercent(battery_msg.voltage);
  battery_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
  battery_msg.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_GOOD;
  battery_msg.present = true;
}

void checkSafetyZone() {
  // 检查是否在安全区域内
  float x = odom_msg.pose.pose.position.x;
  float y = odom_msg.pose.pose.position.y;
  if (x < safe_zone_x_min || x > safe_zone_x_max || y < safe_zone_y_min || y > safe_zone_y_max) {
    if (!emergency_stop) {
      nh.logwarn("Robot outside safety zone! Emergency stop.");
      emergencyStop();
      // 发送返回指令
      geometry_msgs::Twist return_cmd;
      return_cmd.linear.x = 0.0;
      return_cmd.angular.z = 0.0;
      cmd_vel = return_cmd;
    }
  }
}

void emergencyStop() {
  emergency_stop = true;
  linear_vel = 0.0;
  angular_vel = 0.0;
  // 立即停止电机
  motorL.move(0);
  motorR.move(0);
  nh.logerror("EMERGENCY STOP ACTIVATED");
}

void resumeOperation() {
  if (emergency_stop) {
    emergency_stop = false;
    nh.loginfo("Emergency stop released");
  }
}

// ROS 服务调用示例
void handleROSServices() {
  // 这里可以添加 ROS 服务处理
  // 例如：校准服务、参数配置服务等
}

5. 要点解读

串口协议设计与选择

• ASCII 协议（案例 4）：简单直观，适合调试和人机交互。parseCommand() 解析单字符（F/B/L/R）和带参数命令（V0.5/T0.2）。优点是可读性强，缺点是效率低、无校验。
• Modbus RTU（案例 5）：工业标准，适合 PLC 集成。16 位寄存器映射，CRC 校验保证可靠。writeRegisterCallback() 处理写操作，状态字提供完整状态信息。这是工业部署首选。
• ROS Serial（案例 6）：机器人标准，适合与 ROS 导航栈集成。支持 Twist 速度命令、Odometry 里程计、BatteryState 等标准消息。便于与 SLAM、导航、感知模块集成。

指令处理的安全机制

• 速度限制：案例 4 的 constrain(linearSpeed, -maxSpeed, maxSpeed) 防止超速。工业环境通常限制在 1m/s 以内。
• 加速度限制：案例 5 的 handleSpeedCommand() 实现梯形速度曲线，避免急启急停对机械的冲击。
• 安全区域：案例 6 的 checkSafetyZone() 实现地理围栏，超出预设区域自动急停。
• 通讯超时：案例 5 的 safetyMonitor() 检测 5 秒无通讯自动停止，防止失控。
• 急停处理：所有案例都有 emergencyStop() 函数，立即置零速度并发布状态。

状态反馈与监控

• 里程计计算：案例 4 和案例 6 都通过编码器积分计算位置。关键公式：position += (Δleft + Δright)/2 * cos(heading)。
• 电池监控：案例 6 的 BatteryState 消息包含电压、电流、电量百分比。工业应用需监控单节电芯电压，防止不平衡。
• 温度监控：案例 5 监控电机温度，超过 75°C 触发降额 deratePower(0.5)。
• 状态字设计：案例 5 的 16 位状态字 statusWord，每 bit 表示一种状态（就绪、错误、急停等），便于 PLC 快速读取。

工业巡检的特殊功能

• 自动避障：案例 4 的 checkObstacles() 检测 30cm 内障碍物自动转向。工业环境需考虑超声波多径反射问题。
• 点到点导航：案例 4 的 navigateTo(x,y) 实现简单航点导航。工业应用需结合反光板、二维码或 UWB 精确定位。
• 巡检路径：可扩展为预设路径循环巡检，定时报告关键设备状态（温度、振动、噪声）。
• 异常上报：通过 Modbus 异常代码或 ROS 的 logerror 上报故障，触发维护工单。

部署实施的工程考量

• 波特率选择：调试用 115200，工业 Modbus 用 9600/19200，ROS 用 57600/115200。长距离需降低波特率。
• 电缆要求：RS-485（Modbus）需双绞线，终端电阻 120Ω。ROS Serial 可用普通串口，但建议用 RS-485 转换抗干扰。
• 电源管理：工业机器人常采用 24VDC，需 DC-DC 降压为 12V（电机）和 5V/3.3V（控制板）。案例 5 的 battery < 18.0 欠压保护针对 24V 系统（6S 锂电）。
• 防护等级：工业环境需 IP54 以上防护，接头需防水。代码中应监控湿度传感器，高湿度报警。
• 维护接口：保留固件升级接口（如 USB DFU），支持远程升级。案例可扩展为通过 Modbus 写特定寄存器触发固件更新。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和 Arduino 版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。