在这里插入图片描述
Arduino BLDC 之消防机器人双节点冗余 BLDC 控制 + 故障切换，核心是在“高温、烟雾、通信受阻、负载突变”等高危工况下，用两个独立控制节点互为备份：正常时主节点执行 FOC 运动控制、从节点同步监控；主节点失效时，从节点在毫秒级接管电机驱动，使机器人进入安全停机、低速撤离或受控返航，而不是直接失控。 它的关键不是简单“多放一块 MCU”，而是把故障检测、状态同步、切换决策和安全降级做成闭环。

主要特点
. 双节点主从热备，避免单点故障
典型架构中，主节点负责 BLDC FOC 控制、速度/位置闭环和任务执行；从节点通过 UART、CAN 或 SPI 持续接收主节点心跳、状态帧和关键控制参数。主节点正常时，从节点不直接驱动电机，但保持同步；主节点失联、看门狗超时或关键传感器异常时，从节点接管。
在 Arduino/ESP32 生态中，可用两块 MCU 分别作为主从节点，主节点运行 SimpleFOC 或 FOC 速度环，从节点运行监控与接管逻辑。
. 故障检测采用“硬件 + 软件”多层判据
单靠串口超时不足以可靠判断故障。更稳健的做法是组合多种信号：
心跳包周期检测：主节点每 5—20 ms 发送一次状态帧。
看门狗保护：主节点软件跑飞或死循环时，硬件看门狗触发复位。
关键传感器校验：编码器失联、IMU 数据异常、电流采样饱和、母线电压异常时判定故障。
通信 CRC 校验：防止噪声导致误判或误接管。
连续多次失败确认：避免瞬时干扰触发切换。
. 故障切换强调“无扰接管”和“安全状态优先”
切换时不能直接把电机指令置零或随意改写 PWM，否则可能产生冲击、急停、跑偏甚至机械损伤。更合理的流程是：从节点确认主节点失效 → 读取最近一次有效速度/电流指令 → 判断当前是否处于危险区域 → 选择安全策略 → 平滑接管输出。
安全策略通常包括：立即切断扭矩输出、低速直线撤离、原地停机报警、返回最近安全点或进入遥控接管模式。
. 与 BLDC 执行层深度耦合
消防机器人通常负载大、惯量大，BLDC 驱动需要可靠的电流采样、死区时间、过流保护和母线电压监测。 双节点冗余不能只解决“MCU 挂了怎么办”，还要保证驱动级故障不会导致桥臂直通、过流烧毁或电机失控。例如主驱动板异常时，从节点应能独立关断使能信号或切换至备份驱动通道。
. 分层安全架构更适合消防场景
完整系统可划分为：
执行层：BLDC 电机、编码器、驱动桥、电流/电压/温度检测。
控制层：主节点 FOC 闭环、从节点监控与接管。
安全层：硬件急停、STO/EN 切断、看门狗、独立电源监控。
通信层：主从节点同步、远程遥控链路、视频/传感器回传。
任务层：火场侦察、路径规划、灭火作业、人员跟随。
Arduino 级别平台适合做轻量双节点原型；若用于真实消防场景，通常还需要工业级 MCU、隔离电源、CAN 总线、硬件急停和符合功能安全要求的驱动设计。

应用场景
火场侦察机器人：进入浓烟、高温、坍塌风险区域时，主控一旦受干扰或失效，从节点可保证机器人停机或撤离，避免阻塞通道。
灭火作业底盘：携带水炮、泡沫罐或干粉系统时，负载变化大，驱动节点故障可能导致姿态失控，冗余控制可降低二次事故风险。
危化品环境巡检：在可能存在爆炸性气体或粉尘的环境中，控制系统可靠性要求更高，双节点冗余可作为功能安全架构的一部分。
地下空间与隧道救援：通信易受遮挡，远程遥控链路不稳定，本地双节点冗余能提升自主安全能力。
教学与科研平台：可用于验证双 MCU 热备、心跳同步、故障切换、FOC 安全停机和嵌入式看门狗机制。

