Arduino BLDC 之双 BLDC 轮式消防机器人，本质上是把“差速驱动底盘、左右轮独立冗余、互锁急停与消防执行单元”做成一个高可靠、可降级的嵌入式移动平台；它的价值不只是能喷水，而是用冗余驱动和硬件级急停保证火场、浓烟、狭窄空间里依然可受控、可撤离、可安全停机。

主要特点
. 双 BLDC 独立差速驱动
左右轮各由一路 BLDC 独立驱动，前进、后退、原地转向和弧线避障都通过左右轮速度差实现。相比单电机+机械差速，双 BLDC 能更直接地控制转向力矩，也更容易实现单侧失效后的降级行驶。
. 左右轮独立冗余
左右轮驱动链路由独立电机、独立编码器、独立驱动器和独立功率回路组成。当一侧出现堵转、缺相、过流或编码器失联时，系统可立即切断故障侧，保留另一侧低速撤离或转向。这属于“失效后可降级运行”的设计，而不是简单停机。
. 互锁急停机制
急停必须优先于所有运动指令。硬件上可采用常闭触点串联驱动器使能/主功率继电器，按下后强制切断转矩输出；软件上由急停标志位锁定速度指令为零，并在复位前禁止重新驱动。
互锁体现在：急停触发后，自动/遥控/灭火任务都不能绕过停机；复位后也不能自动恢复运行，需要人工确认。更可靠方案可采用双回路急停，一路故障也能保持安全停机。
. 分层实时控制架构
上层处理火源识别、遥控指令、路径规划和任务调度；下层负责 BLDC FOC、编码器反馈、IMU 姿态、急停判断和故障保护。
在 Arduino 生态中，AVR 系列适合做底层执行或轻量原型；若同时运行 SLAM、视觉火源识别和多路传感器融合，建议采用上位机+下位机架构，下位机专注电机闭环与安全保护。
. 消防执行单元联动
机器人通常配备水泵、电磁阀、水炮舵机、火焰/烟雾/温度/气体传感器。灭火动作必须与底盘安全状态联动：急停触发时应优先保证停机，再决定是否保持水泵关闭、报警或等待人工接管。

应用场景
室内火场侦察：进入烟雾浓、温度高、人员不宜进入的房间，执行火焰定位、温度监测和气体检测。
狭窄空间灭火：仓库、机房、地下车库、配电间等空间受限场景，利用差速转向实现小半径机动。
危险品环境初处置：在可能存在可燃气体、烟雾或电气火灾风险的环境中，由机器人先行靠近侦察。
校园/实验室/创客项目：作为消防机器人教学平台，验证 BLDC 差速控制、冗余驱动、急停设计和多传感器融合。
应急演练与编队演示：多台机器人协同执行侦察、灭火、警戒等任务，验证通信、遥控与安全策略。

需要注意的事项
急停必须硬件优先，不能只靠软件
急停信号应直接作用于驱动器使能、主继电器或安全回路，而不是仅在主循环里把速度设为 0。软件死机、通信丢包或传感器异常时，硬件急停仍能切断动力。
冗余不是简单“多装一个电机”
左右轮独立冗余需要独立功率路径、独立故障检测和独立保护。如果两侧共用同一驱动器、同一保险或同一使能信号，单点故障仍会导致整车失效。
BLDC 必须闭环，不能只用开环 ESC
消防机器人对低速可控性、转向精度和故障检测要求更高。应使用带编码器/霍尔反馈的 BLDC 方案，并实现电流、速度、位置多环控制，否则难以判断堵转、打滑和单侧失效。
电源、散热和 EMC 要单独设计
BLDC 启动、转向和灭火泵同时工作时电流冲击大，控制板、传感器和通信模块容易受干扰。建议动力电源与控制电源隔离，驱动器端加大容量低 ESR 电容，敏感信号线屏蔽并远离动力线。
消防场景要重视防爆、防水和热防护
火场存在高温、水汽、导电粉尘和可燃气体风险。外壳、接插件、传感器和电池舱需做防水防尘处理；在可燃气体环境中应避免产生电火花；电池应配备 BMS，并设置过温、过流、欠压和倾角保护。

1、双节点冗余 BLDC 控制 + 故障切换（专利架构简化实现）
适用场景：左右轮各由独立的 Arduino 节点控制，两节点通过有线通信协调，当一侧节点检测到故障时，可切换至备用控制路径或通知对侧调整。该架构参考了 US20220352833A1 专利中“第一节点与第二节点通过冗余网络协调控制两个 BLDC 电机”的设计。

/**
 * 消防机器人 - 双节点冗余BLDC控制 + 故障切换
 * 硬件假设：双Arduino节点(左/右轮各一), 2×BLDC(SimpleFOC), 有线通信(串口)
 * 参考：US20220352833A1 冗余BLDC控制架构[citation:1]
 * 核心：节点间心跳监测 -> 单侧故障检测 -> 降级运行或切换
 */

