Arduino BLDC之机器人三级制动状态机控制（平稳分级制动）的核心在于利用有限状态机（FSM）将刹车过程解耦为滑行缓冲、电气降速与位置锁死三个平滑递进阶段，以消除机械冲击并实现“无超调”精准停靠。 该方案在算力受限的嵌入式平台上，通过多传感器融合与分层决策，解决了传统BLDC电机刹车生硬、易损坏机械结构及无法精准定位的痛点。以下从主要特点、应用场景及注意事项三个维度进行专业解析。

主要特点

1. 三级制动状态机架构（核心控制逻辑）
   这是系统的“决策大脑”，通过有限状态机（FSM）将刹车过程解耦为三个平滑递进的状态，避免传统急停带来的机械冲击。
   第一级：滑行缓冲态（Coasting / Deceleration Phase）：当系统接收到刹车指令或检测到障碍物时，不立即施加制动力，而是先切断电机动力输出（motor.move(0)），让机器人依靠惯性自然滑行。此阶段可设置定时器或距离阈值，避免误触发，同时让电机转速自然衰减，为后续电气制动做准备。
   第二级：电气降速态（Electrical Braking Phase）：当速度降至预设阈值（如额定转速的20%-30%）时，状态机切换至电气制动模式。此时采用回馈制动（再生制动）或短接制动，通过调节PWM占空比动态控制制动力矩。中高速时PWM占空比控制在30%以内以限制短路电流，中低速时再切换为满占空比短接，实现平稳减速。
   第三级：位置锁死态（Position Hold / Locked Phase）：当速度接近零时，状态机进入位置保持模式。此时BLDC电机切换至FOC位置环控制，通过PID算法输出维持力矩，抵抗外部扰动（如斜坡重力、碰撞力），实现“停得准、hold得住”的精准停靠。
2. 多传感器融合的风险评估与分级响应
   制动决策不仅依赖单一信号，而是融合多源数据进行智能判断。
   距离感知层：激光雷达/超声波用于远距离预警（触发第一级滑行），红外/触碰传感器用于近距离确认（触发第二级电气制动）。
   姿态感知层：IMU监测机器人倾斜角和加速度，防止在斜坡上制动时发生倾覆或溜车。
   力矩/电流感知层：通过电流环实时监测BLDC电机输出力矩，当检测到异常碰撞或堵转时，电流急剧上升可作为最后一道防线，直接触发紧急制动。
   分级响应机制：不同风险等级触发不同制动级别。例如，远处静态障碍只需滑行减速，快速接近的动态障碍需立即电气制动，而碰撞或倾覆风险则直接锁死电机并切断使能。
3. 基于FOC的平滑转矩控制与S曲线规划
   BLDC电机配合FOC（磁场定向控制）是实现平稳制动的物理基础。
   低转矩脉动：FOC通过正弦电流驱动消除换相阶跃，转矩纹波降低70%以上，确保制动过程中无抖动、无冲击。
   S形速度曲线（S-curve Trajectory）：在制动过程中引入加加速度（jerk）限制，使加速度连续变化，避免梯形速度曲线中加速度突变导致的机械谐振和跟踪误差。
   力矩线性控制：FOC的q轴电流（Iq）与输出转矩呈线性关系，使得制动力矩可精确调节，实现“踩多少刹多少”的拟人化制动体验。

应用场景
该系统凭借其平稳性、精准性和高安全性，主要应用于以下场景：
服务机器人与人机共融场景：在餐厅、酒店、商场等人流密集环境中，机器人需要与人类安全共存。三级制动确保机器人在检测到行人时先平滑减速（避免惊吓），再精准停靠（避免碰撞），最后保持静止（防止被推倒）。
精密定位与自动化设备：如3D打印机、CNC机床、激光雕刻机等，要求打印头或刀头在高速运动后精准停靠在指定位置，无超调、无振荡。三级制动中的位置锁死态通过PID位置环实现亚毫米级定位精度。
物流搬运机器人（AGV/AMR）：在仓储物流中，重载AGV需要在货架前精准停靠以进行货物存取。三级制动可有效抑制重载惯性带来的停车超调，同时位置锁死态可防止在斜坡上溜车。
机械臂关节伺服驱动：多自由度机械臂的每个关节都需要快速、准确地到达指定角度。三级制动确保关节在高速运动后无冲击地停在目标位置，并通过力矩保持抵抗重力矩和外部扰动。
教育与科研验证平台：高校在机器人学、自动控制原理课程中，利用该平台演示状态机编程、PID调参、FOC控制等核心概念，是学习嵌入式运动控制的理想载体。

需要注意的事项
在实现该系统时，需重点关注以下技术细节与工程红线：

1. MCU选型与控制周期（算力瓶颈）
   MCU性能要求：标准Arduino Uno（ATmega328P, 16MHz）无法高效运行FOC+状态机+多传感器融合。强烈推荐ESP32（双核240MHz，支持硬件浮点）、STM32F4/F7（Cortex-M4/M7，支持高级定时器）或Teensy 4.1（600MHz）。
   控制周期：FOC电流环需≥1kHz（建议10-20kHz），速度环/位置环≥200Hz，状态机判断≥50Hz。所有控制回路必须使用硬件定时器中断，严禁使用delay()函数。
2. 传感器校准与数据滤波
   编码器分辨率：关节定位精度直接取决于传感器分辨率。12-bit磁编码器（如AS5600）分辨率约0.088°，14-bit（如MA730）约0.022°。编码器应安装在关节输出轴而非电机轴，以补偿减速器齿隙。
   数据滤波：传感器信号易受电磁干扰。建议对编码器数据进行滑动平均或卡尔曼滤波处理；对电流数据进行低通滤波，以剔除尖峰干扰。