需要注意的事项
主从节点必须电源和复位独立
如果两块 MCU 共用同一路 LDO、同一复位电路或同一噪声敏感电源，主节点故障可能同时拖垮从节点。建议控制电源与动力电源隔离，主从节点供电尽量独立，并加入过压、欠压和浪涌保护。
切换逻辑要先定义“安全状态”
切换不是简单地把控制权交给从节点，而要先回答：当前应该停机、撤离、保持姿态，还是等待遥控？消防机器人尤其要避免“切换成功但机器人继续向前冲”的情况。
电机驱动级也要考虑冗余或安全关断
仅冗余 MCU 无法覆盖驱动 MOSFET 损坏、电流采样失效、桥臂直通等故障。至少应保留硬件级过流保护、死区时间、EN/STO 切断和急停回路。 更高可靠方案可采用双驱动通道、双绕组电机或独立使能控制。
通信链路要防误判
火场环境中电磁干扰、高温和线缆损伤都可能导致串口或 CAN 数据异常。应加入心跳超时、CRC 校验、连续失败计数和故障确认窗口，避免因为一次丢包就触发切换。
切换过程要限制扭矩突变
从节点接管时，应继承最近一次有效指令，并对电流、速度和 PWM 占空比加入斜率限制。否则即使切换成功，也可能造成轮胎打滑、底盘偏航或机械冲击。
Arduino 原型与工程化产品要区分边界
用 Arduino/ESP32 做双节点冗余验证是可行的，但真实消防机器人还需考虑防护等级、防爆要求、防水防尘、电池 BMS、热管理和功能安全认证。 原型阶段可先实现“双 MCU 心跳 + 看门狗 + 安全停机”，再逐步加入状态同步、无扰切换和驱动级冗余。

在这里插入图片描述
1、双节点心跳监测 + 主备无缝切换
消防机器人常采用双 MCU 节点架构：主节点运行 FOC 控制，备节点同步监测并准备接管。核心是心跳超时检测与状态快速恢复。

#include <SimpleFOC.h>
#include <Arduino.h>

// ===== 硬件配置 =====
#define MY_ROLE 0  // 0=主节点, 1=备节点
BLDCMotor motor(7);
BLDCDriver3PWM driver(9, 5, 6, 8);
Encoder encoder(2, 4, 2048);

// ===== 节点间通信（简化使用Serial1，实际建议CAN/RS485）=====
#define HEARTBEAT_TIMEOUT 300  // ms，超时判定主节点故障

// ===== 运行状态 =====
enum RunMode {
  MODE_INIT,
  MODE_NORMAL,     // 双节点正常，主控运行
  MODE_DEGRADED,   // 降级：单节点驱动
  MODE_ESTOP       // 急停
};

RunMode currentMode = MODE_INIT;
unsigned long lastHeartbeat = 0;
unsigned long heartbeatCounter = 0;

// 关键状态同步结构（主节点发给备节点）
struct ControlState {
  float targetSpeed;
  float kp;
  float ki;
  uint16_t crc;
};

ControlState sharedState = {5.0, 0.5, 10.0, 0};

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);  // 节点间通信

  // 电机初始化
  motor.linkSensor(&encoder);
  driver.voltage_power_supply = 24;
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = sharedState.kp;
  motor.PID_velocity.I = sharedState.ki;
  motor.init();
  motor.initFOC();

  lastHeartbeat = millis();
  currentMode = MODE_NORMAL;
}

// 发送心跳包（带状态和CRC）
void sendHeartbeat() {
  Serial1.print("HB,");
  Serial1.print(MY_ROLE);
  Serial1.print(",");
  Serial1.print(sharedState.targetSpeed, 2);
  Serial1.print(",");
  Serial1.print(sharedState.kp, 3);
  Serial1.print(",");
  Serial1.print(sharedState.ki, 2);
  Serial1.print(",");
  Serial1.println(sharedState.crc);
}

