在机器人控制算法的底层架构中，基于Arduino的BLDC硬件急停状态机，是保障系统安全与运动控制鲁棒性的核心防线。结合独立中断响应与分级制动策略，这套机制能够确保机器人在面临突发风险时，以最优的动力学路径实现安全停机。

一、核心机制解析

1. 独立中断响应（抢占式最高优先级）
   在底层控制逻辑中，急停信号具有绝对的“一票否决权”。通过配置外部中断（如Arduino的 attachInterrupt），系统能够绕过主循环（loop）中可能存在的任何耗时操作（如串口通信、复杂算法解算或传感器轮询），在微秒级时间内强制接管控制权。
   失效安全（Fail-Safe）设计：硬件上通常采用常闭（NC）型急停开关串联在控制回路中。这意味着一旦发生线路断开、接插件松动或电源波动，系统会默认触发中断，强制切断电机动力，确保“故障导向安全”。
   逻辑隔离：中断服务函数（ISR）内部必须保持极简，仅执行全局状态标志位的翻转或直接清零PWM输出，严禁在ISR中进行复杂的浮点运算或状态机跳转，以防阻塞后续中断。
2. 分级制动策略（动力学平滑过渡）
   单纯的“断电”会导致BLDC电机因惯性滑行，或在重载下引发机械冲击。分级制动通过状态机将停机过程拆解为多个受控阶段：
   软急停（受控减速）：针对非致命风险（如前方检测到动态障碍物），状态机进入减速分支。通过逐步降低速度环的设定值，利用电机的反电动势或主动施加反向力矩，实现平滑减速至零。
   硬急停（0类停止）：针对极端危险（如物理碰撞或人工拍下急停按钮），状态机直接切断电机使能（Disable），并触发电子刹车（如短接BLDC绕组实现动态制动），强制电机瞬间锁死。

二、典型应用场景
人机协作与服务机器人：在餐厅、商场等动态环境中，当视觉或雷达传感器捕捉到行人突然闯入安全包络线时，系统触发软急停，平滑减速以避免因急刹导致的货物倾倒或机械臂剧烈晃动。
高速移动底盘（AGV/AMR）：在高速运行（如8-10m/s）状态下，若发生通信丢失或底层硬件故障，独立中断机制能瞬间切断动力，防止机器人失控冲出安全区域。
工业机械臂与自动化设备：在调试或维护阶段，操作人员可通过硬件急停按钮随时切断伺服/BLDC驱动器的使能信号，确保人员进入工作半径时的绝对安全。

三、关键注意事项
状态机的原子性与互斥：在编写状态机代码时，必须确保“运行”、“减速”、“停止”等状态之间的切换条件严格互斥。例如，正反转切换必须经过“停转”中间态，让电流先衰减到安全范围，否则瞬间的反向电动势极易击穿驱动MOS管。
避免“死循环”与逻辑竞争：在实现反向制动（利用反转力矩克服惯性）时，需设置严格的超时或速度阈值退出条件。若逻辑不严密，可能导致电机在“正转停止”与“反转制动”之间无限循环切换，引发接触器或继电器频繁吸合释放。
能量管理与电气冲击：BLDC电机在急停瞬间，巨大的动能会转化为电能回馈至直流母线，可能导致电压骤升触发过压保护。在硬件设计上，需预留足够的电气裕量，或增加缓冲电容/泄放电路来吸收峰值能量。
复位机制的严谨性：急停触发后，状态机必须保持锁定。复位操作仅能解除停止命令，严禁系统自动恢复运行。必须由操作员在确认安全后，通过明确的启动指令（如按下启动按钮）重新初始化状态机，防止“带病重启”。

1、硬件中断基础急停响应
此案例聚焦于独立中断响应的最小实现。通过硬件中断引脚连接急停按钮，当按钮触发（下降沿）时，ISR仅设置一个标志位，主循环检测到标志后执行紧急停止，确保即使主程序正在执行长任务，急停信号也不会被遗漏。

#include <SimpleFOC.h>

// BLDC电机定义
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// 急停按钮引脚（使用中断引脚2）
#define EMERGENCY_PIN 2

// 急停标志：ISR与主循环共享，必须volatile
volatile bool emergencyStop = false;

// ISR：尽可能短，只设置标志
void emergencyISR() {
  emergencyStop = true;
}

void setup() {
  Serial.begin(115200);
  
  // 初始化电机
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  
  // 配置急停按钮：上拉输入，下降沿触发（按下时拉低）
  pinMode(EMERGENCY_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(EMERGENCY_PIN), emergencyISR, FALLING);
  
  Serial.println("System ready. Press emergency button to stop.");
}