3. 严格的电源管理与电磁兼容（EMC）
   隔离供电：BLDC电机在制动时会产生反向电动势和电流尖峰，极易导致MCU复位。必须使用独立DC-DC模块为控制电路供电，并在电机驱动端并联大容量低ESR电容（1000μF-4700μF）吸收反电动势。
   共地连接：MCU的GND必须与电机驱动模块的GND物理连接，否则控制信号无法形成回路。
   信号屏蔽：编码器、IMU等敏感信号线需使用屏蔽线，并远离动力线布线，PCB布局时注意AGND与DGND单点连接。
4. 制动参数整定与安全冗余
   速度阈值标定：第一级到第二级的切换速度阈值需通过实验标定，过高会导致滑行距离过长，过低会导致电气制动时电流冲击过大。
   PID参数整定：位置锁死态的PID参数需精细标定。Kp过大会导致振荡，Kd过小会导致超调，Ki用于消除稳态误差但需防止积分饱和。建议采用增量式PID或带限幅的积分分离PID。
   硬件急停：必须设置独立的硬件急停电路（如物理急停按钮），采用中断级处理，可强切电机使能（EN），确保在状态机失控时能瞬间切断动力。
   死区时间控制：在驱动BLDC电机的三相全桥逆变电路中，必须插入短暂的“死区时间”（通常0.5-2μs），防止上下桥臂MOSFET同时导通造成短路。

1、基础三级制动状态机（减速 → 软停 → 抱闸保持）
适用场景：轮式机器人精准停机、路径跟踪终点制动、移动作业停机等场景，特点是要求制动平稳、无顿挫、无过冲，避免因制动过急导致货物倾倒或人员晃动。

/**
 * 三级制动状态机 - 平稳分级制动
 * 硬件假设：Arduino Mega, BLDC + 编码器, 电机驱动板(PWM+方向), 电磁抱闸(BRAKE_PIN)
 * 状态流转：RUNNING -> DECEL -> SOFT_STOP -> HOLD
 */

#include <SimpleFOC.h>

BLDCMotor motor(7);
Encoder encoder(2, 4, 2048);

#define BRAKE_PIN  9
#define DECEL_THRESHOLD      30   // 进入减速制动的速度阈值 (cm/s)
#define SOFT_STOP_THRESHOLD  5    // 进入软停车的速度阈值 (cm/s)
#define HOLD_SPEED_THRESHOLD 0.5  // 进入抱闸的速度阈值 (cm/s)
#define DECEL_RATE           8.0  // 减速阶段减速度 (cm/s²)
#define SOFT_DECEL_RATE      3.0  // 软停车阶段减速度 (cm/s²)

// 状态定义
typedef enum {
  STATE_RUNNING,     // 正常运行
  STATE_DECEL,       // 一级：减速制动
  STATE_SOFT_STOP,   // 二级：软停车
  STATE_HOLD,        // 三级：抱闸保持
  STATE_ERROR        // 异常状态
} BrakeState;

BrakeState currentState = STATE_RUNNING;
float currentSpeed = 0.0;
float targetSpeed = 0.0;
unsigned long lastUpdate = 0;

// 编码器中断（4倍频）
volatile long encCount = 0;
void encoderISR() {
  bool a = digitalRead(2);
  bool b = digitalRead(4);
  if (a && !b) encCount++;
  else if (!a && b) encCount--;
}

float readSpeed() {
  static long lastEnc = 0;
  static unsigned long lastTime = 0;
  unsigned long now = micros();
  if (now - lastTime < 5000) return currentSpeed; // 200Hz更新
  
  long deltaEnc = encCount - lastEnc;
  float dt = (now - lastTime) / 1000000.0;
  // 假设编码器2048线，轮径10cm，4倍频 -> 8192脉冲/圈
  currentSpeed = (deltaEnc / 8192.0) * PI * 10.0 / dt; // cm/s
  
  lastEnc = encCount;
  lastTime = now;
  return currentSpeed;
}

void setup() {
  Serial.begin(115200);
  pinMode(BRAKE_PIN, OUTPUT);
  digitalWrite(BRAKE_PIN, LOW); // 抱闸初始释放
  
  attachInterrupt(digitalPinToInterrupt(2), encoderISR, CHANGE);
  
  motor.linkSensor(&encoder);
  motor.init();
  motor.initFOC();
  motor.controller = MotionControlType::velocity;
  
  targetSpeed = 20.0; // 初始巡航速度
  Serial.println("三级制动状态机就绪");
}

void loop() {
  motor.loopFOC();
  currentSpeed = readSpeed();
  
  switch (currentState) {
    case STATE_RUNNING:
      motor.move(targetSpeed / 10.0); // 简化的速度映射
      if (Serial.available() && Serial.readStringUntil('\n') == "STOP") {
        currentState = STATE_DECEL;
        Serial.println("进入一级减速制动");
      }
      break;
      
    case STATE_DECEL:
      // 一级：按 DECEL_RATE 减速
      targetSpeed -= DECEL_RATE * 0.02; // 50Hz循环
      if (targetSpeed < 0) targetSpeed = 0;
      motor.move(targetSpeed / 10.0);
      
      if (currentSpeed < DECEL_THRESHOLD && currentSpeed >= SOFT_STOP_THRESHOLD) {
        currentState = STATE_SOFT_STOP;
        Serial.println("进入二级软停车");
      }
      if (currentSpeed < SOFT_STOP_THRESHOLD) {
        currentState = STATE_SOFT_STOP;
      }
      break;
      
    case STATE_SOFT_STOP:
      // 二级：更慢的减速度，精细逼近零速
      targetSpeed -= SOFT_DECEL_RATE * 0.02;
      if (targetSpeed < 0) targetSpeed = 0;
      motor.move(targetSpeed / 10.0);
      
      if (currentSpeed < HOLD_SPEED_THRESHOLD) {
        currentState = STATE_HOLD;
        digitalWrite(BRAKE_PIN, HIGH); // 抱闸锁死
        Serial.println("进入三级抱闸保持");
      }
      break;
      
    case STATE_HOLD:
      motor.move(0); // 电机零力矩
      // 保持抱闸
      break;
      
    case STATE_ERROR:
      digitalWrite(BRAKE_PIN, HIGH);
      motor.move(0);
      break;
  }
  
  lastUpdate = millis();
  Serial.print("速度:"); Serial.print(currentSpeed, 1);
  Serial.print(" 状态:"); Serial.println(currentState);
  delay(20); // 50Hz
}