// 接收对端心跳
void receiveHeartbeat() {
  if (Serial1.available()) {
    String data = Serial1.readStringUntil('\n');
    if (data.startsWith("HB,")) {
      lastHeartbeat = millis();
      // 备节点：同步主节点控制参数
      if (MY_ROLE == 1) {
        int comma1 = data.indexOf(',');
        int comma2 = data.indexOf(',', comma1 + 1);
        int comma3 = data.indexOf(',', comma2 + 1);
        int comma4 = data.indexOf(',', comma3 + 1);
        sharedState.targetSpeed = data.substring(comma1 + 1, comma2).toFloat();
        sharedState.kp = data.substring(comma2 + 1, comma3).toFloat();
        sharedState.ki = data.substring(comma3 + 1, comma4).toFloat();
      }
    }
  }
}

void loop() {
  motor.loopFOC();

  sendHeartbeat();
  receiveHeartbeat();

  // 故障检测：心跳超时
  if (millis() - lastHeartbeat > HEARTBEAT_TIMEOUT) {
    if (MY_ROLE == 1) {
      // 备节点接管：恢复主节点最后的状态
      motor.PID_velocity.P = sharedState.kp;
      motor.PID_velocity.I = sharedState.ki;
      currentMode = MODE_DEGRADED;
      Serial.println("[切换] 备节点接管控制");
    } else {
      // 主节点失联（可能自身故障），尝试恢复或安全停机
      currentMode = MODE_ESTOP;
      Serial.println("[故障] 主节点心跳丢失，进入急停");
    }
  }

  // 执行控制
  switch (currentMode) {
    case MODE_NORMAL:
    case MODE_DEGRADED:
      motor.move(sharedState.targetSpeed);
      break;
    case MODE_ESTOP:
      motor.move(0);
      break;
  }

  delay(10);
}

切换逻辑：备节点持续接收主节点的心跳与控制参数，超时后利用同步的 PID 参数和速度指令直接接管，避免切换瞬间的扭矩阶跃。

2、双节点多数表决 + 故障隔离
两个控制节点同时对编码器数据和电流采样进行交叉校验，通过一致性检查判定故障节点并隔离。

#include <SimpleFOC.h>

// ===== 双节点配置 =====
BLDCMotor motorA(7);  // 节点A电机
BLDCMotor motorB(7);  // 节点B电机（冗余）
Encoder encoderA(2, 3, 2048);
Encoder encoderB(4, 5, 2048);

// ===== 故障状态 =====
struct NodeHealth {
  bool nodeA_healthy;
  bool nodeB_healthy;
  float speedA, speedB;
  float currentA, currentB;
} health = {true, true, 0, 0, 0, 0};

// 一致性阈值
const float SPEED_TOLERANCE = 0.5;   // rad/s
const float CURRENT_TOLERANCE = 0.3; // A

// 投票输出
float finalSpeed = 0;

void setup() {
  Serial.begin(115200);

  // 双编码器初始化
  encoderA.init();
  encoderB.init();

  // 双电机初始化（简化）
  motorA.linkSensor(&encoderA);
  motorA.linkDriver(&driverA);
  motorA.controller = MotionControlType::velocity;
  motorA.init();
  motorA.initFOC();

  // motorB 同理...
}

// 一致性检查与表决
void consistencyCheck() {
  // 读取双路传感器数据
  health.speedA = motorA.shaft_velocity;
  health.speedB = motorB.shaft_velocity;
  // 电流读取（需配置电流传感器）
  // health.currentA = motorA.current_sense->getDCCurrent();
  // health.currentB = motorB.current_sense->getDCCurrent();

  float speedDiff = abs(health.speedA - health.speedB);

  // 速度一致性判定
  if (speedDiff > SPEED_TOLERANCE) {
    // 判断哪一路异常：与目标值比较
    float targetSpeed = motorA.target;
    if (abs(health.speedA - targetSpeed) > abs(health.speedB - targetSpeed)) {
      health.nodeA_healthy = false;
      Serial.println("[表决] 节点A速度异常，隔离");
    } else {
      health.nodeB_healthy = false;
      Serial.println("[表决] 节点B速度异常，隔离");
    }
  }

  // 电流一致性判定（过流保护）
  if (health.currentA > 3.0) {
    health.nodeA_healthy = false;
    Serial.println("[表决] 节点A过流，隔离");
  }
  if (health.currentB > 3.0) {
    health.nodeB_healthy = false;
    Serial.println("[表决] 节点B过流，隔离");
  }

  // 多数表决：健康节点驱动
  if (health.nodeA_healthy && health.nodeB_healthy) {
    finalSpeed = (health.speedA + health.speedB) / 2;  // 取均值
  } else if (health.nodeA_healthy) {
    finalSpeed = health.speedA;
  } else if (health.nodeB_healthy) {
    finalSpeed = health.speedB;
  } else {
    finalSpeed = 0;  // 双故障，安全停机
    Serial.println("[表决] 双节点故障，停机");
  }
}