void loop() {
  // 1. 检查急停标志（在主循环中处理实际停止逻辑）
  if (emergencyStop) {
    // 立即停止电机输出
    motorL.move(0);
    motorR.move(0);
    motorL.loopFOC();
    motorR.loopFOC();
    
    Serial.println("EMERGENCY STOP ACTIVATED!");
    
    // 等待按钮释放后复位
    while (digitalRead(EMERGENCY_PIN) == LOW) {
      delay(10);
    }
    emergencyStop = false;
    Serial.println("System resumed.");
    return;
  }
  
  // 2. 正常运动控制
  motorL.target = 1.0;
  motorR.target = 1.0;
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();
  
  delay(50);
}

关键逻辑：ISR的唯一职责是标记急停事件，而非执行停止逻辑。这是因为ISR中不能使用delay()、millis()会停止递增、Serial.print可能丢失数据。主循环检测到标志后执行实际的电机停止，这种“标记-处理”模式确保了响应的即时性和安全性。

2、分级制动状态机（急停→减速→断电）
此案例实现分级制动策略。急停触发后并非立即断电，而是先执行“动态制动”（能耗制动）快速降低转速，确认安全后再切断电机使能。这种分级设计避免了高速急停对机械结构的冲击，同时确保最终的安全断电。

#include <SimpleFOC.h>

// BLDC电机定义（带使能引脚）
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(9,10,11,8), drvR(5,6,7,4);  // 使能引脚

// 急停按钮
#define EMERGENCY_PIN 2
volatile bool emergencyStop = false;

// 急停状态机
enum EStopState {
  ESTOP_IDLE,       // 正常状态
  ESTOP_BRAKING,    // 分级1：动态制动（能耗制动）
  ESTOP_CUTOFF      // 分级2：切断电机使能
};
EStopState estopState = ESTOP_IDLE;
unsigned long brakeStartTime = 0;

// 制动参数
const float BRAKE_CURRENT_LIMIT = 0.5;  // 制动电流限制(A)
const unsigned long BRAKE_DURATION = 500;  // 制动持续时间(ms)

void emergencyISR() {
  emergencyStop = true;
}

void setup() {
  Serial.begin(115200);
  
  // 初始化电机
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  
  // 急停按钮配置
  pinMode(EMERGENCY_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(EMERGENCY_PIN), emergencyISR, FALLING);
}

void loop() {
  // 1. 急停状态机
  switch (estopState) {
    case ESTOP_IDLE:
      if (emergencyStop) {
        estopState = ESTOP_BRAKING;
        brakeStartTime = millis();
        Serial.println("E-STOP: Braking phase");
        
        // 分级1：动态制动——施加反向制动电流
        motorL.target = -BRAKE_CURRENT_LIMIT;
        motorR.target = -BRAKE_CURRENT_LIMIT;
      }
      break;
      
    case ESTOP_BRAKING:
      // 维持制动电流，持续BRAKE_DURATION
      motorL.move(motorL.target);
      motorR.move(motorR.target);
      motorL.loopFOC();
      motorR.loopFOC();
      
      if (millis() - brakeStartTime > BRAKE_DURATION) {
        estopState = ESTOP_CUTOFF;
        Serial.println("E-STOP: Cutoff phase");
      }
      break;
      
    case ESTOP_CUTOFF:
      // 分级2：切断电机使能（硬件层断电）
      motorL.disable();
      motorR.disable();
      Serial.println("E-STOP: Motors disabled");
      
      // 等待按钮释放后复位
      if (digitalRead(EMERGENCY_PIN) == HIGH) {
        emergencyStop = false;
        estopState = ESTOP_IDLE;
        motorL.enable();
        motorR.enable();
        Serial.println("System reset.");
      }
      break;
  }
  
  // 2. 正常运动控制（仅在IDLE状态）
  if (estopState == ESTOP_IDLE && !emergencyStop) {
    motorL.target = 1.0;
    motorR.target = 1.0;
    motorL.move(motorL.target);
    motorR.move(motorR.target);
  }
  
  // 非制动状态下继续运行FOC
  if (estopState != ESTOP_BRAKING) {
    motorL.loopFOC();
    motorR.loopFOC();
  }
  
  delay(10);
}

关键逻辑：分级制动的核心是“先减速、后断电”。动态制动（能耗制动）通过施加反向电流将动能转化为热能，快速降低转速。当转速降至安全范围后，再切断电机使能，避免直接断电导致电机失控滑行。这种分级策略在重载机器人中尤为重要。