要点：该代码的核心是基于速度阈值的状态自动流转。减速阶段以 8.0 cm/s² 的减速度快速降速，当速度进入 5~30 cm/s 区间时切换到软停车阶段，以更低的 3.0 cm/s² 减速度精细逼近零速，最后在速度低于 0.5 cm/s 时触发抱闸。这种“先快后慢”的策略避免了高速下直接抱闸对机械结构的冲击。

2、硬件中断急停 + 三级制动（优先级仲裁）
适用场景：工业 AGV、商用服务机器人等对安全性要求极高的场景，急停信号必须绕过常规逻辑直接响应，同时分级制动平衡制动速度与机械冲击。

/**
 * 硬件中断急停 + 三级制动状态机
 * 硬件假设：Arduino Mega, BLDC + 编码器, 驱动板, 电磁抱闸, 急停按钮(NC常闭)
 * 核心：ISR极简响应(仅置位标志+硬件断电) -> 主循环执行分级制动
 */

#include <SimpleFOC.h>

BLDCMotor motor(7);
Encoder encoder(2, 4, 2048);

#define BRAKE_PIN    9
#define ESTOP_PIN    3   // 外部中断引脚，接常闭急停按钮

// 制动阈值
#define DECEL_THRESHOLD      40
#define SOFT_STOP_THRESHOLD  8
#define HOLD_THRESHOLD       1.0

// 状态定义
typedef enum {
  STATE_RUNNING,
  STATE_PRE_WARN,     // 预警减速
  STATE_DECEL,        // 动态制动
  STATE_SOFT_STOP,    // 软停车
  STATE_HOLD,         // 抱闸保持
  STATE_EMERGENCY     // 急停(不可逆)
} BrakeState;

volatile bool emergencyFlag = false;
BrakeState currentState = STATE_RUNNING;
float currentSpeed = 0.0;
float targetSpeed = 0.0;

void estopISR() {
  emergencyFlag = true;
  // ISR中极简操作：仅置位标志，禁止delay/Serial
}

void setup() {
  Serial.begin(115200);
  pinMode(BRAKE_PIN, OUTPUT);
  digitalWrite(BRAKE_PIN, LOW);
  
  pinMode(ESTOP_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), estopISR, FALLING);
  
  attachInterrupt(digitalPinToInterrupt(2), []() {
    bool a = digitalRead(2); bool b = digitalRead(4);
    if (a && !b) encCount++;
    else if (!a && b) encCount--;
  }, CHANGE);
  
  motor.linkSensor(&encoder);
  motor.init(); motor.initFOC();
  motor.controller = MotionControlType::velocity;
  
  targetSpeed = 25.0;
}

void loop() {
  motor.loopFOC();
  currentSpeed = readSpeed();
  
  // 最高优先级：急停信号检查
  if (emergencyFlag) {
    currentState = STATE_EMERGENCY;
    emergencyFlag = false;
  }
  
  switch (currentState) {
    case STATE_RUNNING:
      motor.move(targetSpeed / 10.0);
      // 可添加其他触发条件进入PRE_WARN
      break;
      
    case STATE_PRE_WARN:  // 预警减速 (低强度)
      targetSpeed -= 4.0 * 0.02;
      if (targetSpeed < 0) targetSpeed = 0;
      motor.move(targetSpeed / 10.0);
      if (currentSpeed < DECEL_THRESHOLD) currentState = STATE_DECEL;
      break;
      
    case STATE_DECEL:  // 动态制动 (中强度，能耗制动)
      // 通过反向电流产生制动转矩，同时监测电流防止过流
      targetSpeed -= 10.0 * 0.02; // 更快减速
      if (targetSpeed < -5.0) targetSpeed = -5.0; // 限制反向速度
      motor.move(targetSpeed / 10.0);
      if (currentSpeed < SOFT_STOP_THRESHOLD) {
        currentState = STATE_SOFT_STOP;
      }
      break;
      
    case STATE_SOFT_STOP:
      targetSpeed -= 3.0 * 0.02;
      if (targetSpeed < 0) targetSpeed = 0;
      motor.move(targetSpeed / 10.0);
      if (currentSpeed < HOLD_THRESHOLD) currentState = STATE_HOLD;
      break;
      
    case STATE_HOLD:
      motor.move(0);
      digitalWrite(BRAKE_PIN, HIGH);
      break;
      
    case STATE_EMERGENCY:  // 急停：不可逆，仅硬件复位可恢复
      motor.move(0);
      digitalWrite(BRAKE_PIN, HIGH);
      // 此处保持，直到系统复位
      break;
  }
  
  Serial.print("速度:"); Serial.print(currentSpeed, 1);
  Serial.print(" 状态:"); Serial.println(currentState);
  delay(20);
}