#include <SimpleFOC.h>

// 本节点控制的电机（以左轮节点为例）
BLDCMotor myMotor(7);
Encoder myEncoder(2, 4, 2048);
BLDCDriver3PWM myDriver(5, 6, 9, 8);

// 节点角色
#define NODE_LEFT  0
#define NODE_RIGHT 1
const int MY_ROLE = NODE_LEFT;

// 节点间通信（简化：串口1用于节点间，串口0用于调试）
// 实际项目中可用RS485/CAN

// 故障检测
enum FaultState {
  FAULT_NONE,
  FAULT_ENCODER,
  FAULT_DRIVER,
  FAULT_OVERCURRENT,
  FAULT_COMM_LOST
};
FaultState myFault = FAULT_NONE;

// 节点间心跳
unsigned long lastPeerHeartbeat = 0;
const unsigned long HEARTBEAT_TIMEOUT = 500; // 500ms无心跳判定通信故障

// 运行模式
enum RunMode {
  MODE_NORMAL,      // 正常双轮驱动
  MODE_DEGRADED,    // 降级：单侧故障，对侧补偿
  MODE_ESTOP        // 急停
};
RunMode currentMode = MODE_NORMAL;

// 目标速度
float targetSpeed = 3.0;
float mySpeed = 0;
float peerSpeed = 0;

void setup() {
  Serial.begin(115200);    // 调试
  Serial1.begin(115200);   // 节点间通信
  
  myDriver.voltage_power_supply = 24;
  myDriver.init();
  myMotor.linkDriver(&myDriver);
  myMotor.linkSensor(&myEncoder);
  myMotor.init();
  myMotor.initFOC();
  myMotor.controller = MotionControlType::velocity;
  
  lastPeerHeartbeat = millis();
}

// 发送心跳和数据给对侧节点
void sendHeartbeat() {
  // 格式: "H,role,speed,fault\n"
  Serial1.print("H,");
  Serial1.print(MY_ROLE);
  Serial1.print(",");
  Serial1.print(mySpeed, 2);
  Serial1.print(",");
  Serial1.println(myFault);
}

// 接收对侧节点数据
void receivePeerData() {
  if (Serial1.available()) {
    String data = Serial1.readStringUntil('\n');
    // 解析 "H,role,speed,fault"
    if (data.startsWith("H,")) {
      int firstComma = data.indexOf(',');
      int secondComma = data.indexOf(',', firstComma + 1);
      int thirdComma = data.indexOf(',', secondComma + 1);
      
      if (secondComma > 0 && thirdComma > 0) {
        peerSpeed = data.substring(secondComma + 1, thirdComma).toFloat();
        lastPeerHeartbeat = millis();
      }
    }
  }
}

// 本地故障检测
void checkLocalFault() {
  // 简化：检测电机电流异常（实际需根据驱动器反馈）
  // 此处模拟：如果电机堵转，电流异常
  static unsigned long lastCheck = 0;
  if (millis() - lastCheck > 100) {
    lastCheck = millis();
    
    // SimpleFOC 电流检测（需配置电流传感器）
    // float current = myMotor.current_sp;
    // if (abs(current) > 20.0) myFault = FAULT_OVERCURRENT;
    
    // 编码器合理性检查
    float vel = myMotor.shaftVelocity();
    if (abs(vel) > 50.0 && abs(targetSpeed) < 1.0) {
      myFault = FAULT_ENCODER;
    }
  }
}

void loop() {
  myMotor.loopFOC();
  
  sendHeartbeat();
  receivePeerData();
  checkLocalFault();
  
  // 通信故障检测
  if (millis() - lastPeerHeartbeat > HEARTBEAT_TIMEOUT) {
    myFault = FAULT_COMM_LOST;
    currentMode = MODE_DEGRADED;
  }
  
  // 模式决策
  switch (currentMode) {
    case MODE_NORMAL:
      if (myFault != FAULT_NONE) {
        // 本侧故障，进入降级模式
        currentMode = MODE_DEGRADED;
      }
      mySpeed = targetSpeed;
      break;
      
    case MODE_DEGRADED:
      // 降级运行：如果本侧健康，补偿对侧
      if (myFault == FAULT_NONE && peerSpeed < 0.5) {
        // 对侧可能故障，本侧降低速度防止失控
        mySpeed = targetSpeed * 0.5;
      } else if (myFault != FAULT_NONE) {
        // 本侧故障，尝试停止
        mySpeed = 0;
        currentMode = MODE_ESTOP;
      }
      break;
      
    case MODE_ESTOP:
      mySpeed = 0;
      break;
  }
  
  myMotor.move(mySpeed);
  
  Serial.print("Role:"); Serial.print(MY_ROLE);
  Serial.print(" Speed:"); Serial.print(mySpeed);
  Serial.print(" PeerSpeed:"); Serial.print(peerSpeed);
  Serial.print(" Fault:"); Serial.print(myFault);
  Serial.print(" Mode:"); Serial.println(currentMode);
  
  delay(20);
}