3、多重故障综合急停（硬件+软件双冗余）
此案例实现多重故障源的综合急停管理。除了急停按钮，还集成过流检测、通信超时、倾角超限等多个故障源，通过统一的中断标志和状态机进行分级响应。同时，设计硬件急停回路（继电器切断电机供电）作为软件失效时的最后防线。

#include <SimpleFOC.h>
#include <MPU6050.h>

// BLDC电机定义（带硬件使能）
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(9,10,11,8), drvR(5,6,7,4);

// 硬件急停继电器（常闭回路）
#define RELAY_PIN 6  // 控制继电器断开电机电源

// 故障源引脚
#define ESTOP_BUTTON 2    // 急停按钮
#define OVERCURRENT_PIN 3 // 过流检测（比较器输出）

// 故障标志（volatile，ISR共享）
volatile bool estopButtonPressed = false;
volatile bool overcurrentDetected = false;

// 综合故障状态
enum FaultLevel {
  FAULT_NONE,
  FAULT_WARNING,    // 减速预警
  FAULT_BRAKE,      // 分级制动
  FAULT_CUTOFF      // 硬件断电
};
FaultLevel currentFault = FAULT_NONE;
unsigned long faultStartTime = 0;

// IMU（倾角检测）
MPU6050 mpu;
float pitchAngle = 0;

// 通信超时检测
unsigned long lastCommTime = 0;
const unsigned long COMM_TIMEOUT = 2000;  // 2秒通信超时

// 各故障源的ISR
void estopButtonISR() {
  estopButtonPressed = true;
}

void overcurrentISR() {
  overcurrentDetected = true;
}

void setup() {
  Serial.begin(115200);
  
  // 初始化电机
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  
  // 初始化IMU
  Wire.begin();
  mpu.initialize();
  
  // 配置故障源中断
  pinMode(ESTOP_BUTTON, INPUT_PULLUP);
  pinMode(OVERCURRENT_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ESTOP_BUTTON), estopButtonISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(OVERCURRENT_PIN), overcurrentISR, FALLING);
  
  // 硬件急停继电器（常闭，通电时闭合）
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, HIGH);  // 继电器吸合，电机供电正常
  
  lastCommTime = millis();
}

void loop() {
  // 1. 多故障源检测与分级
  bool anyFault = false;
  FaultLevel faultToApply = FAULT_NONE;
  
  // 急停按钮（最高优先级）
  if (estopButtonPressed) {
    faultToApply = FAULT_CUTOFF;
    anyFault = true;
  }
  // 过流检测
  else if (overcurrentDetected) {
    faultToApply = FAULT_BRAKE;
    anyFault = true;
  }
  // 倾角超限（软件检测）
  else {
    int16_t ax, ay, az;
    mpu.getAcceleration(&ax, &ay, &az);
    pitchAngle = atan2(ay, az) * 180 / PI;
    if (abs(pitchAngle) > 45) {  // 倾角超过45度
      faultToApply = FAULT_BRAKE;
      anyFault = true;
    }
  }
  
  // 通信超时检测（软件）
  if (millis() - lastCommTime > COMM_TIMEOUT) {
    faultToApply = FAULT_WARNING;
    anyFault = true;
  }
  
  // 2. 故障状态机
  if (anyFault && faultToApply > currentFault) {
    currentFault = faultToApply;
    faultStartTime = millis();
    
    switch (currentFault) {
      case FAULT_WARNING:
        Serial.println("FAULT: Warning - reducing speed");
        break;
      case FAULT_BRAKE:
        Serial.println("FAULT: Braking");
        // 动态制动
        motorL.target = -0.5;
        motorR.target = -0.5;
        break;
      case FAULT_CUTOFF:
        Serial.println("FAULT: EMERGENCY CUTOFF");
        // 硬件断电：继电器断开，切断电机电源
        digitalWrite(RELAY_PIN, LOW);
        // 软件层同时禁用
        motorL.disable();
        motorR.disable();
        break;
    }
  }
  
  // 3. 故障恢复（仅按钮释放时允许复位）
  if (currentFault == FAULT_CUTOFF && !estopButtonPressed) {
    digitalWrite(RELAY_PIN, HIGH);
    motorL.enable();
    motorR.enable();
    currentFault = FAULT_NONE;
    overcurrentDetected = false;
    Serial.println("System reset.");
  }
  
  // 4. 正常运动（无故障时）
  if (currentFault == FAULT_NONE && !anyFault) {
    motorL.target = 1.0;
    motorR.target = 1.0;
  }
  
  // 5. 执行电机控制
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 模拟通信心跳
  if (millis() % 1000 < 10) lastCommTime = millis();
  
  delay(10);
}

