【花雕学编程】Arduino BLDC 之基于串口指令的远程控制工业巡检机器人

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

#include<Servo.h> Servo esc;// 电调控制对象 String inputCommand;int currentSpeed =1000;// 默认低脉冲（停止）voidsetup(){ 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");}voidloop(){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);}elseif(inputCommand =="STOP"){ currentSpeed =1000; esc.writeMicroseconds(currentSpeed); Serial.println("Emergency Stop");}}}

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

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

#include<Servo.h> Servo escLeft, escRight;// 左右电机电调 String command;voidsetup(){ 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");}voidloop(){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);}elseif(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);}elseif(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;// 编码器信号输入（需中断引脚）volatilelong pulseCount =0;double targetRPM =100, actualRPM =0;double outputPWM =1000; PID myPID(&actualRPM,&outputPWM,&targetRPM,0.5,0.1,0.01, DIRECT);// PID参数需调试voidcountPulses(){ pulseCount++;}// 编码器中断函数voidsetup(){ 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");}voidloop(){staticunsignedlong 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);voiddoA_L(){ encoderL.handleA();}voiddoB_L(){ encoderL.handleB();}voiddoA_R(){ encoderR.handleA();}voiddoB_R(){ encoderR.handleB();}// 控制参数float linearSpeed =0.0;// 线速度 m/sfloat angularSpeed =0.0;// 角速度 rad/sfloat maxSpeed =1.0;// 最大速度 1m/sfloat wheelSeparation =0.3;// 轮距0.3mfloat wheelRadius =0.05;// 轮半径0.05m// 指令结构structCommand{char type;// 指令类型: M=移动, S=停止, C=配置, R=读取float param1;// 参数1float param2;// 参数2unsignedlong timestamp;// 时间戳}; Command currentCmd ={'S',0,0,0};bool newCommand =false; String serialBuffer ="";// 机器人状态structRobotState{float posX =0.0, posY =0.0;// 位置float heading =0.0;// 航向float battery =12.6;// 电池电压float temp =25.0;// 温度unsignedlong uptime =0;// 运行时间}; RobotState state;// 超声波传感器#defineSONAR_TRIG9#defineSONAR_ECHO10float obstacleDistance =0.0;voidsetup(){ 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");}voidloop(){// 1. 读取串口指令readSerialCommands();// 2. 执行当前指令if(newCommand){executeCommand(currentCmd); newCommand =false;}// 3. 运动控制motionControl();// 4. 更新状态updateRobotState();// 5. 障碍物检测checkObstacles();// 6. 执行FOC控制 motorL.loopFOC(); motorR.loopFOC();// 7. 定期状态报告staticunsignedlong lastReport =0;if(millis()- lastReport >1000){// 1Hz报告sendStatusReport(); lastReport =millis();}}voidreadSerialCommands(){while(Serial.available()){char c = Serial.read();if(c =='\n'|| c =='\r'){if(serialBuffer.length()>0){parseCommand(serialBuffer); serialBuffer ="";}}else{ serialBuffer += c;}}}voidparseCommand(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/sbreak;case'B':// 后退 cmd.param1 =-0.3;break;case'L':// 左转 cmd.type ='T'; cmd.param1 =0.5;// 0.5rad/sbreak;case'R':// 右转 cmd.type ='T'; cmd.param1 =-0.5;break;case'S':// 停止 cmd.type ='S';break;case'?':// 状态查询 cmd.type ='R';break;}}elseif(buffer.startsWith("V")){// 速度设置 V0.5 cmd.type ='M'; buffer.remove(0,1); cmd.param1 = buffer.toFloat();}elseif(buffer.startsWith("T")){// 转向设置 T0.2 cmd.type ='T'; buffer.remove(0,1); cmd.param1 = buffer.toFloat();}elseif(buffer.startsWith("C")){// 配置命令parseConfigCommand(buffer, cmd);}elseif(buffer.startsWith("G")){// 前往坐标 G1.5,2.0 cmd.type ='G';parseCoordinates(buffer, cmd);} currentCmd = cmd; newCommand =true;// 回显接收的指令 Serial.print("INSPECT> Received: "); Serial.println(buffer);}voidexecuteCommand(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;}}voidmotionControl(){// 差速运动学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);}voidupdateRobotState(){// 更新里程计staticfloat 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;}voidcheckObstacles(){// 超声波测距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");}}}voidsendStatusReport(){ 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;#defineMODBUS_SLAVE_ID1#defineMODBUS_BAUDRATE9600#defineMODBUS_SERIALSerial2 // 硬件串口2// Modbus寄存器映射enumModbusRegisters{// 保持寄存器 (可读可写) 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};// 机器人状态structIndustrialState{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;// 控制模式enumControlMode{ SPEED_CTRL, POSITION_CTRL, TRAJECTORY_CTRL }; ControlMode ctrlMode = SPEED_CTRL;voidsetup(){ 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");}voidloop(){// 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. 诊断输出staticunsignedlong lastDiag =0;if(millis()- lastDiag >1000){sendDiagnosticInfo(); lastDiag =millis();}}voidinitModbusRegisters(){// 初始化保持寄存器默认值// 速度相关 (单位: 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);}}boolwriteRegisterCallback(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);}returntrue;}returnfalse;}voidhandleSpeedCommand(uint16_t speedReg){// 处理速度命令float speedSetpoint = speedReg /100.0;// 转换为m/s// 应用加速度限制staticfloat 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);}voidhandlePositionCommand(){// 处理位置命令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;// 角度PIDfloat 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);}}voidupdateInputRegisters(){// 更新输入寄存器// 实际速度 (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);}voidupdateStatusWord(){// 更新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;}}voidsafetyMonitor(){// 工业安全监控// 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. 通讯超时staticunsignedlong 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订阅者voidcmdVelCallback(const geometry_msgs::Twist& msg){ cmd_vel = msg;} ros::Subscriber<geometry_msgs::Twist>cmd_vel_sub("cmd_vel",&cmdVelCallback);// 紧急停止服务voidemergencyStopCallback(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/sfloat max_angular_vel =1.0;// 最大角速度 1rad/sfloat 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;voidsetup(){// 初始化串口 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";}voidloop(){// 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}voidvelocityControl(){// 应用速度限制 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);}voidupdateOdometry(){// 读取编码器计数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;staticfloat 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;}voidpublishROSData(){staticunsignedlong last_odom_pub =0;staticunsignedlong last_battery_pub =0;unsignedlong 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);}voidupdateBatteryMessage(){ 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;}voidcheckSafetyZone(){// 检查是否在安全区域内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;}}}voidemergencyStop(){ emergency_stop =true; linear_vel =0.0; angular_vel =0.0;// 立即停止电机 motorL.move(0); motorR.move(0); nh.logerror("EMERGENCY STOP ACTIVATED");}voidresumeOperation(){if(emergency_stop){ emergency_stop =false; nh.loginfo("Emergency stop released");}}// ROS服务调用示例voidhandleROSServices(){// 这里可以添加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版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。