要点：该代码体现了节点级冗余与协调控制的架构。参考专利 US20220352833A1，两个节点通过有线通信交换心跳和故障状态，当一侧检测到故障时，另一侧可感知并调整行为。关键设计在于：心跳超时作为通信故障的判据，而非等待显式故障信号（通信本身可能已中断）。降级模式下，健康侧降低速度而非全速运行，防止因对侧失控导致机器人整体失稳。

2、硬件互锁急停状态机 + 双通道使能控制（参考轮椅制动专利）
搜索结果显示，US10945900 专利揭示了一种电动轮椅的互锁急停架构：左右电机的制动继电器通过串联的常闭开关链连接，任何一个急停开关断开都会同时切断两个电机的制动回路，实现“任一急停触发 → 双侧同时制动”的互锁效果。以下案例简化实现了这一逻辑。

/**
 * 消防机器人 - 互锁急停 + 双通道使能控制
 * 硬件假设：Arduino Mega, 2×BLDC驱动(带使能引脚EN), 急停按钮(常闭), 抱闸继电器
 * 参考：US10945900 电动轮椅互锁制动架构[citation:5]
 * 核心：急停按钮串联 -> 任一触发双侧断电；使能引脚独立控制
 */

#include <SimpleFOC.h>

BLDCMotor motorL(7);
BLDCMotor motorR(7);
Encoder encL(2, 4, 2048);
Encoder encR(3, 5, 2048);

// 驱动器使能引脚（低电平使能，高电平禁用）
#define EN_LEFT    22
#define EN_RIGHT   23

// 抱闸继电器（高电平释放，低电平抱闸）
#define BRAKE_L    24
#define BRAKE_R    25

// 硬件急停按钮（常闭，按下时断开）
#define ESTOP_PIN  2

// 互锁使能信号（来自硬件串联链，低电平有效）
#define INTERLOCK_OK 26

// 急停状态
volatile bool estopTriggered = false;

void estopISR() {
  estopTriggered = true;
}

void setup() {
  Serial.begin(115200);
  
  // 急停按钮：常闭，按下时接地，触发中断
  pinMode(ESTOP_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), estopISR, FALLING);
  
  // 互锁链状态
  pinMode(INTERLOCK_OK, INPUT_PULLUP);
  
  // 使能引脚
  pinMode(EN_LEFT, OUTPUT);
  pinMode(EN_RIGHT, OUTPUT);
  digitalWrite(EN_LEFT, LOW);   // 使能
  digitalWrite(EN_RIGHT, LOW);
  
  // 抱闸控制
  pinMode(BRAKE_L, OUTPUT);
  pinMode(BRAKE_R, OUTPUT);
  digitalWrite(BRAKE_L, HIGH);  // 释放抱闸
  digitalWrite(BRAKE_R, HIGH);
  
  motorL.linkSensor(&encL);
  motorR.linkSensor(&encR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  
  Serial.println("互锁急停系统就绪");
}

// 执行急停：双侧同时禁用 + 抱闸
void executeEmergencyStop() {
  // 1. 禁用双侧驱动器（硬件级切断动力）
  digitalWrite(EN_LEFT, HIGH);
  digitalWrite(EN_RIGHT, HIGH);
  
  // 2. 激活双侧抱闸
  digitalWrite(BRAKE_L, LOW);
  digitalWrite(BRAKE_R, LOW);
  
  // 3. 电机零力矩
  motorL.move(0);
  motorR.move(0);
  
  Serial.println("!!! 急停激活：双侧动力切断 + 抱闸 !!!");
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // ========== 最高优先级：急停检查 ==========
  // 条件1：软件急停标志（中断触发）
  // 条件2：硬件互锁链断开（常闭开关链某处断开）
  bool hardwareInterlock = digitalRead(INTERLOCK_OK) == HIGH; // 高电平表示链断
  
  if (estopTriggered || hardwareInterlock) {
    executeEmergencyStop();
    // 急停状态锁定，等待硬件复位
    while (1) {
      Serial.println("急停锁定 - 等待硬件复位");
      delay(500);
    }
  }
  
  // ========== 正常控制 ==========
  // 简化：双轮同步直行
  float speed = 3.0;
  
  // 安全检查：如果一侧使能异常，双侧停止
  if (digitalRead(EN_LEFT) == HIGH || digitalRead(EN_RIGHT) == HIGH) {
    executeEmergencyStop();
    return;
  }
  
  motorL.move(speed);
  motorR.move(speed);
  
  Serial.print("L:"); Serial.print(speed);
  Serial.print(" R:"); Serial.print(speed);
  Serial.print(" Interlock:"); Serial.println(hardwareInterlock ? "OPEN" : "OK");
  
  delay(20);
}