关键逻辑：多重故障源的综合管理需要优先级仲裁。急停按钮（最高优先级）触发硬件断电，过流和倾角超限触发动态制动，通信超时触发预警减速。硬件急停回路（继电器）是软件失效时的最后防线——即使MCU死机，继电器仍能物理切断电机电源。

要点解读

1. 硬件急停回路是“最后防线”，独立于软件
   软件急停可能因MCU死机、程序卡死或中断被禁用而失效。工程标准要求：急停按钮必须直接切断电机驱动器的电源回路（使用常闭触点），而非仅依赖MCU的软件判断。Arduino的GPIO仅作为辅助检测手段，真正的安全保证来自硬件回路。

2. ISR必须“极短”，只标记不执行
   中断服务程序运行期间，所有其他中断被阻塞，millis()停止递增，delay()完全失效，Serial.print可能丢失数据。因此ISR的唯一正确做法是：设置一个volatile标志位，然后立即返回。实际的制动、断电逻辑在主循环中执行。

3. 分级制动兼顾安全性与机械寿命
   直接断电（自由停车）可能导致高速电机失控滑行，而立即施加最大制动力矩又可能损伤机械结构。工程上推荐三级制动：预警减速（低强度）→ 动态制动（中强度，能耗制动）→ 硬件断电（最终）。动态制动通过反向电流将动能转化为热能，比机械抱闸更平顺。

4. 多重故障源需要优先级仲裁
   实际系统中急停源不止一个：急停按钮、过流保护、通信超时、倾角超限、碰撞检测等。必须建立优先级仲裁机制——急停按钮最高（硬件断电），过流次之（快速制动），通信超时最低（预警减速）。当多个故障同时发生时，执行最高优先级的响应。

5. 共享变量必须使用volatile并注意原子性
   ISR与主循环共享的变量必须声明为volatile，防止编译器优化导致变量“看起来没变化”。对于多字节变量（如long），主循环读取时可能需要短暂禁用中断（noInterrupts()/interrupts()）以确保读取的原子性。在Arduino Uno等8位平台上，int（16位）的读写是原子的，但long（32位）不是。

1、三模冗余电机控制+硬件急停状态机（工业消防应急机器人核心）
适用场景：工业消防应急机器人，需同时驱动三套冗余BLDC电机（如履带驱动、云台旋转、水炮控制），核心需求是单套电机故障时仍能维持基础功能，且触发急停后快速切断动力，保障人员与设备安全。

核心逻辑：采用三套独立的BLDC驱动与编码器系统，通过“三取二”表决机制消除单传感器/电机误判；硬件急停按钮直接接入MCU中断，触发时强制切断所有电机动力；基于状态机实现从“正常运行→警告→急停→恢复”的确定性状态切换，急停状态为不可逆汇点，仅硬件复位可恢复。

#include <SimpleFOC.h>
#include <CRC16.h>

// ────── 三套独立冗余系统 ──────
BLDCMotor motorA = BLDCMotor(7);
BLDCMotor motorB = BLDCMotor(8);
BLDCMotor motorC = BLDCMotor(9);

BLDCDriver3PWM driverA = BLDCDriver3PWM(2,3,4,5);
BLDCDriver3PWM driverB = BLDCDriver3PWM(6,7,8,9);
BLDCDriver3PWM driverC = BLDCDriver3PWM(10,11,12,13);

Encoder encA = Encoder(14,15,2048);
Encoder encB = Encoder(16,17,2048);
Encoder encC = Encoder(18,19,2048);

// 硬件急停按钮（独立于MCU的最后防线）
#define E_STOP_PIN 7

// ────── 安全状态机 ──────
enum SafetyState {
    NORMAL,         // 正常运行
    CAUTION,        // 警告：接近危险阈值
    EMERGENCY_STOP, // 紧急停止
    ESTOP_RECOVERY  // 急停恢复中
};
SafetyState currentState = NORMAL;

// ────── 冗余投票结构 ──────
struct RedundantData {
    float position;
    float velocity;
    float current;
    uint16_t crc;
    bool valid;
};
RedundantData dataA, dataB, dataC;

// ────── 三取二表决器 ──────
float majorityVote(float a, float b, float c) {
    if ((a-b)*(a-c) <= 0) return a;  // a在b,c之间
    if ((b-a)*(b-c) <= 0) return b;
    return c;
}