要点：该案例体现了安全优先级仲裁的设计原则。急停信号通过外部中断直接响应，ISR 仅置位 emergencyFlag，不做任何耗时操作。主循环检测到标志后，进入不可逆的 STATE_EMERGENCY，强制抱闸并等待硬件复位。三级制动的核心价值在于：动态制动阶段允许电机以反向转矩快速降速，但限制反向速度防止过流；软停车阶段以更低减速度逼近零速，避免顿挫；抱闸阶段最终锁定位置。

3、基于 SimpleFOC 力矩控制的平滑制动（FOC 力矩感知 + 分级响应）
适用场景：对制动力矩精度要求高的协作机器人或精密移动平台，利用 FOC 的力矩控制能力实现更柔顺的减速曲线。

/**
 * 基于 SimpleFOC 力矩控制的平滑分级制动
 * 核心：利用 FOC 的力矩控制模式，以力矩指令替代速度指令实现制动
 * 优势：力矩平滑，无速度环超调，适合对顿挫敏感的场景
 */

#include <SimpleFOC.h>

BLDCMotor motor(7);
Encoder encoder(2, 4, 2048);
BLDCDriver3PWM driver(5, 6, 9, 8);

#define BRAKE_PIN      10
#define TORQUE_DECEL   0.3   // 减速力矩 (Nm)
#define TORQUE_SOFT    0.1   // 软停力矩 (Nm)
#define HOLD_THRESHOLD 1.0

typedef enum {
  STATE_RUNNING,
  STATE_TORQUE_BRAKE,  // 力矩制动
  STATE_SOFT_BRAKE,    // 软力矩制动
  STATE_HOLD
} BrakeState;

BrakeState state = STATE_RUNNING;
float currentSpeed = 0.0;

void setup() {
  Serial.begin(115200);
  pinMode(BRAKE_PIN, OUTPUT);
  digitalWrite(BRAKE_PIN, LOW);
  
  driver.voltage_power_supply = 24;
  driver.init();
  motor.linkDriver(&driver);
  motor.linkSensor(&encoder);
  motor.init();
  motor.initFOC();
  
  // 力矩控制模式，配合电流检测
  motor.controller = MotionControlType::torque;
  motor.torque_controller = TorqueControlType::voltage;
  
  // 先以速度模式启动
  motor.controller = MotionControlType::velocity;
  motor.move(3.0);
  delay(2000);
  
  Serial.println("开始制动...");
  state = STATE_TORQUE_BRAKE;
}

void loop() {
  motor.loopFOC();
  currentSpeed = readSpeed();
  
  switch (state) {
    case STATE_RUNNING:
      motor.move(3.0);
      break;
      
    case STATE_TORQUE_BRAKE:
      // 施加反向力矩制动
      // FOC力矩控制下，负力矩即为制动
      motor.move(-TORQUE_DECEL);
      
      if (abs(currentSpeed) < 10.0) {
        state = STATE_SOFT_BRAKE;
        Serial.println("切换到软力矩制动");
      }
      break;
      
    case STATE_SOFT_BRAKE:
      // 更小的制动力矩，精细逼近零速
      motor.move(-TORQUE_SOFT);
      
      if (abs(currentSpeed) < HOLD_THRESHOLD) {
        state = STATE_HOLD;
        digitalWrite(BRAKE_PIN, HIGH);
        Serial.println("抱闸保持");
      }
      break;
      
    case STATE_HOLD:
      motor.move(0); // 零力矩
      break;
  }
  
  Serial.print("速度:"); Serial.print(currentSpeed, 1);
  Serial.print(" 状态:"); Serial.println(state);
  delay(20);
}

float readSpeed() {
  static long lastEnc = 0;
  static unsigned long lastTime = 0;
  unsigned long now = micros();
  if (now - lastTime < 5000) return currentSpeed;
  long delta = encCount - lastEnc;
  float dt = (now - lastTime) / 1000000.0;
  currentSpeed = (delta / 8192.0) * PI * 10.0 / dt;
  lastEnc = encCount; lastTime = now;
  return currentSpeed;
}

要点：该案例展示了 FOC 力矩控制在制动场景中的独特优势。传统速度环制动在接近零速时容易出现积分饱和和超调，而力矩控制直接指定制动力矩大小，物理意义更明确。TORQUE_DECEL = 0.3 Nm 和 TORQUE_SOFT = 0.1 Nm 的两级力矩配置实现了自然的“先强后弱”减速曲线，无需复杂的速度规划。SimpleFOC 的 TorqueControlType::voltage 模式无需电流传感器即可实现近似力矩控制。

要点解读

1. 三级制动的本质是“用时间换平稳”，而不是“用力量换速度”。 从减速到软停再到抱闸，每一级都在降低制动强度，代价是制动距离延长。如果机器人需要从 30 cm/s 完全停止，三级制动可能需要 2~3 秒，而直接抱闸可能只需 0.5 秒。选择三级制动的场景，一定是“平稳性优先于制动距离”的场景——如搬运易碎品的 AGV、载人的服务机器人、精密定位平台。如果场景要求“最短制动距离”，应该选择直接动态制动甚至机械抱闸，而不是三级制动。

2. 减速阈值和减速率必须与负载惯量匹配，否则会顿挫或过冲。 案例一中的 DECEL_THRESHOLD = 30 和 SOFT_STOP_THRESHOLD = 5 不是通用值，而是根据机器人质量、轮径和电机扭矩估算的。如果负载惯量大而减速率设置过小，机器人会在软停阶段“滑行”很久；如果减速率设置过大，则会产生反向冲击。实践中，减速率应从保守值（如 5 cm/s²）开始，逐步调大直到观察到轻微顿挫，再回调 20% 作为最终值。