要点：该案例的核心是互锁急停的硬件-软件协同设计。参考专利 US10945900 的架构，急停按钮采用串联常闭开关链——任何一个开关断开，整个使能回路失电，双侧驱动器同时被禁用。代码中的 executeEmergencyStop() 同时执行三个动作：禁用使能引脚、激活抱闸、电机零力矩。关键安全原则：急停状态是不可逆汇点，必须通过硬件复位（如主开关重新上电）才能恢复，而非软件复位。

3、分级制动 + 冗余速度反馈仲裁（消防规范符合性）
搜索结果显示，XF892.1-2010 消防机器人标准要求“紧急停止按钮按下后，消防机器人应立即停止所有动作”，且“制动距离应不大于0.3m”。消防场景的急停不能是简单的断电滑行，而需要受控制动。以下案例实现了基于冗余速度反馈的分级制动状态机。

/**
 * 消防机器人 - 分级制动 + 冗余速度反馈仲裁
 * 硬件假设：Arduino Mega, 2×BLDC, 双编码器/霍尔, 抱闸继电器
 * 参考：XF892.1-2010 消防机器人标准[citation:12], 硬件急停状态机[citation:2][citation:6]
 * 核心：双源速度反馈 -> 仲裁 -> 分级制动（减速->软停->抱闸）
 */

#include <SimpleFOC.h>

BLDCMotor motorL(7);
BLDCMotor motorR(7);
Encoder encL(2, 4, 2048);   // 主编码器
Encoder encR(3, 5, 2048);
// 冗余霍尔传感器（简化：数字引脚）
#define HALL_L_A 14
#define HALL_L_B 15
#define HALL_R_A 16
#define HALL_R_B 17

#define BRAKE_L   24
#define BRAKE_R   25
#define ESTOP_PIN 2

// 制动分级阈值 (cm/s)
#define DECEL_THRESHOLD       30
#define SOFT_STOP_THRESHOLD   8
#define HOLD_THRESHOLD        1.0

// 制动状态
enum BrakeState {
  BRAKE_RUNNING,
  BRAKE_DECEL,
  BRAKE_SOFT_STOP,
  BRAKE_HOLD,
  BRAKE_EMERGENCY
};
BrakeState brakeState = BRAKE_RUNNING;

volatile bool estopFlag = false;
float targetSpeed = 5.0;
float currentSpeed = 0;

// 冗余速度读取：编码器 + 霍尔交叉验证
float readRedundantSpeed() {
  // 主源：编码器
  float encSpeed = motorL.shaftVelocity() * 10.0; // rad/s -> cm/s 近似
  
  // 冗余源：霍尔脉冲计数（简化）
  static unsigned long lastHallTime = 0;
  static int hallCount = 0;
  static float hallSpeed = 0;
  
  // 霍尔中断计数（需额外中断处理，此处简化示意）
  // 实际项目中应在ISR中计数，主循环计算速度
  
  // 仲裁：如果编码器和霍尔偏差过大，标记故障
  // 简化：优先使用编码器，霍尔作为合理性检查
  float speedDiff = abs(encSpeed - hallSpeed);
  if (speedDiff > 20.0 && abs(encSpeed) > 10.0) {
    // 传感器偏差过大，降级使用霍尔
    return hallSpeed;
  }
  
  return encSpeed;
}

void estopISR() {
  estopFlag = true;
}

void setup() {
  Serial.begin(115200);
  
  pinMode(ESTOP_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), estopISR, FALLING);
  
  pinMode(BRAKE_L, OUTPUT);
  pinMode(BRAKE_R, OUTPUT);
  digitalWrite(BRAKE_L, HIGH); // 释放
  digitalWrite(BRAKE_R, HIGH);
  
  pinMode(HALL_L_A, INPUT_PULLUP);
  pinMode(HALL_L_B, INPUT_PULLUP);
  pinMode(HALL_R_A, INPUT_PULLUP);
  pinMode(HALL_R_B, INPUT_PULLUP);
  
  motorL.linkSensor(&encL);
  motorR.linkSensor(&encR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  currentSpeed = readRedundantSpeed();
  
  // ========== 急停触发 ==========
  if (estopFlag) {
    brakeState = BRAKE_EMERGENCY;
    estopFlag = false;
  }
  
  // ========== 分级制动状态机 ==========
  switch (brakeState) {
    case BRAKE_RUNNING:
      motorL.move(targetSpeed);
      motorR.move(targetSpeed);
      // 可添加触发条件
      break;
      
    case BRAKE_DECEL:
      // 一级：快速减速（电机制动）
      targetSpeed -= 8.0 * 0.05;
      if (targetSpeed < 0) targetSpeed = 0;
      motorL.move(targetSpeed);
      motorR.move(targetSpeed);
      if (currentSpeed < SOFT_STOP_THRESHOLD) {
        brakeState = BRAKE_SOFT_STOP;
      }
      break;
      
    case BRAKE_SOFT_STOP:
      // 二级：精细逼近零速
      targetSpeed -= 3.0 * 0.05;
      if (targetSpeed < 0) targetSpeed = 0;
      motorL.move(targetSpeed);
      motorR.move(targetSpeed);
      if (currentSpeed < HOLD_THRESHOLD) {
        brakeState = BRAKE_HOLD;
        digitalWrite(BRAKE_L, LOW);  // 抱闸
        digitalWrite(BRAKE_R, LOW);
      }
      break;
      
    case BRAKE_HOLD:
      motorL.move(0);
      motorR.move(0);
      break;
      
    case BRAKE_EMERGENCY:
      // 急停：立即禁用 + 抱闸（最快制动）
      motorL.move(0);
      motorR.move(0);
      digitalWrite(BRAKE_L, LOW);
      digitalWrite(BRAKE_R, LOW);
      Serial.println("紧急制动 - 立即抱闸");
      // 保持
      break;
  }
  
  Serial.print("State:"); Serial.print(brakeState);
  Serial.print(" Speed:"); Serial.print(currentSpeed);
  Serial.print(" Target:"); Serial.println(targetSpeed);
  
  delay(20);
}