void setupTripleRedundancy() {
    // 初始化三套系统
    driverA.voltage_power_supply = 24;
    driverB.voltage_power_supply = 24;
    driverC.voltage_power_supply = 24;
    
    driverA.init(); driverB.init(); driverC.init();

    motorA.linkDriver(&driverA); motorB.linkDriver(&driverB); motorC.linkDriver(&driverC);
    motorA.linkSensor(&encA); motorB.linkSensor(&encB); motorC.linkSensor(&encC);
    
    // 差异化PID参数（容忍单点参数漂移）
    motorA.controller = MotionControlType::torque;
    motorA.PID_velocity.P = 0.3; motorA.PID_velocity.I = 8.0;
    motorB.controller = MotionControlType::torque;
    motorB.PID_velocity.P = 0.28; motorB.PID_velocity.I = 7.5;
    motorC.controller = MotionControlType::torque;
    motorC.PID_velocity.P = 0.32; motorC.PID_velocity.I = 8.5;
    
    motorA.init(); motorB.init(); motorC.init();
    motorA.initFOC(); motorB.initFOC(); motorC.initFOC();
    
    pinMode(E_STOP_PIN, INPUT_PULLUP);
}

void safetyStateMachine() {
    static unsigned long cautionTimer = 0;
    static unsigned long estopTimer = 0;
    
    // 读取硬件急停（最高优先级，中断级响应）
    if (digitalRead(E_STOP_PIN) == LOW) {
        currentState = EMERGENCY_STOP;
        motorA.setPhaseVoltage(0,0);
        motorB.setPhaseVoltage(0,0);
        motorC.setPhaseVoltage(0,0);
        return;
    }
    
    // 采集三路数据
    dataA.position = encA.getAngle();
    dataA.velocity = motorA.shaft_velocity;
    dataA.current = motorA.phase_current;
    dataA.valid = (abs(dataA.current) < 10.0);  // 电流异常即判无效
    
    dataB.position = encB.getAngle();
    dataB.velocity = motorB.shaft_velocity;
    dataB.current = motorB.phase_current;
    dataB.valid = (abs(dataB.current) < 10.0);
    
    dataC.position = encC.getAngle();
    dataC.velocity = motorC.shaft_velocity;
    dataC.current = motorC.phase_current;
    dataC.valid = (abs(dataC.current) < 10.0);
    
    // 三取二表决
    float votedVelocity = majorityVote(dataA.velocity, dataB.velocity, dataC.velocity);
    float votedCurrent = majorityVote(dataA.current, dataB.current, dataC.current);
    
    // 状态切换逻辑
    switch (currentState) {
        case NORMAL:
            if ((votedCurrent > 8.0 && votedVelocity > 0.5) || (abs(dataA.velocity - dataB.velocity) > 1.0)) {
                currentState = CAUTION;
                cautionTimer = millis();
            } else {
                motorA.move(0.5); motorB.move(0.5); motorC.move(0.5);
            }
            break;
        case CAUTION:
            motorA.move(0.2); motorB.move(0.2); motorC.move(0.2);
            if (millis() - cautionTimer > 2000) currentState = EMERGENCY_STOP;
            break;
        case EMERGENCY_STOP:
            motorA.move(0); motorB.move(0); motorC.move(0);
            break;
        case ESTOP_RECOVERY:
            motorA.move(0.1); motorB.move(0.1); motorC.move(0.1);
            if (millis() - estopTimer > 3000) currentState = NORMAL;
            break;
    }
}

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

void loop() {
    motorA.loopFOC(); motorB.loopFOC(); motorC.loopFOC();
    safetyStateMachine();
    delay(10);
}

5、多传感器融合避障+硬件中断急停（特种救援机器人）
适用场景：地震废墟、火灾现场等特种救援场景，机器人需在浓烟、粉尘、非结构化地形中作业，核心需求是“安全本能优先”——极近距离障碍或碰撞时，硬件中断直接触发急停，同时通过多传感器实现常规避障，兼顾安全与作业连续性。

核心逻辑：采用“超声波+红外”多传感器融合感知，划分“紧急区-预警区-安全区”三级风险区域；前方紧急区传感器接入硬件中断，触发时直接切断电机动力（独立于主循环）；主循环通过模糊逻辑实现常规避障，急停恢复后执行后退动作再重新评估环境，避免二次碰撞。