void loop() {
  motorA.loopFOC();
  motorB.loopFOC();

  consistencyCheck();

  // 仅健康节点执行输出
  if (health.nodeA_healthy) {
    motorA.move(finalSpeed);
  } else {
    motorA.move(0);  // 故障节点停止输出
  }

  if (health.nodeB_healthy) {
    motorB.move(finalSpeed);
  } else {
    motorB.move(0);
  }

  delay(10);
}

仲裁机制：两个节点同时采集数据，通过速度差和电流阈值判定异常节点并隔离。正常时取均值输出，单节点故障时由健康节点独立驱动，双故障时安全停机。

3、安全状态机降级 + 硬件急停联锁
消防机器人必须具备明确的降级策略：双节点正常→单节点降级→急停。配合硬件急停回路实现失效导向安全。

#include <SimpleFOC.h>

// ===== 硬件急停（常闭接点，断线即触发）=====
#define ESTOP_PIN 7  // 常闭接点，低电平=正常

// ===== 状态机 =====
enum SystemState {
  STATE_IDLE,      // 空闲待命
  STATE_NORMAL,    // 双节点正常
  STATE_DEGRADED,  // 单节点降级
  STATE_ESTOP,     // 硬件急停触发
  STATE_RECOVERY   // 故障恢复
};

SystemState currentState = STATE_IDLE;
SystemState previousState = STATE_IDLE;

// ===== 节点健康 =====
bool nodeA_ok = true;
bool nodeB_ok = true;
unsigned long lastNodeA_hb = 0;
unsigned long lastNodeB_hb = 0;
#define NODE_TIMEOUT 500

// ===== 电机 =====
BLDCMotor motorA(7);
BLDCMotor motorB(7);

void setup() {
  Serial.begin(115200);
  pinMode(ESTOP_PIN, INPUT_PULLUP);  // 急停常闭接点

  // 电机初始化...
}

// 硬件急停检测（最高优先级）
bool checkEStop() {
  return digitalRead(ESTOP_PIN) == HIGH;  // 接点断开=急停
}

// 节点健康监测
void checkNodeHealth() {
  // 通过CAN/串口接收心跳
  // if (receivedFromA) lastNodeA_hb = millis();
  // if (receivedFromB) lastNodeB_hb = millis();
  
  nodeA_ok = (millis() - lastNodeA_hb < NODE_TIMEOUT);
  nodeB_ok = (millis() - lastNodeB_hb < NODE_TIMEOUT);
}

// 状态转换逻辑
void updateStateMachine() {
  // 硬件急停：最高优先级，直接跳转
  if (checkEStop()) {
    currentState = STATE_ESTOP;
    return;
  }

  // 根据节点健康度决定状态
  switch (currentState) {
    case STATE_NORMAL:
      if (!nodeA_ok && !nodeB_ok) {
        currentState = STATE_ESTOP;  // 双故障，急停
      } else if (!nodeA_ok || !nodeB_ok) {
        currentState = STATE_DEGRADED;  // 单故障，降级
      }
      break;

    case STATE_DEGRADED:
      if (nodeA_ok && nodeB_ok) {
        currentState = STATE_NORMAL;  // 恢复
      } else if (!nodeA_ok && !nodeB_ok) {
        currentState = STATE_ESTOP;  // 恶化，急停
      }
      break;

    case STATE_ESTOP:
      // 急停解除且节点恢复，进入恢复流程
      if (!checkEStop() && (nodeA_ok || nodeB_ok)) {
        currentState = STATE_RECOVERY;
        previousState = STATE_NORMAL;
      }
      break;

    case STATE_RECOVERY:
      // 恢复后回到正常或降级
      if (nodeA_ok && nodeB_ok) {
        currentState = STATE_NORMAL;
      } else if (nodeA_ok || nodeB_ok) {
        currentState = STATE_DEGRADED;
      }
      break;
  }
}