3. 硬件中断急停与分级制动是互补而非替代关系。 急停按钮通过硬件中断直接切断动力，是最后的安全防线，响应时间在微秒级。但急停是“一刀切”的粗暴制动，对机械结构和负载都有冲击。分级制动适用于“计划内的平稳停机”——如到达终点、收到正常停止指令。两者应该共存：正常停机走三级制动状态机，异常情况走硬件急停。

4. FOC 力矩控制让制动更“柔性”，但需要解决零速附近的转矩控制精度。 案例三用 motor.move(-TORQUE_DECEL) 直接指定制动力矩，避免了速度环在低速时的积分饱和问题。但 SimpleFOC 在极低转速（<1 rad/s）下的力矩控制精度会下降，因为反电动势过小，电压模式力矩控制的近似误差增大。如果需要在零速附近精确控制抱闸时机，建议在软停阶段切换回速度模式，用速度环的积分项来精细逼近零速。

5. 抱闸时序是三级制动的“最后一厘米”，也是最容易被忽视的细节。 案例中 digitalWrite(BRAKE_PIN, HIGH) 在速度低于 HOLD_THRESHOLD 时触发。如果抱闸过早（速度仍较高），会对抱闸片和电机轴产生冲击；如果抱闸过晚（速度已接近零但电机仍在输出力矩），则会产生“电机与抱闸对抗”。理想的抱闸时机是：电机力矩降至接近零（motor.move(0)），等待 50~100ms 让机械系统稳定，再闭合抱闸。案例一直接在 STATE_HOLD 中同时执行 motor.move(0) 和抱闸，可以进一步优化为先零力矩、延时、再抱闸。

4、常规作业路径终点三级平稳制动程序（核心：路径终点精准停靠）
适用场景：巡检、搬运等常规作业机器人，在路径终点实现平稳分级制动，避免急停的冲击力与定位偏差。

核心逻辑
基于预设距离，划分三级制动区间，匹配不同制动策略
通过状态机切换巡航、一级制动、二级制动、三级制动四个状态，逐步降低速度与扭矩
三级制动完成后触发停机，实现精准停靠

// 常规作业终点三级平稳制动程序
// 硬件：BLDC驱动、光电编码器（速度反馈）、霍尔定位传感器、蜂鸣器
// 功能：路径终点分级制动，精准停靠，降低停车冲击

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM_PIN = 9;   // BLDC转速控制
const byte DIR_PIN = 8;   // BLDC方向控制
const byte BRK_PIN = 7;   // 硬件制动引脚（可选，辅助制动）
const byte PIN_ENC = 2;   // 编码器中断引脚
const byte PIN_HOME = 4;  // 终点定位传感器
const byte SND_PIN = 13;  // 蜂鸣器（到位提示）

// ========== 制动参数 ==========
enum BrakeStage { CRUISE, FIRST_BRAKE, SECOND_BRAKE, THIRD_BRAKE, PARKING }; // 0巡航，1-3三级制动，4停放
BrakeStage stage = CRUISE;
const float DIST_THRESHOLD[3] = {200.0, 80.0, 20.0}; // 三级制动触发距离（cm）
const uint8_t CRUISE_SPEED = 180;  // 巡航速度（0-255）
const uint8_t SPEED_1 = 140;       // 一级制动目标速度
const uint8_t SPEED_2 = 80;        // 二级制动目标速度
const uint8_t SPEED_3 = 30;        // 三级制动目标速度
float speed = CRUISE_SPEED;         // 当前实际速度
uint16_t dist = 300;                // 当前距离终点距离，模拟值
bool parkingComplete = false;

// ========== 函数声明 ==========
void speedPID(uint8_t target);
void applyBrake(uint8_t spd);
void enterStage(BrakeStage s);
long readEncoder();

void setup() {
  Serial.begin(115200);
  Serial.println("三级制动系统启动");

  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(BRK_PIN, OUTPUT);
  pinMode(PIN_HOME, INPUT);
  pinMode(SND_PIN, OUTPUT);
  digitalWrite(BRK_PIN, HIGH); // 释放制动
  attachInterrupt(digitalPinToInterrupt(PIN_ENC), readEncoder, LOW);
}

void loop() {
  // 模拟距离递减（实际可通过编码器/定位计算）
  if (dist > 0) dist--;

  // 1. 状态机触发与切换
  switch (stage) {
    case CRUISE:
      speed = CRUISE_SPEED;
      if (dist < DIST_THRESHOLD[0]) {
        stage = FIRST_BRAKE;
        enterStage(stage);
      }
      break;

    case FIRST_BRAKE:
      // 距离降低则切换至二级制动
      if (dist < DIST_THRESHOLD[1]) {
        stage = SECOND_BRAKE;
        enterStage(stage);
      } else {
        speedPID(SPEED_1);
        // 缓慢降速，避免落差过大
        speed = lerp(speed, SPEED_1, 0.2f);
      }
      break;

    case SECOND_BRAKE:
      if (dist < DIST_THRESHOLD[2]) {
        stage = THIRD_BRAKE;
        enterStage(stage);
      } else {
        speedPID(SPEED_2);
        speed = lerp(speed, SPEED_2, 0.15f);
      }
      break;

    case THIRD_BRAKE:
      // 距离终点足够近，进入最终停靠
      if (dist < 5 || digitalRead(PIN_HOME) == HIGH) {
        stage = PARKING;
        enterStage(stage);
        parkingComplete = true;
      } else {
        speedPID(SPEED_3);
        speed = lerp(speed, SPEED_3, 0.1f);
      }
      break;

    case PARKING:
      // 缓慢降速至0，完成停靠
      speed = max((int)(speed - 5), 0);
      applyBrake(speed);
      if (speed == 0) {
        digitalWrite(BRK_PIN, LOW); // 启用硬件制动，锁定位置
        digitalWrite(SND_PIN, HIGH);
        Serial.println("制动完成，已精准停靠");
      }
      break;
  }
  Serial.print("当前阶段：");Serial.print(stage);
  Serial.print(" 距离终点：");Serial.println(dist);
  delay(20);
}