要点：该案例实现了符合消防机器人标准的分级制动架构。与普通急停的“一刀切断电”不同，消防机器人的急停需要在最短制动距离内完成制动（标准要求 ≤0.3m），同时避免机械冲击。分级制动状态机从快速电机制动（DECEL）→ 精细软停（SOFT_STOP）→ 机械抱闸（HOLD）逐步过渡。冗余速度反馈（编码器 + 霍尔）通过交叉验证避免单一传感器故障导致的制动误判。

要点解读

1. 左右轮独立冗余的核心价值不是“性能提升”，而是“故障后仍能撤离”。 消防机器人的工作环境（火场、浓烟、高温）对电子设备极不友好，单侧驱动失效的概率远高于普通场景。独立冗余架构使机器人在一侧完全失效时，仍能通过另一侧的动力实现原地旋转或弧线撤离。关键设计原则：两个节点的通信必须是有线冗余（如 RS485 双绞线），而非无线——火场中的无线通信可能因浓烟和金属结构严重衰减。

2. 互锁急停的本质是“串联使能链”，而非“软件判断”。 参考 US10945900 专利，左右电机的制动回路通过串联常闭开关连接，任何一个急停开关或故障检测触点断开，整个使能链失电，双侧同时制动。这种硬件互锁无法被软件绕过，符合工业安全规范中“安全功能必须独立于控制系统”的要求。软件层面的急停检测（如中断）是补充，而非替代。

3. 消防机器人的急停不能是“自由滑行”，必须是“受控制动”。 XF892.1-2010 要求制动距离 ≤0.3m。如果急停仅切断电机动力，机器人会因惯性滑行远超 0.3m。正确的做法是：电机制动（反向力矩）优先，机械抱闸作为最终保障。分级制动状态机（案例三）体现了这一原则：先以受控减速度降速，再抱闸锁定。BLDC 配合 FOC 的反向力矩控制能力是实现短距制动的关键。

4. 冗余速度反馈需要“仲裁机制”，而非简单平均。 案例三中编码器和霍尔的速度差异如果超过阈值，不能取平均——因为可能是编码器故障导致读数异常。正确的做法是：优先使用编码器（精度高），霍尔作为合理性检查。如果两者偏差过大且编码器读数明显异常（如速度为负但电机正转），则降级使用霍尔。这种“主-备-仲裁”策略比简单平均更可靠。

5. Arduino 在消防机器人中的合理定位是“执行层+安全监测层”，核心安全功能应由硬件电路实现。 消防机器人的完整架构通常包括：硬件急停电路（独立于 MCU）、安全继电器链（硬件互锁）、MCU（Arduino/STM32 执行控制逻辑）、上位机（路径规划）。Arduino 不应参与“是否触发急停”的决策——这个决策必须由硬件电路完成。Arduino 的职责是：执行制动指令、监测状态、向操作员报告。搜索结果中的硬件急停状态机（案例二）体现了这一分层原则。

4、双驱动基础行驶+冗余检测+互锁急停程序（核心：基础控制+安全联锁）
适用场景：消防巡检、初期火灾探查，要求机器人可直行、转向，具备基础冗余和紧急停机功能。

核心逻辑
左右BLDC独立PWM控制，实现直行、转向、变速行走
双通道编码器反馈，实时检测轮速，识别电机故障
独立急停按键+联锁逻辑，异常时同步切断双电机电源，实现安全停机

// 双BLDC消防机器人基础控制+冗余急停程序
// 硬件：左/右BLDC电机、编码器、双通道急停按键、PWM驱动
// 功能：独立驱动、轮速反馈、冗余检测、联锁急停

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM_LEFT = 9, DIR_LEFT = 8;
const byte PWM_RIGHT = 10, DIR_RIGHT = 11;
const byte ENC_LEFT_A=2, ENC_LEFT_B=3;   // 左编码器
const byte ENC_RIGHT_A=4, ENC_RIGHT_B=5; // 右编码器
const byte ESTOP_LEFT = 12; // 左急停
const byte ESTOP_RIGHT=13;  // 右急停

