基于 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 应定期或在事件触发时向上位机回传机器人的状态信息（如当前坐标、电池电压、电机温度、传感器读数），以便操作员全面掌握现场情况，实现闭环控制。

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脚本）发送串口指令控制机器人电机启停和调速。

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");
    }
  }
}

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

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闭环控制。

要点解读
通信协议设计
指令格式：采用易解析的字符串协议（如"SPEED:1500"），避免二进制协议的兼容性问题。
校验机制：工业场景建议增加CRC校验或ACK响应（如案例1可扩展为"SPEED:1500\nACK"）。
多电机同步控制
时间同步：案例2中通过独立PWM信号控制多电机，需确保串口指令同时到达（或通过硬件同步）。
仲裁逻辑：优先级高的指令（如急停）应立即覆盖当前动作。
闭环控制必要性
PID调参：案例3中Kp、Ki、Kd需根据负载调整（如空载Kp=0.5，重载Kp=1.2）。
传感器选择：编码器分辨率需匹配电机转速（如高速电机用增量式编码器，低速用霍尔传感器）。
安全与容错
看门狗机制：若串口通信中断，应自动降速至安全值（如案例1中可添加超时检测）。
硬件保护：电机驱动回路加熔断器，防止堵转烧毁（如案例3中建议增加电流监测）。
扩展性与模块化
协议扩展：预留自定义指令（如"LED:ON"控制指示灯）。
多设备通信：通过RS485总线替代串口，实现多机器人协同（需修改案例1的通信接口）。

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");
}

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();
  }
}

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服务处理
  // 例如: 校准服务、参数配置服务等
}

要点解读
串口协议设计与选择
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版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。