// 简易PID调速，快速逼近目标速度
void speedPID(uint8_t target) {
  int error = target - speed;
  int output = speed + error * 0.1; // 比例控制
  output = constrain(output, 0, 255);
  applyBrake(output);
}

// 应用电机驱动，驱动BLDC
void applyBrake(uint8_t spd) {
  digitalWrite(DIR_PIN, HIGH); // 保持运行方向
  analogWrite(PWM_PIN, spd);
}

// 进入新制动阶段，打印提示
void enterStage(BrakeStage s) {
  const char* stageName[] = {"巡航", "一级制动", "二级制动", "三级制动", "停放"};
  Serial.print("切换制动阶段：");Serial.println(stageName[s]);
}

// 读取编码器脉冲，计算实际速度（简化）
long readEncoder() {
  // 累积脉冲数，用于速度计算，此处简化处理
  return 0;
}

适用场景优化
加入里程累积功能，精准计算剩余距离，适配长距离作业场景
优化制动区间阈值，根据作业总距离动态调整三级区间长度，提升制动平滑度
增加加速度限幅，避免阶段切换时速度突变，进一步降低冲击

5、复合场景下的三级制动与应急响应程序（核心：动态场景制动适配）
适用场景：仓储物流、复杂地形机器人，在正常停靠与紧急避障双重需求下，实现动态切换制动策略，兼顾平稳性与安全性。

核心逻辑
基于距离划分为三级制动区间，对应不同制动强度
结合避障传感器触发应急制动，紧急情况下跳过分级，直接急停
避障解除后，平滑回归正常分级制动流程，实现平顺过渡

// 复合场景三级制动+应急响应程序
// 硬件：BLDC、避障红外、距离传感器、蜂鸣器、状态指示灯
// 功能：正常分级制动，紧急避障急停，场景动态适配

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM_PIN = 9, DIR_PIN = 8, BRK_PIN = 7;
const byte IR_AVOID = 2;   // 前方避障传感器
const byte DIST_SENSOR = A0;// 距离传感器（红外测距）
const byte LED_RED = 13, LED_GREEN = 12; // 状态指示灯

// ========== 制动参数 ==========
enum BrakeMode { NORMAL_BRAKE, EMERGENCY_STOP };
enum State { IDLE, CRUISE, FIRST_BRAKE, SECOND_BRAKE, THIRD_BRAKE, PARKING };
State state = IDLE;
BrakeMode mode = NORMAL_BRAKE;
const int DIST_LEVEL[3] = {250, 100, 40}; // 三级制动距离阈值
const uint8_t BASE_SPEED = 200;
const uint8_t SPEED_SET[3] = {150, 90, 40}; // 三级制动目标速度
int currentSpeed = 0;
bool emergencyFlag = false;

// ========== 函数声明 ==========
void checkEmergency();
void normalBrake(int level);
void emergencyStop();
uint16_t readDistance();

void setup() {
  Serial.begin(115200);
  Serial.println("复合制动系统启动");

  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(BRK_PIN, OUTPUT);
  pinMode(IR_AVOID, INPUT);
  pinMode(DIST_SENSOR, INPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  digitalWrite(BRK_PIN, HIGH);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_RED, LOW);
}

void loop() {
  uint16_t dist = readDistance(); // 1. 读取当前距离
  checkEmergency(); // 2. 检测紧急避障

  // 3. 紧急制动模式
  if (mode == EMERGENCY_STOP) {
    emergencyStop();
    // 紧急解除后恢复正常制动
    if (!emergencyFlag && dist > 50) {
      mode = NORMAL_BRAKE;
      state = CRUISE;
      Serial.println("紧急解除，恢复分级制动");
      digitalWrite(LED_RED, LOW);
    }
    return;
  }

  // 4. 正常分级制动
  switch (state) {
    case IDLE:
      digitalWrite(LED_GREEN, HIGH);
      state = CRUISE;
      break;

    case CRUISE:
      currentSpeed = BASE_SPEED;
      applyMotor(currentSpeed, HIGH);
      if (dist < DIST_LEVEL[0]) {
        state = FIRST_BRAKE;
        Serial.println("进入一级制动");
      }
      break;

    case FIRST_BRAKE:
      normalBrake(0);
      if (dist < DIST_LEVEL[1]) {
        state = SECOND_BRAKE;
        Serial.println("进入二级制动");
      }
      break;

    case SECOND_BRAKE:
      normalBrake(1);
      if (dist < DIST_LEVEL[2]) {
        state = THIRD_BRAKE;
        Serial.println("进入三级制动");
      }
      break;

    case THIRD_BRAKE:
      normalBrake(2);
      if (dist < 15) {
        state = PARKING;
        Serial.println("进入停靠阶段");
      }
      break;

    case PARKING:
      // 缓慢降速到0，平滑停靠
      currentSpeed = max(currentSpeed - 10, 0);
      applyMotor(currentSpeed, HIGH);
      if (currentSpeed == 0) {
        digitalWrite(BRK_PIN, LOW); // 锁定制动
        digitalWrite(LED_GREEN, LOW);
        Serial.println("停靠完成");
      }
      break;
  }
  delay(15);
}