void loop() {
  checkNodeHealth();
  updateStateMachine();

  // 根据状态执行控制
  switch (currentState) {
    case STATE_NORMAL:
      // 双节点协同驱动
      motorA.move(5.0);
      motorB.move(5.0);
      break;

    case STATE_DEGRADED:
      // 单节点降额运行（扭矩减半，速度降低）
      if (nodeA_ok) {
        motorA.move(2.5);  // 降速
        motorB.move(0);
      } else {
        motorA.move(0);
        motorB.move(2.5);
      }
      break;

    case STATE_ESTOP:
      // 硬件急停：电机断电
      motorA.move(0);
      motorB.move(0);
      // 实际项目应通过驱动使能引脚直接切断
      break;

    case STATE_RECOVERY:
      // 恢复流程：缓慢加速回正常
      motorA.move(1.0);
      motorB.move(1.0);
      break;
  }

  motorA.loopFOC();
  motorB.loopFOC();
  delay(10);
}

安全设计：硬件急停采用常闭接点串入驱动使能回路，断线即触发，不依赖软件逻辑。状态机管理“正常→降级→急停→恢复”的完整生命周期，降级时对剩余健康节点做降额处理。

要点解读

  1. 心跳机制是双节点冗余的“生命体征”
    双节点架构中，备节点必须持续感知主节点的存活状态。心跳包不仅要传递“活着”的信号，还应携带关键控制参数（目标速度、PID 增益）。超时阈值通常设为控制周期的 10-20 倍（如 500ms），过短易误触发，过长则切换延迟大。

  2. 状态同步决定切换是否“无扰”
    故障切换最怕“状态不一致”——主节点以 5 rad/s 运行，备节点接管时却从 0 开始加速，会造成机械冲击。备节点必须实时同步主节点的目标速度、PID 积分项、FOC 电角度。案例一中的 ControlState 结构体就是这一思路的简化实现。

  3. 多数表决只能容忍“少数故障”
    双节点表决的局限在于：两个节点各执一词时，无法判断谁对谁错（缺少仲裁者）。因此双节点通常配合第三个参考源（如独立的电流传感器、IMU 或看门狗定时器）来打破僵局。若只有两路，则需依赖“与目标值偏差”来判定异常方。

  4. 硬件急停必须“绕过软件”
    消防机器人一旦失控，后果严重。软件急停（在 loop 中检测标志位）依赖 MCU 正常运行，存在死机盲区。正确的做法是：急停按钮串联在驱动器的使能引脚上，采用常闭接点——断线即断使能，实现失效导向安全。软件只负责监测急停状态并上报，不参与“是否切断动力”的决策。

  5. 降级策略要“有梯度、可恢复”
    双节点冗余不应在单故障时直接“全停”，而应有明确的降级梯度：双节点正常→单节点降额运行（降速/降扭矩）→双故障安全停机。同时，故障恢复后应通过恢复状态逐步回到正常，避免直接跳变。这要求代码中维护清晰的 SystemState 状态机，且每次转换都有明确的触发条件和动作。

在这里插入图片描述
4、主从被动冗余BLDC控制+故障切换
适用场景：中小型消防侦察机器人，主节点负责动力与控制，从节点热备，主节点失效时从节点0延迟接管。

#include <SimpleFOC.h>
#include <ArduinoRS485.h>
RS485 rs485(Serial2); // 主从通讯总线
SimpleFOCMotor2 motor_main = SimpleFOCMotor2(8,9,10, A2,A3);
SimpleFOCMotor2 motor_backup = SimpleFOCMotor2(5,6,11, A0,A1);
enum NodeState{ NORMAL, FAULT, TAKEOVER };
NodeState main_state = NORMAL;
NodeState backup_state = BACKUP;