// ========== 变量定义 ==========
int targetLeftSpeed = 120, targetRightSpeed = 120;
long encLeft = 0, encRight = 0, lastEncLeft=0, lastEncRight=0;
unsigned long lastTime = 0;
bool estopActive = false, motorError = false;

// ========== 函数声明 ==========
void readEncoders(unsigned long dt);
bool checkRedundancy();
void driveMotors(int leftPWM, int rightPWM);
void emergencyStop();

void setup() {
  Serial.begin(115200);
  Serial.println("双BLDC消防机器人基础系统启动");

  pinMode(PWM_LEFT, OUTPUT); pinMode(DIR_LEFT, OUTPUT);
  pinMode(PWM_RIGHT, OUTPUT); pinMode(DIR_RIGHT, OUTPUT);
  pinMode(ESTOP_LEFT, INPUT_PULLUP); pinMode(ESTOP_RIGHT, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_LEFT_A), [](){ if(digitalRead(ENC_LEFT_A)) encLeft++; }, RISING); 
  attachInterrupt(digitalPinToInterrupt(ENC_RIGHT_A), [](){ if(digitalRead(ENC_LEFT_A)) encRight++; }, RISING);
}

void loop() {
unsigned long dt = millis() - lastTime;
  if (dt <20) return;
  lastTime = millis();

  // 急停检测if (digitalRead(ESTOP_LEFT)==LOW || digitalRead(ESTOP_RIGHT)==LOW) {
    emergencyStop();
    while (digitalRead(ESTOP_LEFT)==LOW || digitalRead(ESTOP_RIGHT)==LOW) {delay(100);}
  }

  // 冗余检测
  motorError = checkRedundancy();
  if (motorError) { emergencyStop(); delay(500); return; } 读取轮速
  readEncoders(dt);

  // 差速PID（简化闭环）
  int leftPWM = targetLeftSpeed + (encLeft - lastEncLeft)/dt;
  int rightPWM = targetRightSpeed + (encRight - lastEncRight)/dt; // 驱动电机
  driveMotors(leftPWM, rightPWM);

  lastEncLeft=encLeft; lastEncRight=encRight;
  Serial.print("左速：");Serial.print((encLeft-lastEncLeft)/dt);
  Serial.print(" 右速：");Serial.print((encRight-lastEncRight)/dt);
  delay(20);
}

// 读取编码器，计算轮速
void readEncoders(unsigned long dt) {
  // 简化计算，实际需按编码器精度换算
}

// 冗余检测：轮速一致性与电机响应
bool checkRedundancy() {
  // 模拟轮速，实际由编码器读取
  int leftSpeed = 100, rightSpeed=110;
  return abs(leftSpeed-rightSpeed)>30; // 误差过大判定故障
}

// 差速驱动电机
void driveMotors(int leftPWM, int rightPWM) {
  leftPWM = constrain(leftPWM, 0, 200);
  rightPWM = constrain(rightPWM, 0, 200);
  digitalWrite(DIR_LEFT, HIGH); digitalWrite(DIR_RIGHT, HIGH);
  analogWrite(PWM_LEFT, leftPWM); analogWrite(PWM_RIGHT, rightPWM);
}

// 互锁急停，同步切断双电机
void emergencyStop() {
  digitalWrite(DIR_LEFT, LOW); digitalWrite(DIR_RIGHT, LOW);
  analogWrite(PWM_LEFT, 0); analogWrite(PWM_RIGHT, 0);
  Serial.println("紧急停机触发！");
}

适用场景优化
加入坡度检测，上下坡自动调整驱动力，防止溜坡
优化编码器滤波，减少抖动干扰，提升轮速检测精度
增加温度检测，电机过热时自动降速或急停，保护设备

5、冗余容错+双急停互锁+故障自动恢复程序（核心：冗余容错+安全联锁）
适用场景：火灾现场浓烟、高温环境，需应对单侧电机故障、信号干扰，保障机器人能安全撤离或作业。

核心逻辑
左右轮独立闭环控制，实现动力冗余，单侧故障可单驱动运行
双路急停联锁，任意急停触发即断电，且重启需双急停复位
故障诊断+自动恢复，非致命故障可自动复位重启，避免作业中断

// 双BLDC冗余容错消防机器人程序
// 硬件：双BLDC、双重编码器、双急停、故障检测、PWM驱动
// 功能：独立闭环、冗余容错、联锁急停、故障恢复

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM[2] = {9,10}, DIR[2]={8,11};
const byte ENC_A[2]={2,4}, ENC_B[2]={3,5};
const byte ESTOP[2]={12,13};
const byte FAULT_PIN=A0; // 故障检测