// 检测紧急避障，触发急停
void checkEmergency() {
  bool obs = digitalRead(IR_AVOID) == HIGH;
  if (obs) {
    emergencyFlag = true;
    mode = EMERGENCY_STOP;
    Serial.println("检测到紧急障碍，触发应急制动");
  } else {
    emergencyFlag = false;
  }
}

// 正常分级制动，逐级降速
void normalBrake(int level) {
  int target = SPEED_SET[level];
  // 线性缓降，避免速度突变
  if (currentSpeed > target) {
    currentSpeed -= (level + 2); // 制动强度随等级提升
  }
  applyMotor(currentSpeed, HIGH);
  digitalWrite(LED_GREEN, currentSpeed > 0 ? HIGH : LOW);
}

// 应急急停
void emergencyStop() {
  // 快速降速，切断动力并启用制动
  currentSpeed = max(currentSpeed - 30, 0);
  applyMotor(currentSpeed, LOW); // 方向拉低，辅助减速
  digitalWrite(LED_RED, HIGH);
  if (currentSpeed == 0) {
    digitalWrite(BRK_PIN, LOW); // 锁定位置
  }
}

// 读取距离值
uint16_t readDistance() {
  uint16_t raw = analogRead(DIST_SENSOR);
  return map(raw, 0, 1023, 0, 500); // 映射为0-500cm
}

// 驱动电机
void applyMotor(uint8_t spd, bool dir) {
  digitalWrite(DIR_PIN, dir);
  analogWrite(PWM_PIN, spd);
}

适用场景优化
增加场景识别，根据场景类型（空旷/狭窄）调整制动阈值，适配不同地形
优化应急恢复机制，加入速度平滑过渡，避免急停后重启的冲击
加入速度上限动态调整，狭窄区域提前降速，减少紧急制动触发概率

6、重载机器人三级制动与防溜坡控制程序（核心：重载平稳制动）
适用场景：重载搬运、坡地作业机器人，重载条件下实现平稳制动，同时防止停靠后溜坡，保障作业安全。#### 核心逻辑

结合负载参数，调整三级制动的减速速率，避免重载惯性导致的制动不足
三级制动后，启用反扭矩与硬件制动双重锁定，防止溜坡
制动过程中实时监控速度，异常时自动升级制动等级

// 重载机器人三级制动+防溜坡控制程序
// 硬件：BLDC、编码器、倾角传感器、负载检测、硬件制动器
// 功能：重载分级制动，速度自适应调节，溜坡防护，安全停靠

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM_PIN = 9, DIR_PIN = 8, BRK_PIN = 7;
const byte ENC_PIN = 2;
const byte ANGLE_PIN = A0;  // 倾角传感器
const byte LOAD_PIN = A1;  // 负载检测（压力传感器）

// ========== 制动参数 ==========
enum BrakeStage { CRUISE, B1, B2, B3, LOCKED };
BrakeStage stage = CRUISE;
// 距离阈值（根据负载动态调整，此处简化）
const int DIST_T[3] = {300, 150, 60};
// 基础速度与负载系数
const uint8_t BASE_SPD = 180;
uint8_t loadFactor = 100; // 负载系数，0-100，越大负载越重
int targetSpeed[3] = {160, 90, 40}; // 三级目标速度（可随负载调整）
int currentSpeed = 0;
float pitchAngle = 0;      // 坡度角度
bool slopeFlag = false;    // 是否存在坡度

// ========== 函数声明 ==========
void calcLoadFactor();
float readAngle();
void slopeProtection();
void heavyBrake(int level);

void setup() {
  Serial.begin(115200);
  Serial.println("重载制动系统启动");

  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(BRK_PIN, OUTPUT);
  pinMode(LOAD_PIN, INPUT);
  pinMode(ANGLE_PIN, INPUT);
  digitalWrite(BRK_PIN, HIGH);
}

void loop() {
  calcLoadFactor(); // 1. 读取负载，计算系数
  float angle = readAngle(); // 2. 读取坡度 pitchAngle = angle;
  slopeFlag = (angle > 3.0f); // 坡度大于3°判定为坡地

  static int dist = 400;
  if (dist > 0) dist--; // 模拟距离递减

  // 3. 状态机：分级制动
  switch (stage) {
    case CRUISE:
      // 巡航阶段，坡地适度降速
      int cruiseSpd = BASE_SPEED - loadFactor / 20;
      cruiseSpd = constrain(cruiseSpd, 120, BASE_SPEED);
      currentSpeed = cruiseSpd;
      applyMotor(currentSpeed, HIGH);
      if (dist < DIST_T[0]) {
        stage = B1;
        Serial.println("重载一级制动启动");
      }
      break;

    case B1:
      heavyBrake(0);
      if (dist < DIST_T[1]) {
        stage = B2;
        Serial.println("重载二级制动启动");
      }
      break;

    case B2:
      heavyBrake(1);
      if (dist < DIST_T[2]) {
        stage = B3;
        Serial.println("重载三级制动启动");
      }
      break;

    case B3:
      heavyBrake(2);
      if (dist < 20) { // 接近终点
        stage = LOCKED;
        Serial.println("进入锁停阶段");
      }
      break;

    case LOCKED:
      // 低速滑行至停，坡地加大制动力
      if (slopeFlag) {
        currentSpeed = max(currentSpeed - 15, 0);
        applyMotor(currentSpeed, LOW); // 反扭矩制动
      } else {
        currentSpeed = max(currentSpeed - 8, 0);
        applyMotor(currentSpeed, HIGH);
      }
      slopeProtection(); // 溜坡监测
      if (currentSpeed == 0) {
        digitalWrite(BRK_PIN, LOW); // 启用硬件锁定
        Serial.println("重载制动完成，已锁定，防溜坡生效");
      }
      break;
  }
  Serial.print("负载：");Serial.print(loadFactor);Serial.print("°坡度：");Serial.print(pitchAngle);
  Serial.print(" 当前速度：");Serial.println(currentSpeed);
  delay(20);
}