void setup() {  Serial.begin(9600);
  motor_main.pole_pairs=15; motor_backup.pole_pairs=15;
  motor_main.voltage_limit=8; motor_backup.voltage_limit=8;
  motor_main.init(); motor_backup.init();
  motor_main.controller=FOC_VELOCITY; motor_backup.controller=FOC_VELOCITY;
  rs485.begin(9600);
}

void check_main_status(int heartbeat_cmd){
  static unsigned long last_heartbeat_time = 0;
  unsigned long now = millis();
  // 主节点100ms未发送心跳判定故障
  if(now - last_heartbeat_time > 100 && main_state == NORMAL){
    main_state = FAULT;
    backup_state = TAKEOVER;
  }
}

void loop(){
  delay(1);
  if(backup_state == TAKEOVER){
    // 从节点接管，发送新心跳
    rs485.print("TAKEOVER");
    float speed_cmd = 150.0f;
    float u = 1.2f * id(speed_cmd - motor_backup.shaft_velocity) + 0.05f*id(speed_cmd - motor_backup.shaft_velocity);
    motor_backup.setVoltage(constrain(u,-8,8));
    motor_main.setVoltage(0); // 禁用主节点
  } else {
    // 主节点正常工作，更新心跳
    rs485.print("HEARTBEAT");
    float speed_cmd = 150.0f;
    float u = 1.2f*id(speed_cmd - motor_main.shaft_velocity) + 0.05f*id(speed_cmd - motor_main.shaft_velocity);
    motor_main.setVoltage(constrain(u,-8,8));
    motor_backup.setVoltage(0); // 从节点热备零出力
  }
}

5、双节点全冗余热备+扭矩同步控制
适用场景：中大型消防救援机器人，双节点均具备完整控制能力，正常运行时扭矩同步，单节点故障时无缝衔接。

#include <SimpleFOC.h>
#include <ArduinoRS485.h>
RS485 rs485(Serial2);
SimpleFOCMotor2 motor1 = SimpleFOCMotor2(8,9,10, A2,A3);
SimpleFOCMotor2 motor2 = SimpleFOCMotor2(5,6,11, A0,A1);
enum FaultType{ NONE, COMM_FAULT, OVERCURRENT, OVERTEMP };
FaultType fault_status = NONE;
int current_torque_cmd = 0;

bool check_self_fault(){
  // 故障自检：过流、过温、驱动异常
  if(abs(motor1.current) > 15 || abs(motor2.current) > 15) return true;
  if(analogRead(A4)/10.0f > 85.0f) return true; // 温度超阈值
  return false;
}

void sync_torque(int cmd){
  // 双节点扭矩同步，保证动力输出一致
  float u1 = cmd * 0.08f;
  float u2 = cmd * 0.08f;
  motor1.setVoltage(constrain(u1,-8,8));
  motor2.setVoltage(constrain(u2,-8,8));
}

void setup(){
  Serial.begin(9600);
  motor1.pole_pairs=15; motor2.pole_pairs=15;
  motor1.init(); motor2.init();
  motor1.controller=FOC_VELOCITY; motor2.controller=FOC_VELOCITY;
  rs485.begin(9600);
}

void loop(){
  delay(1);
  if(check_self_fault()){
    if(fault_status == NONE){
      fault_status = OVERCURRENT;
      rs485.print("FAULT"); // 上报总控
    }
    // 故障节点停机，另一节点独立接管
    motor1.setVoltage(0);
    motor2.setVoltage(1.0f * id(150 - motor2.shaft_velocity));
  } else {
    if(fault_status != NONE) fault_status = NONE;
    int torque = 100;
    rs485.print("SYNC");
    sync_torque(torque);
  }
}

6、故障降级控制+断网独立运行
适用场景：地下/浓烟环境消防机器人，通讯完全中断时，节点可根据自身状态实现降级运行，维持基本移动能力。