// ========== 状态变量 ==========
bool estopState[2]={true,true};
bool motorFault[2]={false,false};
bool recoveryRequest=false;
int targetSpeed[2]={120,120};
long encCount[2]={0,0}, lastEnc[2]={0,0};
unsigned long lastTime=0;

// ========== 函数声明 ==========
void readEncoders();
bool estopInterlock();
bool checkMotorFault();
void handleRecovery();
void driveMotor(uint8_t idx, int pwm);

void setup() {
  Serial.begin(115200);
  Serial.println("冗余容错系统启动");

  for(uint8_t i=0;i<2;i++) {
    pinMode(PWM[i],OUTPUT); pinMode(DIR[i],OUTPUT);
  pinMode(ESTOP[i], INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_A[i]), [i](){ if(digitalRead(ENC_A[i])) encCount[i]++; }, RISING);
    }
pinMode(FAULT_PIN, INPUT);
}

void loop() {
  // 互锁急停检测
  if (!estopInterlock()) {
    driveMotor(0,0); driveMotor(1,0);
    Serial.println("互锁急停生效");
  }

  // 故障检测
  motorFault[0] = checkMotorFault();
  motorFault[1] = checkMotorFault();
  if (motorFault[0] || motorFault[1]) handleRecovery();

  // 编码器读取
  unsigned long dt=millis()-lastTime;  lastTime=millis();
  readEncoders();

  // 独立闭环，容错控制
  int pwm[2];
  for (uint8_t i=0;i<2;i++) {
    if (!motorFault[i]) {      pwm[i] = targetSpeed[i] + (encCount[i]-lastEnc[i])/dt;
    } else {
      pwm[i] = 0;  // 故障侧停转，对侧运行
    }
    driveMotor(i, pwm[i]);
  }

  lastEnc[0]=encCount[0]; lastEnc[1]=encCount[1];
  delay(30);
}

// 急停联锁，双通道触发即停机
bool estopInterlock() {
  estopState[0] = digitalRead(ESTOP[0]);
  estopState[1] = digitalRead(ESTOP[1]);
  return estopState[0] && estopState[1]; // 仅双复位才恢复
}

// 电机故障检测，电流/转速异常判定
bool checkMotorFault() {
  uint16_t faultVal = analogRead(FAULT_PIN);
  return faultVal > 800; // 模拟故障阈值
}

// 故障恢复处理
void handleRecovery() {
  Serial.println("检测到故障，尝试恢复");
  driveMotor(0,0); driveMotor(1,0);
  recoveryRequest=true;
  delay(1000);
  if (estopInterlock()) { // 复位后恢复运行
    motorFault[0]=motorFault[1]=false;
    Serial.println("故障恢复，重新启动");
  } else {
    Serial.println("复位失败，保持停机");
  }
}

// 读取编码器
void readEncoders() {}

// 单电机驱动
void driveMotor(uint8_t idx, int pwm) {
  pwm = constrain(pwm,0,200);
  digitalWrite(DIR[idx], HIGH);
  analogWrite(PWM[idx], pwm);
}

适用场景优化
加入烟雾浓度检测，浓烟时自动降速，保障视野与通信
优化故障诊断逻辑，细分过流、堵转、过热等故障类型，分级处理
增加远程急停控制，支持后台远程触发互锁停机

6、双轮协同转向+消防作业协同控制程序（核心：协同转向+作业联动）
适用场景：大型火场作业，机器人需边行驶边喷水/送风，双轮精准协同转向，保障作业精度。

核心逻辑
双BLDC差速实现原地转向、曲线行驶，转向精度可控
行驶速度与作业设备联动，移动时自动控制消防水炮/风机启停
互锁急停联动作业设备，停机时同步关闭作业，保障安全

// 双BLDC协同转向+消防作业程序
// 硬件：双BLDC、编码器、水炮/风机、急停、PWM驱动
// 功能：精准转向、协同行驶、作业联动、安全急停

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte MOTOR_PWM[2]={9,10}, MOTOR_DIR[2]={8,11};
const byte ENC_PIN[2]={2,4};
const byte ESTOP = 12;
const byte WATER_VALVE=13, FAN=14;

// ========== 控制参数 ==========
int targetLeft=130, targetRight=130;
int turnAngle = 0; // 转向角度（正右，负左）
long enc[2]={0,0}, lastEnc[2]={0,0};
bool workActive=false;

// ========== 函数声明 ==========
void decodeTurn(int angle);
bool estopCheck();
void controlWork();
void driveMotors(int l, int r);

void setup() {
  Serial.begin(115200);
  Serial.println("消防作业协同系统启动");

  for(uint8_t i=0;i<2;i++) {
    pinMode(MOTOR_PWM[i], OUTPUT);    pinMode(MOTOR_DIR[i], OUTPUT);
  }  pinMode(ESTOP, INPUT_PULLUP);
  pinMode(WATER_VALVE, OUTPUT); pinMode(FAN, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_PIN[0]), [](){ enc[0]++; }, RISING);  attachInterrupt(digitalPinToInterrupt(ENC_PIN[1]), [](){ enc[1]++; }, RISING);
}