// 计算负载系数
void calcLoadFactor() {
  uint16_t loadRaw = analogRead(LOAD_PIN);
  loadFactor = map(loadRaw, 0, 1023, 30, 100); // 映射为30-100
}

// 读取坡度角度（简化，直接读取电压映射）
float readAngle() {
  uint16_t raw = analogRead(ANGLE_PIN);
  return (raw - 512) * 0.1f; // 转换为角度，±50°
}

// 重载分级制动，根据负载调整减速速率
void heavyBrake(int level) {
// 负载越大，降速越慢，避免惯性冲击
  int decRate = 5 + (100 - loadFactor) * (level + 1) / 10;
  if (currentSpeed > targetSpeed[level]) {
    currentSpeed -= decRate;
    currentSpeed = constrain(currentSpeed, targetSpeed[level], 255);
  }
  // 坡地额外增加制动力
  if (slopeFlag) {
    currentSpeed -= 3;
  }
  applyMotor(currentSpeed, level >= 1 ? HIGH : LOW);
}

// 溜坡防护：检测到位置偏移或速度反弹，立即锁死制动
void slopeProtection() {
  // 简化逻辑：速度为0时，若坡度存在，立即锁死
  if (currentSpeed == 0 && slopeFlag) {
    digitalWrite(BRK_PIN, LOW);
    Serial.print("检测到坡度，已锁死制动防溜坡，角度："); Serial.println(pitchAngle);
  }
}

// 驱动电机
void applyMotor(int spd, bool dir) {
  spd = constrain(spd, 0, 255);
  digitalWrite(DIR_PIN, dir);
  analogWrite(PWM_PIN, spd);
}

适用场景优化
增加编码器实时反馈，精准计算实际速度与位移，提升制动精度
优化负载检测精度，多传感器融合判断负载等级，适配不同重载场景
加入溜坡预警，检测到轻微位移时提前加大制动力，进一步降低溜坡风险

要点解读
要点1：三级制动区间需“按负载与路径精准划分”，奠定平稳基础
制动区间的合理划分是平稳分级的前提，需结合工况动态适配。
距离层级匹配：根据作业总距离、速度合理划分三级制动区间，一级制动负责大幅降速，二级负责精细调速，三级负责精准停靠，各区间互补覆盖
负载自适应调整：重载机器人需延长制动区间、降低目标速度，预留足够减速距离，匹配大惯性特性，避免制动不足
场景差异化设定：平坦路面制动区间紧凑，坡地作业延长制动距离、提升低等级制动力，适配不同地形的制动需求，保障基础平稳性

要点2：状态机切换需“平滑过渡无跳变”，避免速度冲击
状态机的平滑切换直接决定制动的连贯性，需优化过渡逻辑。
状态触发阈值精确：一级、二级、三级制动的触发距离精准设置，避免区间交叉或间隔过大，确保制动无间隙、不突兀- 速度缓变控制：阶段切换时采用线性插值、限幅降速，避免速度突变，通过PID算法快速且平滑地逼近目标速度，降低切换冲击
状态锁定机制：每个状态设置执行阈值，未达标不切换，如三级制动需将速度降至预设范围再进入停靠，保障过渡连贯

要点3：制动力需“分级量化+动态补偿”，匹配不同制动阶段
制动强度的精准控制是分级制动的核心，需分阶段量化与补偿。
制动强度分级：一级制动减速平缓，二级适中，三级精准微调，逐级提升制动强度，遵循“先缓后微”的原则，实现平稳降速
环境动态补偿：坡地、重载、强风等场景下，提升低等级制动力，或增加反向扭矩补偿，抵消惯性、重力、外力的影响
执行器协同控制：结合PWM调速与硬件制动，三级制动后期启用硬件制动辅助，最后锁定位置，既保证平稳又实现可靠停靠

要点4：传感器反馈需“实时精准”，支撑闭环控制
实时准确的数据反馈是闭环制动的关键，直接影响制动精度。
多传感器融合：融合编码器（速度）、距离传感器（位置）、倾角传感器（坡度）、负载传感器（重量），全面感知机器人状态，为制动提供多维度数据支撑
数据快速采样：缩短传感器采样周期，一般控制在15-20ms，实时捕捉速度、距离变化，及时调整制动力，避免滞后
数据滤波优化：对传感器数据进行中值、均值滤波，过滤噪声干扰，避免异常数据导致制动误判，提升制动稳定性

要点5：安全防护需“全覆盖”，保障制动过程与停靠稳定
制动系统需兼顾过程安全与停靠安全，设置多重防护机制。
应急制动兜底：设置独立紧急制动通道，遇障碍或系统异常时触发急停，跳过分级直接锁定，保障安全优先级最高
防溜坡保护：重载、坡地场景停靠后，启用硬件制动锁定+反扭矩保持，监测到溜坡趋势立即加大制动力，防止装甲移位
故障自检与异常处理：检测到传感器失效、电机堵转、制动力不足等异常，自动切断动力并锁死制动，记录故障信息，避免危险发生，适配复杂工况的稳定运行要求

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