#include <SimpleFOC.h>
SimpleFOCMotor2 motorL = SimpleFOCMotor2(8,9,10, A2,A3);
SimpleFOCMotor2 motorR = SimpleFOCMotor2(5,6,11, A0,A1);
bool comm_lost = false;
bool node1_fault = false;
bool node2_fault = false;
int cmd_footprint[3] = {120,0,0}; // 离线默认指令：直行

void check_fault(){
  if(abs(motorL.current)>18 || analogRead(A4)/10.0f>90.0f) node1_fault=true;
  if(abs(motorR.current)>18 || analogRead(A5)/10.0f>90.0f) node2_fault=true;
}

void setup(){
  Serial.begin(9600);
  motorL.pole_pairs=15; motorR.pole_pairs=15;
  motorL.voltage_limit=8; motorR.voltage_limit=8;
  motorL.init(); motorR.init();
  motorL.controller=FOC_VELOCITY; motorR.controller=FOC_VELOCITY;
}

void loop(){
  delay(1);
  check_fault();
  unsigned long now = millis();
  static unsigned long last_comm_time=0;
  // 连续2s未收到外部指令判定通讯中断
  if(now - last_comm_time > 2000) comm_lost = true;
  int speed_cmd = comm_lost ? cmd_footprint[0] : 150;
  if(!comm_lost){
    // 通讯正常，受控运行
    if(!node1_fault && !node2_fault){
      float uL = 1.0f*id(speed_cmd - motorL.shaft_velocity);
      float uR = 1.0f*id(speed_cmd - motorR.shaft_velocity);
      motorL.setVoltage(constrain(uL,-8,8));
      motorR.setVoltage(constrain(uR,-8,8));
    } else if((!node1_fault) || (!node2_fault)){
      // 单节点故障，降级为单电机慢速运行
      if(!node1_fault){
        motorL.setVoltage(0.5f * id(60 - motorL.shaft_velocity));
        motorR.setVoltage(0);
      } else {
        motorR.setVoltage(0.5f * id(60 - motorR.shaft_velocity));
        motorL.setVoltage(0);
      }
    }
  } else {
    // 通讯中断，执行预存足迹指令，单节点故障则停机等待救援
    if(!node1_fault && !node2_fault){
      float uL = 0.8f * id(speed_cmd - motorL.shaft_velocity);
      float uR = 0.8f * id(speed_cmd - motorR.shaft_velocity);
      motorL.setVoltage(constrain(uL,-8,8));
      motorR.setVoltage(constrain(uR,-8,8));
    } else {
      motorL.setVoltage(0); motorR.setVoltage(0);
    }
  }
}

要点解读
冗余设计的故障触发需多条件判定，避免误切
需同时满足「心跳超时/通讯中断+电流/温度异常」双重条件才触发切换，单一异常仅上报预警，可大幅降低主节点正常工作时误切换的概率。

切换过程需做软启动，禁止直接满功率输出
故障切换后需限制初始电压上限、缓慢提升增益，避免电压突加引发电机堵转或惯性冲击，消防场景下可减少对机身结构与搭载设备的损伤。

双冗余需实现扭矩同步而非速度同步
消防机器人负载大，需保证双电机输出扭矩一致，避免因力差导致偏航或结构受扭，正常运行时需加入闭环扭矩均衡逻辑，而非仅同步速度指令。

必须设计离线降级能力，应对极端通讯中断
地下/浓烟场景极易出现通讯完全中断，需预存简单运动指令（如直行、原地转向），保证机器人具备基本撤离能力，避免完全失联。

故障需分级处理，实现从预警到停机的多层保障
将故障分为轻微（温度偏高）、中度（电流过载）、重度（驱动失效）三级，分别对应降功率、单节点运行、紧急停机，既保障作业连续性又避免过载烧毁硬件。

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

在这里插入图片描述

# 嵌入式硬件 # 单片机 # c++
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

华为eNSP三层交换机实验全解析

avatar DAMO开发者矩阵
cover

把城市压缩成 2067 个盒子：CosFly 无人机跟踪数据管线论文解剖

avatar DAMO开发者矩阵
cover

（二十四）华为华三锐捷迈普思科 QinQ 双层标签配置命令（双层 VLAN 五厂商对照）

avatar DAMO开发者矩阵