void loop() {
  // 急停检测
  if (!estopCheck()) {
    driveMotors(0,0); digitalWrite(WATER_VALVE,LOW); digitalWrite(FAN,LOW);
    Serial.println("急停触发，作业停止");
    while(!estopCheck()) delay(100);
  }

  // 解码转向指令
  decodeTurn(turnAngle);

  // 轮速闭环
  unsigned long dt = millis();
  int leftPWM = targetLeft + (enc[0]-lastEnc[0]);
  int rightPWM = targetRight + (enc[1]-lastEnc[1]);
  driveMotors(leftPWM, rightPWM);

  // 作业联动
  controlWork();

  lastEnc[0]=enc[0]; lastEnc[1]=enc[1];
  Serial.print("左速：");Serial.print(targetLeft);
  Serial.print(" 右速：");Serial.print(targetRight);
  delay(20);
}

// 转向指令解码，差速实现转向
void decodeTurn(int angle) {
  if (angle >50) { targetLeft=100; targetRight=160; }
  else if (angle <50) { targetLeft=160; targetRight=100; }
  else { targetLeft=130; targetRight=130; }
}

// 急停检测
bool estopCheck() { return digitalRead(ESTOP)==HIGH; }

// 作业联动
void controlWork() {
  if (abs(targetLeft-targetRight) <20) { // 直行/微调开启作业
    digitalWrite(WATER_VALVE,HIGH); digitalWrite(FAN,HIGH); workActive=true;
  } else { // 转向时减少作业，避免偏移
    digitalWrite(WATER_VALVE,LOW); digitalWrite(FAN,LOW); workActive=false;
  }
}// 双电机驱动
void driveMotors(int leftPWM, int rightPWM) {
  leftPWM = constrain(leftPWM,0,200);
  rightPWM = constrain(rightPWM,0,200);
  digitalWrite(MOTOR_DIR[0], HIGH); digitalWrite(MOTOR_DIR[1], HIGH);
  analogWrite(MOTOR_PWM[0], leftPWM); analogWrite(MOTOR_PWM[1], rightPWM);
}

适用场景优化
加入作业距离检测，精准控制水炮喷射角度，提升灭火效率
实现转向记忆功能，复杂场景下重复精准转向，适配火场布局
联动环境传感器，高温区域自动加大作业强度，提升灭火效果

要点解读
要点1：双路冗余设计，提升设备可靠性
消防作业环境恶劣，单侧电机/线路故障可能导致机器人瘫痪，双路冗余是核心保障。
硬件冗余：左右轮独立电机、驱动电路、编码器、急停按键，关键部件双份配置，避免单点故障
动力冗余：单侧故障时，对侧电机可独立运行，支撑机器人撤离或继续作业，降低作业中断风险
信号冗余：关键控制信号双路传输，减少信号丢失或干扰，保障控制指令稳定下达
要点2：互锁急停必须双路联动，杜绝误动作
急停是消防机器人的安全底线，互锁设计能避免单开关失效导致的安全事故。
双通道联锁：左右急停按键串联，任意一个触发即切断总电源，防止单急停损坏后失效
强制复位逻辑：急停触发后，必须双路同时复位才能重启，避免误复位或单复位启动
联动作业停机：急停触发时，行驶电机与作业设备同步断电，彻底终止动作，保障现场安全
要点3：独立闭环控制，保障行走与转向精度
双轮独立控制是轮式机器人灵活转向的基础，闭环反馈可抵消负载、地形干扰。
独立PID控制：左右轮各成闭环，分别采集编码器反馈，精准控制转速，避免差速偏差
差速转向精准：通过左右轮转速差实现转向、原地转弯，转向角度可控，适配窄小火场空间
动态补偿：负载变化、地面坡度干扰时，实时调整电机输出，保持速度与转向稳定
要点4：故障诊断+容错运行，兼顾安全与作业效率
火场情况复杂，需精准识别故障并分级处理，避免因小故障完全停机影响救援。

分级故障判定：区分致命故障（堵转、短路）和非致命故障（信号波动），致命故障立即急停，非致命故障可降速运行
自动恢复机制：非致命故障延时复位，自动重启运行，减少作业中断，提升救援连续性
预警提示：故障发生时及时上报后台，提示运维人员，便于快速排查隐患
要点5：行驶与作业联动，适配消防场景实际需求
消防机器人核心是救援作业，行驶控制需与灭火、送风等作业功能协同。
速度-作业联动：直行稳定时全功率作业，转向时降低作业强度，避免移动偏移影响作业精度
环境自适应调整：浓烟、高温环境自动调整行驶速度与作业功率，保障机器人稳定与灭火效果
协同调度：支持多机协同，双轮控制与作业系统统一调度，适配大型火场联合救援

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。