/* ===== 多传感器融合避障 + 硬件中断急停 =====
 * 硬件：Arduino/ESP32 + 2×BLDC差速电机 + 前/左/右超声波 + 左右红外
 * 核心：三层感知区域（紧急/预警/安全）+ 模糊转向决策 + 硬件中断急停
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// BLDC差速电机
BLDCMotor motorL(7), motorR(7);
// 需补充Encoder和Driver初始化...

// 超声波传感器（前、左、右）
#define TRIG_F 2
#define ECHO_F 3
#define TRIG_L 4
#define ECHO_L 5
#define TRIG_R 6
#define ECHO_R 7
NewPing sonarF(TRIG_F, ECHO_F, 200);
NewPing sonarL(TRIG_L, ECHO_L, 150);
NewPing sonarR(TRIG_R, ECHO_R, 150);

// 红外传感器（近距离快速检测）
#define IR_LEFT A0
#define IR_RIGHT A1

// 安全区域阈值
const int EMERGENCY_ZONE = 15;    // 紧急制动区(cm)
const int WARNING_ZONE = 40;      // 预警区(cm)
const int SAFE_ZONE = 80;         // 安全区(cm)

// 机器人状态
enum RobotState { PATH_FOLLOWING, AVOIDING, RETURNING };
RobotState state = PATH_FOLLOWING;

// 安全本能中断
volatile bool safetyTriggered = false;

void IRAM_ATTR safetyISR() {
    // 硬件级紧急制动：完全绕过主循环
    motorL.move(0);
    motorR.move(0);
    safetyTriggered = true;
}

void setup() {
    Serial.begin(115200);
    
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    pinMode(IR_LEFT, INPUT);
    pinMode(IR_RIGHT, INPUT);
    
    // 前方传感器接入硬件中断（危险距离触发）
    attachInterrupt(digitalPinToInterrupt(ECHO_F), safetyISR, FALLING);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 安全本能恢复检测
    if (safetyTriggered) {
        emergencyReverse();
        safetyTriggered = false;
        return;
    }
    
    // 2. 多传感器数据采集
    int distF = sonarF.ping_cm();
    int distL = sonarL.ping_cm();
    int distR = sonarR.ping_cm();
    bool irLeft = digitalRead(IR_LEFT);
    bool irRight = digitalRead(IR_RIGHT);
    
    // 数据有效性过滤
    if (distF <= 0 || distF > 150) distF = 150;
    if (distL <= 0 || distL > 150) distL = 150;
    if (distR <= 0 || distR > 150) distR = 150;
    
    // 3. 分层避障决策
    // 第一层：紧急制动区（硬安全边界）
    if (distF < EMERGENCY_ZONE && distF > 0) {
        motorL.move(0); motorR.move(0);
        emergencyReverse();
        return;
    }
    
    // 第二层：避障决策（预警区）
    if (distF < WARNING_ZONE) {
        state = AVOIDING;
        
        // 评估左右空间，选择转向方向
        bool leftClear = (distL > SAFE_ZONE || distL <= 0) && !irLeft;
        bool rightClear = (distR > SAFE_ZONE || distR <= 0) && !irRight;
        
        // 模糊转向决策
        if (leftClear && !rightClear) {
            motorL.move(-0.3); motorR.move(0.5);  // 左转
        } else if (rightClear && !leftClear) {
            motorL.move(0.5); motorR.move(-0.3);  // 右转
        } else if (leftClear && rightClear) {
            motorL.move(0.4); motorR.move(0.4);   // 直行
        } else {
            // 两侧均不安全，随机转向
            if (random(2) == 0) {
                motorL.move(-0.4); motorR.move(0.4);
            } else {
                motorL.move(0.4); motorR.move(-0.4);
            }
        }
    } else {
        // 安全区：沿预设路径行驶
        state = PATH_FOLLOWING;
        motorL.move(0.5); motorR.move(0.5);
    }
}

void emergencyReverse() {
    motorL.move(-0.3); motorR.move(-0.3);
    delay(500);
    motorL.move(0); motorR.move(0);
    delay(200);
}

6、双核任务分离+多级故障处理急停（人机协作服务机器人）
适用场景：人机协作服务机器人（如医院配送、养老院护理），需同时运行AI推理、语音交互、电机控制等多任务，核心需求是“实时控制与高层计算解耦”——电机控制等硬实时任务由独立核心执行，急停故障由硬件看门狗和多级中断响应，避免高层任务卡顿导致安全失效。

核心逻辑：采用ESP32双核架构，Core0专责BLDC电机FOC控制（1kHz周期），Core1运行AI、通信等计算任务；硬件急停与电机过流信号接入独立中断，触发时Core0立即执行分级制动；构建“硬件快速故障→中断级故障→任务级故障→系统级降级”四级故障处理体系，故障恢复后需人工确认，避免自动重启引发二次风险。

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

// ===== BLDC电机定义 =====
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(12,13,14,15);

// ===== 双核任务句柄 =====
TaskHandle_t motorControlTaskHandle = NULL;
TaskHandle_t commTaskHandle = NULL;

// ===== 安全急停参数 =====
#define E_STOP_PIN 3        // 硬件急停按钮
#define OVERCURRENT_PIN A2  // 过流检测传感器
const float OVERCURRENT_THRESHOLD = 5.0; // 过流阈值（A）
volatile bool estopFlag = false;
volatile bool overcurrentFlag = false;

// ===== 双核任务：电机控制（Core 0，硬实时）=====
void motorControlTask(void *pvParameters) {
    Serial.println("[Core0] 电机控制任务启动");
    
    // 初始化电机
    driverL.voltage_power_supply = 24;
    driverR.voltage_power_supply = 24;
    driverL.init(); driverR.init();
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    // 注册硬件中断
    pinMode(E_STOP_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(E_STOP_PIN), [](){
        estopFlag = true;
        motorL.move(0); motorR.move(0);
    }, FALLING);
    
    // 过流中断
    pinMode(OVERCURRENT_PIN, INPUT);
    attachInterrupt(digitalPinToInterrupt(OVERCURRENT_PIN), [](){
        float current = analogRead(OVERCURRENT_PIN) * 0.1;
        if (current > OVERCURRENT_THRESHOLD) {
            overcurrentFlag = true;
            // 一级制动：反向力矩快速减速
            motorL.move(-0.2); motorR.move(-0.2);
        }
    }, CHANGE);
    
    float targetSpeedL = 0.5, targetSpeedR = 0.5;
    while (1) {
        // 故障分级处理
        if (estopFlag) {
            // 急停：切断动力，保持零输出
            motorL.move(0); motorR.move(0);
            // 等待人工复位
            while (digitalRead(E_STOP_PIN) == LOW) delay(10);
            estopFlag = false;
        } else if (overcurrentFlag) {
            // 二级制动：能耗制动（短接三相）
            motorL.move(0); motorR.move(0);
            delay(500); // 能耗制动持续时间
            overcurrentFlag = false;
            targetSpeedL = 0.3; targetSpeedR = 0.3; // 降级速度
        } else {
            // 正常控制
            motorL.move(targetSpeedL);
            motorR.move(targetSpeedR);
        }
        
        motorL.loopFOC();
        motorR.loopFOC();
        vTaskDelay(pdMS_TO_TICKS(1)); // 1kHz控制周期
    }
}

// ===== 双核任务：通信与故障监控（Core 1，软实时）=====
void commTask(void *pvParameters) {
    Serial.println("[Core1] 通信监控任务启动");
    
    // 初始化WiFi（绑定到Core1，避免干扰Core0）
    WiFi.setHostname("ServiceRobot");
    WiFi.begin("SSID", "PASSWORD");
    
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("WiFi connected");
    
    unsigned long lastHealthReport = 0;
    while (1) {
        // 1. 系统健康检查
        bool motorStatus = (motorL.shaft_velocity > 0 || motorR.shaft_velocity > 0);
        bool estopStatus = (digitalRead(E_STOP_PIN) == HIGH);
        float current = analogRead(OVERCURRENT_PIN) * 0.1;
        
        // 2. 故障上报（WiFi）
        if (millis() - lastHealthReport > 2000) {
            String healthData = "ESP32_Health: Motor=" + String(motorStatus) + 
                               ", EStop=" + String(estopStatus) + 
                               ", Current=" + String(current);
            WiFi.client.println(healthData);
            lastHealthReport = millis();
        }
        
        // 3. 看门狗签到（系统级故障检测）
        // 实际需接入FreeRTOS看门狗，此处简化为标志位检查
        if (estopFlag || overcurrentFlag) {
            Serial.println("[Core1] 检测到故障，触发系统降级");
            // 通知Core0进入降级模式（通过共享内存或消息队列）
        }
        
        vTaskDelay(pdMS_TO_TICKS(50)); // 20Hz监控频率
    }
}

void setup() {
    Serial.begin(115200);
    
    // 【核心】将任务绑定至指定核心
    xTaskCreatePinnedToCore(
        motorControlTask,    // 任务函数
        "MotorControl",      // 任务名称
        4096,                // 堆栈大小
        NULL,                // 参数
        5,                   // 优先级（高，确保硬实时）
        &motorControlTaskHandle,
        0                    // Core 0（硬实时核心）
    );
    
    xTaskCreatePinnedToCore(
        commTask,
        "CommMonitor",
        8192,
        NULL,
        2,                   // 优先级（低，不影响硬实时）
        &commTaskHandle,
        1                    // Core 1（计算核心）
    );
    
    // 启动FreeRTOS调度器
    vTaskStartScheduler();
}

void loop() {
    // FreeRTOS调度器接管后，此处为空
    delay(1000);
}

要点解读

1. 独立中断响应：构建硬件级安全“最后防线”
   硬件急停的核心是“独立于主循环的中断触发机制”，需通过硬件信号直接触发MCU外部中断，绕过软件逻辑的延迟与阻塞，确保急停响应时间达到微秒级。关键设计包括：
   硬件信号直接接入：急停按钮、防撞条、过流检测等安全传感器，需直接接入MCU的外部中断引脚（如ESP32的GPIO中断、Arduino的attachInterrupt），而非通过主循环轮询，避免主任务卡顿导致急停失效；
   中断服务程序（ISR）极简设计：ISR仅执行“置位急停标志+切断电机动力”等核心操作，不包含复杂逻辑（如数据计算、通信上报），防止ISR执行时间过长阻塞其他中断；
   中断优先级最高化：在多任务系统中，急停中断优先级需设为最高，确保任何场景下都能优先响应，例如双核架构中，急停中断可打断Core0的电机控制任务，强制执行制动。
2. 分级制动策略：平衡安全与机械冲击
   BLDC电机惯性大，直接断电自由滑行距离长，瞬间机械抱闸易造成机械损伤，需设计“快速减速→能耗制动→机械锁定”的分级制动逻辑，适配不同故障严重程度：
   一级制动：反向力矩快速减速：急停触发初期，通过FOC控制给电机施加反向力矩，利用电磁阻力快速消耗动能，减速过程平滑无冲击，适用于过载、碰撞等非致命故障；
   二级制动：能耗制动消耗回馈能量：电机转速降低后，短接电机三相绕组，通过内置泄放电阻消耗反电动势产生的能量，防止母线电压泵升损坏驱动电路，同时实现快速停车，适用于急停触发后的快速制动；
   三级制动：机械抱闸锁定：电机接近静止时，触发机械抱闸（如电磁刹车）锁定电机轴，防止机器人因惯性滑移或倾覆，适用于高空作业、斜坡行驶等需绝对静止的场景。
3. 状态机安全架构：确保状态切换的确定性
   急停系统需通过有限状态机（FSM） 管理安全状态，确保所有状态切换遵循严格逻辑，避免状态混乱导致失控，核心设计原则包括：
   急停状态为“汇点状态”：急停状态是最高优先级的不可逆状态，仅能通过硬件复位或人工确认触发恢复，其他状态（正常运行、警告、降级）均可跳转至急停，但急停不可直接跳转至正常运行，需经过“急停恢复”中间状态；
   状态切换原子化：状态切换通过enum严格定义，禁止使用goto等非结构化跳转，每个状态的进入/退出需执行明确的初始化与清理操作（如急停状态退出前需复位所有故障标志、校准传感器）；
   故障锁存与消抖：传感器故障（如急停按钮抖动、过流信号跳变）需加入软件消抖（如连续3次检测到故障才触发状态切换），同时对故障标志进行锁存，防止故障消失后状态自动恢复，需人工干预复位。
4. 冗余与容错设计：消除单点失效风险
   急停系统的可靠性依赖“硬件-软件-通信”三重冗余，确保单一节点故障时系统仍能安全运行，核心措施包括：
   硬件冗余：关键信号（如急停按钮、编码器Z相、过流检测）采用双通道异构输入，例如急停按钮同时接入MCU中断和独立硬件比较器，硬件比较器直接切断电机驱动电源，即使MCU死机也能实现物理断电；
   软件冗余：采用多套独立的控制算法（如编码器PID+电流积分估算位移）进行交叉校验，若数据偏差超过阈值，触发故障降级，而非依赖单一算法；
   通信冗余：核心控制指令（如急停信号）通过独立通信链路传输，与普通数据（如传感器数据）分离，避免通信拥堵导致指令延迟，同时采用ACK确认与重发机制，确保指令可靠传输。
5. 实时性与资源隔离：保障安全任务优先级
   急停与电机控制是硬实时任务，需通过资源隔离确保其不受高层任务干扰，核心设计要点包括：
   双核任务分离：采用ESP32等双核MCU，将电机FOC控制、急停中断响应等硬实时任务绑定至专用核心，高层任务（如AI推理、通信）运行于另一核心，避免计算密集型任务占用硬实时核心资源；
   非阻塞编程：禁止在硬实时任务中使用delay()等阻塞函数，采用硬件定时器中断或FreeRTOS任务延时，确保控制周期稳定；
   硬件看门狗监控：启用硬件看门狗（如ESP32的独立看门狗），核心任务需定期“喂狗”，若任务卡死导致喂狗超时，看门狗触发系统复位，防止程序跑飞导致安全功能失效，复位前需将故障信息存入Flash，便于后续排查。

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