“Arduino BLDC 之三机器人编队保持 + VFF 逃逸”可以理解为：三台机器人采用领航-跟随或分布式协同方式保持期望队形；当遇到障碍物、狭窄通道或相互干扰时，引入虚拟力场/向量场机制，使编队临时变形、绕行并从局部死锁中逃逸；底层由 BLDC 差速或全向底盘执行速度指令，Arduino 负责传感器采集、局部通信、力场计算和电机控制。
一、主要特点

1. 编队保持与局部避障分层耦合
   上层负责维持三机之间的相对位置，例如纵队、三角队形或楔形；下层通过虚拟力场处理障碍物斥力、队友互斥和目标引力。两者不是简单叠加，而是按优先级和权重合成，避免“既要保持队形又要避让障碍”时出现指令冲突。
2. VFF 提供反应式避障能力
   虚拟力场将环境障碍、队友位置和全局目标转化为合力矢量。障碍物产生斥力，目标或领航者产生引力，队友之间也可设置互斥力，从而在动态环境中实现连续、平滑的避障路径。
3. 逃逸机制解决局部极小值问题
   传统人工势场容易在 U 型障碍、对称障碍或三机相互夹持时陷入振荡或停滞。VFF 逃逸通常通过随机扰动、临时子目标点、向量场旋转或速度空间调整，使机器人脱离“合力接近零但仍未到达目标”的状态。
4. 三机编队拓扑简单但典型
   三台机器人足以验证领航-跟随、虚拟结构、一致性或势场编队等核心问题，同时又能暴露通信延迟、邻居切换、队形恢复和避障优先级等真实工程难点。
5. BLDC FOC 提供柔顺执行基础
   VFF 输出的是连续变化的速度/角速度指令，若电机执行层存在转矩脉动或响应滞后，编队容易抖动。BLDC 配合编码器闭环和 FOC，可在频繁加减速、差速转向和小角度修正时降低执行误差。
6. 分布式通信决定编队稳定性
   三机之间需要共享位置、航向、速度或状态字。若采用 NRF24L01、ESP-NOW、CAN 或 UART 中继，通信周期和丢包率会直接影响虚拟力场计算的时效性。
   二、应用场景
7. 小型仓储/实验室 AGV 编队
   三台 AGV 以纵队或三角队形搬运物料，遇到货架、托盘或临时障碍时自动绕行，障碍清除后恢复队形。
8. 走廊/管廊巡检集群
   在狭窄通道中保持固定间距，遇到行人、设备或门洞时通过 VFF 斥力拉开距离，通过后重新聚合。
9. 教学与科研平台
   适合演示领航-跟随、人工势场、局部极小值逃逸、差速运动学、FOC 速度闭环和多机通信同步。
10. 搜救/危险环境探测
    三机编队进入未知区域，领航者负责探路，跟随者保持覆盖范围；遇到废墟、转角或临时障碍时弹性变形并逃逸。
11. 编队表演与协同展示
    三机可完成三角旋转、穿插换位、避障重组等动作，VFF 逃逸能让动作更自然，减少卡死和碰撞。
12. 农业/园区协同巡检
    多台小型机器人沿路径移动，保持传感覆盖；遇到田埂、树木、路障时局部绕行，避免整体停滞。
    三、需要注意的事项
13. 引力、斥力与互斥力必须分权重设计
    目标引力过大会导致机器人贴障通过；障碍斥力过大会使机器人远离通道；队友互斥过强会让编队难以聚合。应分别整定引力增益、障碍斥力增益、队友互斥增益和作用距离。
14. 局部极小值逃逸不能持续开启
    随机扰动或向量场旋转应在检测到“速度过低、合力过小、误差长期不收敛”时触发，并在脱离后及时关闭，否则编队会持续漂移。
15. 避障优先级应高于队形保持
    当障碍物距离低于安全阈值时，应暂时降低队形保持权重，优先保证不碰撞。避障结束后再逐步恢复队形，避免机器人在障碍边缘反复拉扯。
16. 三机邻居关系切换要平滑
    绕行过程中，跟随者可能暂时失去原领航者视线或通信，需要切换到备用邻居或虚拟领航点。切换时应限制目标点跳变，防止速度指令突变。
17. BLDC 速度指令必须限幅、限加速度
    VFF 合力变化较快，直接下发给电机会造成急转、打滑和电流尖峰。应对线速度、角速度和加速度设置上限，并加入斜坡滤波。
18. Arduino 算力需合理分配
    ATmega328P 可承担单机 FOC、简单 VFF 计算和低频通信；若同时运行三机状态同步、多障碍感知、逃逸判断和编码器闭环，建议迁移到 ESP32-S3、Teensy 4.1、STM32，或采用“上位机规划 + 下位机执行”的分层架构。
19. 通信延迟要有超时保护
    若跟随者长时间未收到领航者或邻居状态，应进入安全模式，例如减速、原地等待或切换为本地避障，而不是继续按过期数据运动。
20. 相对定位误差会累积
    仅靠编码器和 IMU 估计三机相对位姿，时间一长队形会漂移。可加入 UWB、视觉标签、共享航点或外部定位进行校正。
21. 传感器盲区需处理
    超声波、红外或 ToF 在近距离、强反射面或窄通道中可能误报。应加入滑动窗口滤波、异常值剔除和最大有效距离限制。
22. 实机测试应从双机开始
    先验证单机 VFF 避障，再做双机互斥与跟随，最后扩展为三机编队。每增加一机，通信拓扑和力场耦合复杂度都会明显上升。

整体链路是：获取领航者/邻居状态与障碍信息 → 计算目标引力、障碍斥力和队友互斥力 → 检测局部死锁并触发 VFF 逃逸 → 合成期望线速度和角速度 → 限幅限加速度后下发 BLDC FOC → 编码器/IMU 闭环修正 → 障碍清除后平滑恢复队形。

1、三机器人编队 VFF 基础力场 + 旋转逃逸力（核心框架）
适用场景：三台机器人保持三角/菱形编队行进，前方出现障碍物时，在基础斥力之上叠加一个切向旋转力，使编队绕障碍物旋转绕行而非正面顶撞。核心逻辑直接来自搜索结果中的“旋转力场逃逸”代码：斥力负责“推开”，旋转力负责“绕过”。

#include <SimpleFOC.h>
#include <math.h>

// 双轮 BLDC 差速底盘（每台机器人相同配置）
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);

// 本机状态（通过 UWB/通信获取，此处简化模拟）
float selfX = 0, selfY = 0, selfTheta = 0;
// 队友状态（通信获取）
struct Teammate { float x, y; bool escaping; };
Teammate mates[2] = {{-1.0, 0.5, false}, {1.0, 0.5, false}};

// 障碍物状态（超声波/激光获取）
float obstacleX = 2.0, obstacleY = 0.0;
float obstacleDist = 999;

// ===== 力场参数 =====
#define FORMATION_K      1.5   // 编队保持力系数
#define FORMATION_RADIUS 1.5   // 编队期望间距 (m)
#define ATTRACTION_K     0.8   // 目标引力系数
#define REPULSION_K      20.0  // 障碍斥力系数
#define ROTATION_K       15.0  // 旋转逃逸力系数
#define ESCAPE_DIST      0.20  // 逃逸触发距离 (m)
#define VORTEX_DIR       1.0   // 涡旋方向：+1 逆时针，-1 顺时针

// 目标点
float targetX = 5.0, targetY = 0.0;

// 力累加器
struct Force { float x, y, theta; };
Force totalForce = {0, 0, 0};

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  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(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

// ===== 编队保持力（虚拟弹簧） =====
void addFormationForce() {
  for (int i = 0; i < 2; i++) {
    float dx = selfX - mates[i].x;
    float dy = selfY - mates[i].y;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < 0.1) continue;

    float error = dist - FORMATION_RADIUS;
    // 队友逃逸时降低编队约束力，避免拉扯
    float k = mates[i].escaping ? FORMATION_K * 0.5 : FORMATION_K;

    totalForce.x += k * error * (dx / dist);
    totalForce.y += k * error * (dy / dist);
  }
}

// ===== 目标引力 =====
void addAttractionForce() {
  float dx = targetX - selfX;
  float dy = targetY - selfY;
  float dist = sqrt(dx*dx + dy*dy);
  if (dist > 0.1) {
    totalForce.x += ATTRACTION_K * (dx / dist);
    totalForce.y += ATTRACTION_K * (dy / dist);
  }
}

// ===== 障碍斥力 + 旋转逃逸力 =====
void addObstacleForce() {
  if (obstacleDist > 0.1 && obstacleDist < ESCAPE_DIST * 3.0) {
    float dx = selfX - obstacleX;
    float dy = selfY - obstacleY;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < 0.1) return;

    // 基础斥力：距离越近，斥力越强
    float repulsionMag = REPULSION_K * (1.0 / (dist / ESCAPE_DIST));
    totalForce.x += repulsionMag * (dx / dist);
    totalForce.y += repulsionMag * (dy / dist);

    // ===== 旋转逃逸力：垂直于斥力方向的切向力 =====
    // 这是“绕行”而非“顶撞”的关键
    float perpX = -dy / dist;  // 垂直方向
    float perpY = dx / dist;
    totalForce.x += ROTATION_K * VORTEX_DIR * perpX;
    totalForce.y += ROTATION_K * VORTEX_DIR * perpY;

    Serial.print("斥力+旋转: dist="); Serial.println(dist);
  }
}

// ===== 朝向调整力 =====
void addOrientationForce() {
  float targetTheta = atan2(totalForce.y, totalForce.x) * 180.0 / PI;
  float thetaError = targetTheta - selfTheta;
  while (thetaError > 180) thetaError -= 360;
  while (thetaError < -180) thetaError += 360;
  totalForce.theta = thetaError * 5.0;
}

// ===== 力 → 差速执行 =====
void executeForce() {
  float speedMap = 0.8, turnMap = 1.2;
  float targetSpeed = totalForce.x * speedMap;
  float targetTurn = totalForce.theta * turnMap;

  float vL = targetSpeed - targetTurn / 2.0;
  float vR = targetSpeed + targetTurn / 2.0;

  vL = constrain(vL, -2.0, 2.0);
  vR = constrain(vR, -2.0, 2.0);

  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 模拟传感器更新
  obstacleDist = 0.25;  // 假设障碍物在 25cm 处
  obstacleX = selfX + 0.25;  // 简化

  // 清空力
  totalForce = {0, 0, 0};

  // 叠加各力
  addFormationForce();
  addAttractionForce();
  addObstacleForce();
  addOrientationForce();

  // 执行
  executeForce();

  delay(50);
}

解读：该案例直接复现了搜索结果中的核心力场结构。addObstacleForce() 中的 perpX/perpY 是旋转逃逸的关键——它们在斥力方向的基础上旋转 90°，产生一个切向分量，使机器人沿障碍物边缘“滑过”而非正面被推开。VORTEX_DIR 控制绕行方向（逆时针/顺时针），在对称障碍场景中需要配合案例二的自适应方向选择来打破死锁。搜索结果中 calculateEscapeForce() 的 force.x += ROTATION_K * VORTEX_DIRECTION * perpX 与本案例逻辑一致。

2、自适应旋转方向选择 + 逃逸状态下的编队力衰减
适用场景：障碍物在正前方且左右空间大致对称时，固定 VORTEX_DIR 会导致所有机器人向同一侧旋转，可能引发编队内部碰撞或绕行效率低下。本案例增加开阔度评估来选择旋转方向，并在队友逃逸时降低编队约束力，避免“逃逸中的机器人被编队力拉回障碍物”。

#include <SimpleFOC.h>
#include <NewPing.h>
#include <math.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);

// 三方向超声波（评估开阔度）
NewPing sonarF(2, 3, 200);
NewPing sonarL(4, 5, 200);
NewPing sonarR(6, 7, 200);

float selfX = 0, selfY = 0, selfTheta = 0;
struct Teammate { float x, y; bool escaping; };
Teammate mates[2] = {{-1.5, 0.8, false}, {1.5, 0.8, false}};

// 力场参数
#define FORMATION_K      1.5
#define FORMATION_RADIUS 1.5
#define ATTRACTION_K     0.6
#define REPULSION_K      25.0
#define ROTATION_K       18.0
#define ESCAPE_DIST      0.20

float obstacleDistF = 999, obstacleDistL = 999, obstacleDistR = 999;

struct Force { float x, y, theta; };
Force totalForce;

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  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(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

// 编队力：队友逃逸时衰减
void addFormationForce() {
  for (int i = 0; i < 2; i++) {
    float dx = selfX - mates[i].x;
    float dy = selfY - mates[i].y;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < 0.1) continue;

    float error = dist - FORMATION_RADIUS;
    float k = mates[i].escaping ? FORMATION_K * 0.3 : FORMATION_K;
    totalForce.x += k * error * (dx / dist);
    totalForce.y += k * error * (dy / dist);
  }
}

// 障碍力：自适应旋转方向
void addObstacleForceAdaptive() {
  if (obstacleDistF > ESCAPE_DIST * 3.0) return;

  // ===== 开阔度评估：左右哪侧空间更大 =====
  float leftOpenness = (obstacleDistL > 0.8) ? obstacleDistL : 0;
  float rightOpenness = (obstacleDistR > 0.8) ? obstacleDistR : 0;

  float vortexDir;
  if (leftOpenness > rightOpenness * 1.2) {
    vortexDir = -1.0;  // 左侧开阔 → 顺时针（向右转绕行）
    Serial.println("左侧开阔，顺时针逃逸");
  } else if (rightOpenness > leftOpenness * 1.2) {
    vortexDir = 1.0;   // 右侧开阔 → 逆时针
    Serial.println("右侧开阔，逆时针逃逸");
  } else {
    // 对称：随机选择打破死锁
    vortexDir = (random(100) < 50) ? 1.0 : -1.0;
    Serial.println("对称障碍，随机逃逸方向");
  }

  float dx = selfX - (selfX + obstacleDistF);  // 简化：障碍在前方
  float dy = 0;
  float dist = obstacleDistF;
  if (dist < 0.1) return;

  // 斥力
  float repMag = REPULSION_K * (1.0 / (dist / ESCAPE_DIST));
  totalForce.x += repMag * (dx / dist);
  totalForce.y += repMag * (dy / dist);

  // 旋转逃逸力（方向自适应）
  float perpX = -dy / dist;
  float perpY = dx / dist;
  totalForce.x += ROTATION_K * vortexDir * perpX;
  totalForce.y += ROTATION_K * vortexDir * perpY;
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 读取三方向距离
  obstacleDistF = sonarF.ping_cm() / 100.0;
  obstacleDistL = sonarL.ping_cm() / 100.0;
  obstacleDistR = sonarR.ping_cm() / 100.0;

  // 判定本机是否逃逸
  bool escaping = (obstacleDistF > 0 && obstacleDistF < ESCAPE_DIST * 2.0);

  totalForce = {0, 0, 0};

  // 编队力（考虑队友逃逸状态）
  addFormationForce();

  // 目标引力
  float dxT = 5.0 - selfX, dyT = 0 - selfY;
  float dT = sqrt(dxT*dxT + dyT*dyT);
  if (dT > 0.1) {
    totalForce.x += ATTRACTION_K * (dxT / dT);
    totalForce.y += ATTRACTION_K * (dyT / dT);
  }

  // 障碍力（自适应旋转）
  addObstacleForceAdaptive();

  // 朝向
  float targetTheta = atan2(totalForce.y, totalForce.x) * 180.0 / PI;
  float thetaErr = targetTheta - selfTheta;
  while (thetaErr > 180) thetaErr -= 360;
  while (thetaErr < -180) thetaErr += 360;
  totalForce.theta = thetaErr * 6.0;

  // 执行
  float speed = constrain(totalForce.x * 0.7, -1.5, 1.5);
  float turn = constrain(totalForce.theta * 1.0, -1.5, 1.5);
  motorL.move((speed - turn * 0.125) / 0.05);
  motorR.move((speed + turn * 0.125) / 0.05);

  delay(40);
}

解读：该案例在案例一基础上增加了两个关键机制。第一是 vortexDir 的自适应选择：leftOpenness > rightOpenness * 1.2 时选择顺时针（向右侧开阔方向绕行），反之逆时针，对称时随机打破死锁。搜索结果中“旋转力场方向自适应”的原文描述与此一致：“实时评估左右两侧空间的开阔度，自动选择斥力最小的方向作为编队的旋转方向”。第二是 mates[i].escaping 标志使编队力衰减到 30%——当一台机器人正在逃逸时，如果队友仍以全力的编队弹簧力拉它，逃逸机器人会被“拽回”障碍物附近，导致逃逸失败。搜索结果中代码片段同样包含 if (teammates[i].isEscaping) currentFormationK = FORMATION_K * 0.5; 的逻辑。

3、逃逸-恢复状态机 + 编队拓扑弹性保持（避免振荡）
场景：旋转逃逸力使机器人绕开障碍物后，如果立即切回纯编队力，机器人可能因编队弹簧的拉力“弹回”障碍物附近，形成振荡。需要状态机管理“编队模式 → 逃逸模式 → 恢复模式”的过渡，在恢复模式下逐步增加编队力权重，使机器人平滑回归队形而非被猛拉回去。

#include <SimpleFOC.h>
#include <math.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);

float selfX = 0, selfY = 0, selfTheta = 0;
struct Teammate { float x, y; };
Teammate mates[2] = {{-1.5, 0.8}, {1.5, 0.8}};

// 状态机
enum Mode { FORMATION, ESCAPE, RECOVERY };
Mode mode = FORMATION;
unsigned long modeTimer = 0;

// 力场参数
#define FORMATION_K      1.8
#define FORMATION_RADIUS 1.5
#define ATTRACTION_K     0.5
#define REPULSION_K      30.0
#define ROTATION_K       22.0
#define ESCAPE_DIST      0.18
#define SAFE_DIST        0.50

float obstacleDist = 999;
float obstacleX = 2.0, obstacleY = 0.0;
float escapeDir = 1.0;

// 恢复模式权重（0→1 渐进）
float recoveryWeight = 0.0;

struct Force { float x, y, theta; };
Force totalForce;

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  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(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void updateMode() {
  bool nearObstacle = (obstacleDist > 0 && obstacleDist < ESCAPE_DIST * 2.0);

  switch (mode) {
    case FORMATION:
      if (nearObstacle) {
        mode = ESCAPE;
        modeTimer = millis();
        escapeDir = (random(100) < 50) ? 1.0 : -1.0;
        recoveryWeight = 0.0;
        Serial.println("→ 逃逸模式");
      }
      break;

    case ESCAPE:
      // 逃逸至少持续 400ms，且障碍已远离
      if (millis() - modeTimer > 400 && obstacleDist > SAFE_DIST) {
        mode = RECOVERY;
        modeTimer = millis();
        recoveryWeight = 0.0;
        Serial.println("→ 恢复模式");
      }
      // 超时保护：2秒后强制恢复
      if (millis() - modeTimer > 2000) {
        mode = RECOVERY;
        recoveryWeight = 0.0;
        Serial.println("→ 超时强制恢复");
      }
      break;

    case RECOVERY:
      // 恢复权重从 0 渐进到 1（约 1.5 秒）
      recoveryWeight += 0.015;
      if (recoveryWeight >= 1.0) {
        recoveryWeight = 1.0;
        mode = FORMATION;
        Serial.println("→ 编队模式");
      }
      // 再次靠近障碍：切回逃逸
      if (nearObstacle && recoveryWeight < 0.5) {
        mode = ESCAPE;
        modeTimer = millis();
        Serial.println("→ 恢复中再遇障碍，重回逃逸");
      }
      break;
  }
}

void computeForces() {
  totalForce = {0, 0, 0};

  // ===== 编队力（权重随模式变化） =====
  float formWeight = 1.0;
  if (mode == ESCAPE) formWeight = 0.2;       // 逃逸时大幅降低
  else if (mode == RECOVERY) formWeight = recoveryWeight;  // 渐进恢复

  for (int i = 0; i < 2; i++) {
    float dx = selfX - mates[i].x;
    float dy = selfY - mates[i].y;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist > 0.1) {
      float error = dist - FORMATION_RADIUS;
      totalForce.x += formWeight * FORMATION_K * error * (dx / dist);
      totalForce.y += formWeight * FORMATION_K * error * (dy / dist);
    }
  }

  // ===== 目标引力 =====
  float dxT = 5.0 - selfX, dyT = 0 - selfY;
  float dT = sqrt(dxT*dxT + dyT*dyT);
  if (dT > 0.1) {
    totalForce.x += ATTRACTION_K * (dxT / dT);
    totalForce.y += ATTRACTION_K * (dyT / dT);
  }

  // ===== 障碍力（仅逃逸模式 + 恢复初期） =====
  if (mode == ESCAPE || (mode == RECOVERY && recoveryWeight < 0.7)) {
    if (obstacleDist < SAFE_DIST) {
      float dx = selfX - obstacleX;
      float dy = selfY - obstacleY;
      float dist = sqrt(dx*dx + dy*dy);
      if (dist > 0.1) {
        float repMag = REPULSION_K * (1.0 / (dist / ESCAPE_DIST));
        totalForce.x += repMag * (dx / dist);
        totalForce.y += repMag * (dy / dist);

        // 旋转逃逸
        totalForce.x += ROTATION_K * escapeDir * (-dy / dist);
        totalForce.y += ROTATION_K * escapeDir * (dx / dist);
      }
    }
  }
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 模拟障碍距离变化
  obstacleDist = 0.25;  // 简化

  updateMode();
  computeForces();

  // 朝向
  float targetTheta = atan2(totalForce.y, totalForce.x) * 180.0 / PI;
  float thetaErr = targetTheta - selfTheta;
  while (thetaErr > 180) thetaErr -= 360;
  while (thetaErr < -180) thetaErr += 360;
  totalForce.theta = thetaErr * 5.0;

  // 执行
  float speed = constrain(totalForce.x * 0.7, -1.8, 1.8);
  float turn = constrain(totalForce.theta * 1.0, -1.5, 1.5);
  motorL.move((speed - turn * 0.125) / 0.05);
  motorR.move((speed + turn * 0.125) / 0.05);

  delay(40);
}

解读：该案例的核心是 recoveryWeight 的渐进机制。逃逸成功后直接切回 FORMATION_K 的全额编队力，机器人会因弹簧拉力突然增大而被“拽”向队友方向，如果障碍物仍在附近，会被拽回障碍区。recoveryWeight += 0.015 使编队力在约 1.5 秒内从 0 逐步恢复到全额，机器人在此期间先依靠目标引力缓慢回归，避免“弹回”。搜索结果中“弹性编队与局部极小值逃逸”的描述强调“允许短暂偏离以避免碰撞”，恢复阶段的渐进权重正是对这一原则的执行层实现。RECOVERY 状态下 recoveryWeight < 0.5 时若再次靠近障碍，切回 ESCAPE，防止恢复过程中被障碍“捕获”。

要点解读

1. VFF 逃逸的核心是“切向旋转力”，而非单纯增大斥力
   搜索结果中旋转逃逸代码的关键行是 force.x += ROTATION_K * VORTEX_DIRECTION * perpX，其中 perpX = -dy/dist 是斥力方向的垂直分量。如果只增大 REPULSION_K，机器人会被正面推开，但可能陷入“推一下、回来、再推一下”的振荡。旋转力使合力方向偏离障碍物中心，产生绕行轨迹。ROTATION_K 与 REPULSION_K 的比例决定绕行弧度：旋转力占比越高，绕行越“圆滑”但回归目标越慢。

2. 旋转方向必须自适应或随机化，固定方向在对称场景中会死锁
   案例二的 vortexDir 根据左右超声波距离选择。如果固定 VORTEX_DIR = 1.0，三台机器人同时遇到正前方对称障碍时，所有机器人向同一侧旋转，编队内部的机器人可能被挤向障碍物或相互碰撞。搜索结果明确将“旋转力场方向自适应”作为菱形编队的核心特性，并指出其本质是“局部极小值逃逸策略——当对称布局导致引力与斥力平衡时，引入方向性扰动打破对称”。

3. 逃逸期间必须衰减编队保持力，否则逃逸机器人会被“拽回”
   案例二和案例三的 formWeight 在逃逸模式下降低到 0.2~0.3。如果逃逸机器人的编队弹簧力仍然全额生效，队友在障碍物另一侧产生的拉力会与障碍斥力对抗：斥力推它绕行，编队力拉它回归，合力可能指向障碍物本身。搜索结果中代码片段同样在队友 isEscaping 时将 currentFormationK 减半。衰减的代价是编队暂时松散，但这是“允许短暂偏离以避免碰撞”的必要妥协。

4. 逃逸到恢复的过渡需要渐进权重，避免“弹回”振荡
   案例三的 recoveryWeight += 0.015 是防止振荡的关键。逃逸成功后，机器人位于障碍物侧方，编队目标位置在障碍物另一侧。如果立即全额编队力，合力直接指向队友，机器人会沿直线“切”向编队位置——这条直线可能穿过障碍物。渐进权重使机器人先沿当前方向缓慢移动，编队力逐步增大，轨迹呈弧线回归而非直线穿插。搜索结果中“弹性编队”的概念强调“障碍消失后弹簧恢复力自动将机器人拉回标准编队位置”，但未明确恢复速率；工程上，恢复速率必须低于逃逸响应速率，否则逃逸失去意义。

5. Arduino 算力约束下，三机器人 VFF 的力计算应降频、BLDC 执行应保频
   案例中的 delay(40) 对应约 25Hz 的力场更新频率。三台机器人的力叠加涉及多个平方根、反正切和条件判断，在 Arduino Uno 上 25Hz 已接近负载。合理的架构是：FOC 环以最大频率运行（去掉 delay 或使用定时器），力场计算与状态机降频到 20-30Hz。搜索结果中“算力与实时性”被列为实际部署的重点关注项，并建议使用 ESP32 或 Teensy 而非 Uno 来承载三机协同的 VFF 计算与通信。如果使用 Uno，应将力场计算简化为查表或减少队友数量（如仅计算最近队友的编队力）。

4、三角队形 + VFF 合力 + 局部极小检测 + 切向逃逸
适用硬件：ESP32 + 差速 BLDC 底盘 ×3 + 编码器 + 前/左/右超声 + ESP-NOW。

核心思路：VFF（虚拟力场）把编队与避障统一成一个力场——引力指向队形中的目标位、斥力来自障碍。它很轻量，但有一个致命缺陷：两个障碍之间或障碍与队形目标之间必然存在合力为零的局部极小点，机器人在那里会“僵住”。所以关键不在调参，而在检测到极小后主动注入一个切向力，把机器人“推”出陷阱。

/*
 * 案例4：三角队形 + VFF + 局部极小检测 + 逃逸
 * 机器人 0 = Leader，1、2 = 跟随者（本代码运行在 1 或 2 上）
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

BLDCMotor wl(7), wr(7);
BLDCDriver3PWM dl(9,10,11,8), dr(22,23,24,25);
Encoder el(2,3,2048), er(32,33,2048);
void onL(){ el.handleA(); } void onR(){ er.handleA(); }

const float R_W = 0.065f, TRACK = 0.34f;
const uint8_t MY_ID = 1;

// ---- 队形：三角，Leader 在原点朝 +x ----
const float OFF[3][2] = {{0.0f, 0.0f}, {-0.85f, -0.62f}, {-0.85f, 0.62f}};

// ---- 机间广播 ----
struct Msg { uint8_t id; float x, y, th, v; unsigned long t; };
Msg leader = {0,0,0,0,0};
bool hasLeader = false;

void onRecv(const uint8_t*, const uint8_t* d, int len) {
  if (len != sizeof(Msg)) return;
  Msg m; memcpy(&m, d, sizeof(m));
  if (m.id == 0) { leader = m; hasLeader = (millis() - m.t < 500); }
}

// ---- 位姿 ----
float px = 0, py = 0, pth = 0, vCur = 0, wCur = 0;

// ---- VFF 参数 ----
const float K_ATT   = 1.20f;
const float K_REP   = 0.45f;
const float D_OBS   = 0.70f;     // 障碍影响半径
const float F_CLAMP = 2.00f;     // 合力上限

// ---- 局部极小检测 ----
unsigned long minT0 = 0;         // 疑似极小的起始时刻
bool  escape = false;
unsigned long escT0 = 0;
float escStartDist = 0;

// ---- 测距 ----
const int TRIG[3] = {6, 8, 10}, ECHO[3] = {7, 9, 11};
const float OBS_ANG[3] = {0.0f, -0.90f, 0.90f};   // 前 / 左 / 右
float dObs[3] = {9, 9, 9};

float readUS(int i) {
  digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
  digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
  long us = pulseIn(ECHO[i], HIGH, 15000);
  if (us <= 0) return 9.0f;
  float r = us * 0.0343f / 2.0f / 100.0f;
  return (r > 0.05f && r < 2.5f) ? r : 9.0f;
}

// ---- 目标位 ----
void goalPos(float &gx, float &gy) {
  gx = leader.x + OFF[MY_ID][0]*cosf(leader.th) - OFF[MY_ID][1]*sinf(leader.th);
  gy = leader.y + OFF[MY_ID][0]*sinf(leader.th) + OFF[MY_ID][1]*cosf(leader.th);
}

void setup() {
  for (int i = 0; i < 3; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
  el.init(); el.enableInterrupts(onL);
  er.init(); er.enableInterrupts(onR);
  wl.linkSensor(&el); wr.linkSensor(&er);
  wl.controller = wr.controller = MotionControlType::velocity;
  wl.PID_velocity.P = 0.35f; wl.PID_velocity.I = 4.0f;
  wr.PID_velocity.P = 0.35f; wr.PID_velocity.I = 4.0f;
  wl.voltage_limit = wr.voltage_limit = 12.0f;
  wl.current_limit = wr.current_limit = 4.0f;
  wl.init(); wr.init();
  dl.voltage_power_supply = dr.voltage_power_supply = 12.0f;
  dl.init(); dr.init();
  wl.linkDriver(&dl); wr.linkDriver(&dr);
  wl.initFOC(); wr.initFOC();

  WiFi.mode(WIFI_STA); esp_now_init();
  esp_now_register_recv_cb(onRecv);
  esp_now_peer_info_t pi = {};
  memset(pi.peer_addr, 0xFF, 6); pi.channel = 1;
  esp_now_add_peer(&pi);
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.2f) dt = 0.02f;

  // ---- 1. 感知 ----
  for (int i = 0; i < 3; i++) dObs[i] = readUS(i);

  // ---- 2. 目标位与引力 ----
  float gx, gy; goalPos(gx, gy);
  float ex = gx - px, ey = gy - py;
  float dAtt = hypotf(ex, ey);

  float Fx = 0, Fy = 0;
  float fAttMag = K_ATT * min(dAtt, 2.0f);
  if (dAtt > 1e-3f) { Fx += fAttMag*ex/dAtt; Fy += fAttMag*ey/dAtt; }

  // ---- 3. 障碍斥力 ----
  for (int i = 0; i < 3; i++) {
    if (dObs[i] >= D_OBS) continue;
    float a = pth + OBS_ANG[i];
    float mag = K_REP * (1.0f/max(dObs[i], 0.12f) - 1.0f/D_OBS);
    Fx -= mag * cosf(a);
    Fy -= mag * sinf(a);
  }

  // ---- 4. 局部极小检测：有目标、但合力被抵消 ----
  float fm = hypotf(Fx, Fy);
  if (!escape) {
    bool trapped = (dAtt > 0.45f) && (fm < 0.22f * fAttMag);
    if (trapped) {
      if (minT0 == 0) minT0 = now;
      else if (now - minT0 > 600) {
        escape = true; escT0 = now; escStartDist = dAtt;
        Serial.println("[VFF] local minimum -> ESCAPE");
      }
    } else minT0 = 0;
  }

  // ---- 5. 逃逸：沿等势线切向注入 ----
  if (escape) {
    // 切向 = 引力方向旋转 90°，方向选空间更大（更空）的一侧
    float side = (dObs[1] > dObs[2]) ? 1.0f : -1.0f;   // 左空则向左绕
    float tx = -ey/dAtt, ty = ex/dAtt;                  // 单位切向
    const float F_ESC = 0.9f;
    Fx += side * F_ESC * tx;
    Fy += side * F_ESC * ty;

    // 退出条件：靠近了目标 或 超时
    bool improved = (dAtt < escStartDist - 0.30f);
    bool timeout  = (now - escT0 > 3000);
    if (improved || timeout) {
      escape = false; minT0 = 0;
      Serial.println("[VFF] escape done");
    }
  }

  // ---- 6. 合力限幅 -> (v, w) ----
  float fm2 = hypotf(Fx, Fy);
  if (fm2 > F_CLAMP) { Fx *= F_CLAMP/fm2; Fy *= F_CLAMP/fm2; }

  float vCmd = 0, wCmd = 0;
  if (hasLeader && fm2 > 0.02f) {
    float hdg = atan2f(Fy, Fx);
    float err = hdg - pth;
    while (err >  PI) err -= 2*PI;
    while (err < -PI) err += 2*PI;

    float mag = min(fm2, 1.2f);
    vCmd = constrain(mag * cosf(err), -0.15f, 0.85f);
    if (fabsf(err) > 1.2f) vCmd = 0;                 // 角度太大先转不冲
    wCmd = constrain(2.0f * err, -0.9f, 0.9f);
  }

  // ---- 7. 执行 + 里程 ----
  float vL = vCmd - wCmd*TRACK*0.5f, vR = vCmd + wCmd*TRACK*0.5f;
  wl.loopFOC(); wr.loopFOC();
  wl.move(vL/R_W); wr.move(vR/R_W);

  float vl = wl.shaft_velocity*R_W, vr = wr.shaft_velocity*R_W;
  vCur = 0.5f*(vl+vr); wCur = (vr-vl)/TRACK;
  px += vCur*cosf(pth)*dt; py += vCur*sinf(pth)*dt; pth += wCur*dt;

  Serial.print("dAtt="); Serial.print(dAtt, 2);
  Serial.print(" |F|="); Serial.print(fm2, 2);
  Serial.print(" esc="); Serial.println(escape);
  delay(20);
}

关键点：极小判据用的是“合力模长远小于引力模长”而不是“速度≈0”。因为机器人可能正在缓慢蠕动（速度不为零）却已经完全被困住；而合力被抵消是局部极小的定义特征，比速度更早、更可靠地发出信号。

5、全队同步逃逸 + 队友斥力 + 方向投票
适用硬件：三台 ESP32 + 差速 BLDC + 多路超声 + ESP-NOW 组网。

核心思路：案例一各机独立判极小、独立逃逸，会出问题——一台向右绕、一台向左绕，队形立刻被撕开。这一版做两件事：① 把队友也算进斥力场（编队与避障统一在同一力场里，顺带解决机间碰撞）；② 逃逸是团队事件：任一机检测到极小就广播，全队用同一时刻、同一方向进入逃逸，并投票决定绕行侧。

/*
 * 案例5：队友斥力 + 全队同步逃逸 + 方向投票
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wl(7), wr(7);
// 电机初始化同案例一（略）
SemaphoreHandle_t mux;

const float R_W = 0.065f, TRACK = 0.34f;
const uint8_t MY_ID = 1;

const float OFF[3][2] = {{0.0f, 0.0f}, {-0.85f, -0.62f}, {-0.85f, 0.62f}};

// ---- 广播（含逃逸状态）----
struct Msg {
  uint8_t id;
  float x, y, th;
  bool  escape;        // 是否处于逃逸
  int8_t side;         // 建议绕行侧 +1 / -1
  uint8_t stamp;       // 逃逸轮次（避免重复触发）
  unsigned long t;
};
Msg peer[3];
bool alive[3] = {false, false, false};
uint8_t teamStamp = 0;

volatile bool  g_escape = false;
volatile int8_t g_side  = 1;
volatile uint8_t g_stamp = 0;

const float D_TEAM = 0.55f;      // 队友影响半径
const float K_TEAM = 0.55f;
const float DEAD   = 0.28f;      // 队友斥力死区（队形间距本身就在这个范围）

void onRecv(const uint8_t*, const uint8_t* d, int len) {
  if (len != sizeof(Msg)) return;
  Msg m; memcpy(&m, d, sizeof(m));
  if (m.id == MY_ID || m.id > 2) return;
  peer[m.id] = m;
  alive[m.id] = (millis() - m.t < 500);

  // 收到别人发起的逃逸：立即同步（同轮次不重复触发）
  if (m.escape && m.stamp != g_stamp) {
    g_escape = true; g_side = m.side; g_stamp = m.stamp;
  }
}

float px = 0, py = 0, pth = 0, vCur = 0, wCur = 0;

const int TRIG[3] = {6, 8, 10}, ECHO[3] = {7, 9, 11};
const float OBS_ANG[3] = {0.0f, -0.90f, 0.90f};
volatile float dObs[3] = {9, 9, 9};

float readUS(int i) {
  digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
  digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
  long us = pulseIn(ECHO[i], HIGH, 15000);
  if (us <= 0) return 9.0f;
  float r = us * 0.0343f / 2.0f / 100.0f;
  return (r > 0.05f && r < 2.5f) ? r : 9.0f;
}

// ---- 任务1：感知 20Hz ----
void taskSense(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    for (int i = 0; i < 3; i++) dObs[i] = readUS(i);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
  }
}

// ---- 任务2：力场 + 逃逸仲裁 20Hz ----
void taskPlan(void*) {
  TickType_t last = xTaskGetTickCount();
  unsigned long minT0 = 0;
  float escStartDist = 0, escT0 = 0;
  int8_t voteL = 0, voteR = 0;

  for (;;) {
    // ---- 目标位 ----
    float gx, gy;
    if (alive[0]) {
      gx = peer[0].x + OFF[MY_ID][0]*cosf(peer[0].th) - OFF[MY_ID][1]*sinf(peer[0].th);
      gy = peer[0].y + OFF[MY_ID][0]*sinf(peer[0].th) + OFF[MY_ID][1]*cosf(peer[0].th);
    } else { gx = px; gy = py; }

    float ex = gx - px, ey = gy - py;
    float dAtt = hypotf(ex, ey);
    float fAttMag = 1.2f * min(dAtt, 2.0f);

    float Fx = 0, Fy = 0;
    if (dAtt > 1e-3f) { Fx += fAttMag*ex/dAtt; Fy += fAttMag*ey/dAtt; }

    // ---- 障碍斥力 ----
    for (int i = 0; i < 3; i++) {
      if (dObs[i] >= 0.70f) continue;
      float a = pth + OBS_ANG[i];
      float mag = 0.45f*(1.0f/max(dObs[i], 0.12f) - 1.0f/0.70f);
      Fx -= mag*cosf(a); Fy -= mag*sinf(a);
    }

    // ---- 队友斥力（带死区，避免队形内耗）----
    for (int j = 0; j < 3; j++) {
      if (j == MY_ID || !alive[j]) continue;
      float dx = px - peer[j].x, dy = py - peer[j].y;
      float d = hypotf(dx, dy);
      if (d > D_TEAM || d < 1e-3f) continue;
      float mag = K_TEAM * (1.0f/max(d, 0.15f) - 1.0f/D_TEAM);
      if (d < DEAD) mag *= (DEAD - d) / DEAD * 0.5f;   // 死区内削弱
      Fx += mag * dx/d;
      Fy += mag * dy/d;
    }

    // ---- 极小检测 -> 发起全队逃逸 ----
    float fm = hypotf(Fx, Fy);
    if (!g_escape) {
      if (dAtt > 0.45f && fm < 0.22f*fAttMag) {
        if (minT0 == 0) minT0 = millis();
        else if (millis() - minT0 > 600) {
          // 投票：本机按空间大小给出建议，广播出去由对侧机复核
          int8_t mySide = (dObs[1] > dObs[2]) ? 1 : -1;
          teamStamp++;
          g_escape = true; g_side = mySide; g_stamp = teamStamp;
          escStartDist = dAtt; escT0 = millis();

          Msg m = {MY_ID, px, py, pth, true, mySide, teamStamp, millis()};
          esp_now_send(NULL, (uint8_t*)&m, sizeof(m));
        }
      } else minT0 = 0;
    }

    // ---- 逃逸中的切向注入 ----
    if (g_escape) {
      float tx = -ey/max(dAtt, 1e-3f), ty = ex/max(dAtt, 1e-3f);
      Fx += g_side * 0.9f * tx;
      Fy += g_side * 0.9f * ty;

      bool improved = (dAtt < escStartDist - 0.30f);
      bool timeout  = (millis() - escT0 > 3000);
      if (improved || timeout) {
        g_escape = false; minT0 = 0;
        Serial.println("[VFF] team escape done");
      }
    }

    // ---- 合力 -> 速度指令 ----
    float fm2 = hypotf(Fx, Fy);
    if (fm2 > 2.0f) { Fx *= 2.0f/fm2; Fy *= 2.0f/fm2; }

    float vCmd = 0, wCmd = 0;
    if (alive[0] && fm2 > 0.02f) {
      float hdg = atan2f(Fy, Fx);
      float err = hdg - pth;
      while (err >  PI) err -= 2*PI;
      while (err < -PI) err += 2*PI;
      vCmd = constrain(min(fm2,1.2f)*cosf(err), -0.15f, 0.85f);
      if (fabsf(err) > 1.2f) vCmd = 0;
      wCmd = constrain(2.0f*err, -0.9f, 0.9f);
    }

    xSemaphoreTake(mux, portMAX_DELAY);
    // 供执行层读取（示意：写入全局）
    extern volatile float c_v, c_w;
    c_v = vCmd; c_w = wCmd;
    xSemaphoreGive(mux);

    // 心跳广播
    static unsigned long lastBc = 0;
    if (millis() - lastBc > 100) {
      lastBc = millis();
      Msg m = {MY_ID, px, py, pth, g_escape, g_side, g_stamp, millis()};
      esp_now_send(NULL, (uint8_t*)&m, sizeof(m));
    }
    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
  }
}

volatile float c_v = 0, c_w = 0;

// ---- 任务3：执行 1kHz ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float vR = 0, wR = 0;
  for (;;) {
    float v, w;
    xSemaphoreTake(mux, portMAX_DELAY);
    v = c_v; w = c_w;
    xSemaphoreGive(mux);

    vR += constrain(v - vR, -0.002f, 0.001f);
    wR += constrain(w - wR, -0.005f, 0.005f);

    float vL = vR - wR*TRACK*0.5f, vRt = vR + wR*TRACK*0.5f;
    wl.loopFOC(); wr.loopFOC();
    wl.move(vL/R_W); wr.move(vRt/R_W);

    float vl = wl.shaft_velocity*R_W, vr = wr.shaft_velocity*R_W;
    px += 0.5f*(vl+vr)*cosf(pth)*0.001f;
    py += 0.5f*(vl+vr)*sinf(pth)*0.001f;
    pth += (vr-vl)/TRACK*0.001f;
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  for (int i = 0; i < 3; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
  // 电机初始化同案例一
  WiFi.mode(WIFI_STA); esp_now_init();
  esp_now_register_recv_cb(onRecv);
  esp_now_peer_info_t pi = {};
  memset(pi.peer_addr, 0xFF, 6); pi.channel = 1;
  esp_now_add_peer(&pi);
  for (int i = 0; i < 3; i++) peer[i] = {0,0,0,0,false,0,0,0};

  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskSense, "sens", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskPlan,  "plan", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,  "exec", 4096, NULL, 3, NULL, 1);
  Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }

关键点：队友斥力的死区（DEAD=0.28）是必须的。三角队形的标称间距约 1.05 m，但转弯时内侧两机会自然靠近到 0.5 m 左右——没有死区的话，斥力会在正常编队机动中持续把队伍推散，队形精度反而下降。

6、逃逸升级链 —— VFF → 沿墙绕行 → 队形重构
适用硬件：三台 ESP32 + 差速 BLDC + 前后左右超声 + ESP-NOW。

核心思路：VFF 逃逸只对“浅陷阱”有效（一个障碍挡在队形与目标之间）。遇到 U 形槽、窄通道这类深陷阱时，切向力会一直在原地打转。所以必须有升级链：VFF 逃逸 → 沿墙绕行（Wall Following，能保证走出任何有限障碍）→ 队形重构（三角改纵队过窄道）→ 恢复编队。每一级都有超时，每级都要能退回。

/*
 * 案例6：VFF -> WALL_FOLLOW -> RESHAPE 升级链 + 超时回退
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

BLDCMotor wl(7), wr(7);
// 初始化同案例一（略）

const float R_W = 0.065f, TRACK = 0.34f;
const uint8_t MY_ID = 1;

enum Mode { M_VFF, M_ESCAPE, M_WALL, M_RESHAPE, M_REFORM };
volatile Mode mode = M_VFF;
unsigned long modeT0 = 0;
float modeDist0 = 0;

// ---- 队形（两套：三角 / 纵队）----
const float TRI[3][2]   = {{0.0f,0.0f}, {-0.85f,-0.62f}, {-0.85f,0.62f}};
const float COL[3][2]   = {{0.0f,0.0f}, {-0.70f, 0.00f}, {-1.40f,0.00f}};
bool  useColumn = false;

struct Msg { uint8_t id; float x,y,th; bool col; unsigned long t; };
Msg leader = {0,0,0,false,0};
bool alive = false;

void onRecv(const uint8_t*, const uint8_t* d, int len) {
  if (len != sizeof(Msg)) return;
  Msg m; memcpy(&m, d, sizeof(m));
  if (m.id == 0) { leader = m; alive = (millis()-m.t < 500); useColumn = m.col; }
}

float px=0, py=0, pth=0, vCur=0, wCur=0;

const int TRIG[4] = {6, 8, 10, 12}, ECHO[4] = {7, 9, 11, 13};
const float OBS_ANG[4] = {0.0f, -0.90f, 0.90f, 3.1416f};   // 前 左 右 后
float dObs[4] = {9,9,9,9};

float readUS(int i) {
  digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
  digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
  long us = pulseIn(ECHO[i], HIGH, 15000);
  if (us <= 0) return 9.0f;
  float r = us * 0.0343f / 2.0f / 100.0f;
  return (r > 0.05f && r < 2.5f) ? r : 9.0f;
}

void enterMode(Mode m, float distNow) {
  mode = m; modeT0 = millis(); modeDist0 = distNow;
  Serial.printf("[MODE] -> %d\n", (int)m);
}

void setup() {
  for (int i = 0; i < 4; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
  // 电机初始化同案例4
  WiFi.mode(WIFI_STA); esp_now_init();
  esp_now_register_recv_cb(onRecv);
  esp_now_peer_info_t pi = {};
  memset(pi.peer_addr, 0xFF, 6); pi.channel = 1;
  esp_now_add_peer(&pi);
  enterMode(M_VFF, 0);
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.2f) dt = 0.02f;

  for (int i = 0; i < 4; i++) dObs[i] = readUS(i);
  float tIn = (millis() - modeT0)/1000.0f;

  // ---- 当前队形目标位 ----
  const float (*OFF)[2] = useColumn ? COL : TRI;
  float gx, gy;
  if (alive) {
    gx = leader.x + OFF[MY_ID][0]*cosf(leader.th) - OFF[MY_ID][1]*sinf(leader.th);
    gy = leader.y + OFF[MY_ID][0]*sinf(leader.th) + OFF[MY_ID][1]*cosf(leader.th);
  } else { gx = px; gy = py; }

  float ex = gx - px, ey = gy - py;
  float dAtt = hypotf(ex, ey);

  float vCmd = 0, wCmd = 0;

  switch (mode) {
    // ---------- 常规 VFF ----------
    case M_VFF: {
      float Fx = 0, Fy = 0;
      if (dAtt > 1e-3f) { Fx += 1.2f*min(dAtt,2.0f)*ex/dAtt;
                          Fy += 1.2f*min(dAtt,2.0f)*ey/dAtt; }
      for (int i = 0; i < 4; i++) {
        if (dObs[i] >= 0.70f) continue;
        float a = pth + OBS_ANG[i];
        float mag = 0.45f*(1.0f/max(dObs[i],0.12f) - 1.0f/0.70f);
        Fx -= mag*cosf(a); Fy -= mag*sinf(a);
      }
      float fm = hypotf(Fx, Fy);
      float fAtt = 1.2f*min(dAtt,2.0f);

      if (dAtt > 0.45f && fm < 0.22f*fAtt) {
        static unsigned long mT0 = 0;
        if (mT0 == 0) mT0 = now;
        else if (now - mT0 > 600) { enterMode(M_ESCAPE, dAtt); mT0 = 0; }
      }

      float hdg = atan2f(Fy, Fx);
      float err = hdg - pth;
      while (err >  PI) err -= 2*PI;
      while (err < -PI) err += 2*PI;
      vCmd = constrain(min(fm,1.2f)*cosf(err), -0.15f, 0.85f);
      if (fabsf(err) > 1.2f) vCmd = 0;
      wCmd = constrain(2.0f*err, -0.9f, 0.9f);
      break;
    }

    // ---------- 切向逃逸 ----------
    case M_ESCAPE: {
      float side = (dObs[1] > dObs[2]) ? 1.0f : -1.0f;
      float tx = -ey/max(dAtt,1e-3f), ty = ex/max(dAtt,1e-3f);
      float Fx = 0.9f*side*tx, Fy = 0.9f*side*ty;
      if (dAtt > 1e-3f) { Fx += 0.6f*ex/dAtt; Fy += 0.6f*ey/dAtt; }

      float hdg = atan2f(Fy, Fx);
      float err = hdg - pth;
      while (err >  PI) err -= 2*PI;
      while (err < -PI) err += 2*PI;
      vCmd = constrain(0.60f*cosf(err), 0.0f, 0.55f);
      wCmd = constrain(2.2f*err, -0.9f, 0.9f);

      // 有效 -> 回 VFF；3.5s 无效 -> 升级沿墙
      if (dAtt < modeDist0 - 0.30f) enterMode(M_VFF, dAtt);
      else if (tIn > 3.5f)         enterMode(M_WALL, dAtt);
      break;
    }

    // ---------- 沿墙绕行 ----------
    case M_WALL: {
      // 保持与左侧墙 0.40m，前方让行；无左侧墙则改右侧
      bool leftWall = (dObs[1] < 0.80f);
      float dSideRef = 0.40f;
      float dSide = leftWall ? dObs[1] : dObs[2];
      float sgn  = leftWall ? 1.0f : -1.0f;

      float eSide = dSide - dSideRef;
      wCmd = constrain(-sgn * 1.8f * eSide , -0.8f, 0.8f);
      vCmd = 0.35f;
      if (dObs[0] < 0.45f) { vCmd = 0.10f; wCmd = sgn*0.6f; }   // 前方贴脸：转
      if (dObs[0] < 0.28f) { vCmd = 0.0f; }                      // 太近：停

      // 绕出条件：目标在正前方且距离明显缩短
      if (dAtt < modeDist0 - 0.60f && tIn > 1.0f) enterMode(M_REFORM, dAtt);
      else if (tIn > 12.0f)                        enterMode(M_REFORM, dAtt); // 超时也放弃
      break;
    }

    // ---------- 队形重构（请求换纵队）----------
    case M_RESHAPE: {
      vCmd = 0; wCmd = 0;
      if (tIn > 0.5f) enterMode(M_REFORM, dAtt);
      break;
    }

    // ---------- 恢复编队 ----------
    case M_REFORM: {
      if (dAtt > 0.35f) {
        float Fx = 1.2f*ex/dAtt, Fy = 1.2f*ey/dAtt;
        float hdg = atan2f(Fy, Fx);
        float err = hdg - pth;
        while (err >  PI) err -= 2*PI;
        while (err < -PI) err += 2*PI;
        vCmd = constrain(0.8f*cosf(err), 0.0f, 0.70f);
        wCmd = constrain(2.0f*err, -0.9f, 0.9f);
      } else {
        enterMode(M_VFF, dAtt);      // 归位完成，回到常规
      }
      break;
    }
  }

  // ---- 执行 + 里程 ----
  float vL = vCmd - wCmd*TRACK*0.5f, vR = vCmd + wCmd*TRACK*0.5f;
  wl.loopFOC(); wr.loopFOC();
  wl.move(vL/R_W); wr.move(vR/R_W);

  float vl = wl.shaft_velocity*R_W, vr = wr.shaft_velocity*R_W;
  vCur = 0.5f*(vl+vr); wCur = (vr-vl)/TRACK;
  px += vCur*cosf(pth)*dt; py += vCur*sinf(pth)*dt; pth += wCur*dt;

  static unsigned long lb = 0;
  if (millis() - lb > 100) {
    lb = millis();
    Msg m = {MY_ID, px, py, pth, false, millis()};
    esp_now_send(NULL, (uint8_t*)&m, sizeof(m));
  }
  Serial.print("mode="); Serial.print((int)mode);
  Serial.print(" dAtt="); Serial.print(dAtt, 2);
  Serial.print(" t="); Serial.println(tIn, 1);
  delay(20);
}

关键点：M_WALL 的超时不是“再试一次”，而是“放弃并回编队”。沿墙绕行理论上一定能走出有限障碍，但代价可能是绕行十几米、远超任务允许的时间。工业场景里能放弃比能绕出来更重要——绕出去三分钟，不如退回来重新规划。

要点解读
一、VFF 的局部极小是方法本身的缺陷，不能靠调参消除
势场法的数学结构决定了：只要引力和斥力在某个点等值反向，合力就为零，机器人就停在那里。调大引力会让机器人在障碍前“硬顶”（撞上去），调小引力的会让它更容易被困，调大斥力会震荡，调小斥力会碰撞——没有任何一组参数能同时解决所有地形。这一点必须在架构上承认：VFF 负责“大多数情况下的轻量反应”，另有一条独立通道负责“从陷阱里出来”。把两者混在一起调参，是势场法实践中最常见的失败路径。

二、极小检测要看“力被抵消”，不是看“车不动”
最常见的错误判据是“速度连续若干帧接近零就判定被困”。但机器人可能正在缓慢蠕动（速度 5 cm/s）却已经完全陷在陷阱里；反过来，正常等队友、正常转弯时速度也会短暂归零。正确的判据是合力模长相对于引力模长的比值（案例一 fm < 0.22*fAttMag）：这个比值只在你真的被挡住时才变小，与速度、与是否在等队友都无关。而且它比速度更早出现——在机器人还没完全停住时就已经能报警。

三、逃逸方向要选“空间更大的一侧”，并且必须全队同步
切向力的方向如果固定（永远逆时针），在一半的地形里会绕远路甚至绕不出去。案例一、二都用“比较左右两侧测距，往空的一侧绕”。但更关键的是全队同步：三机各自独立判极小、各自选方向，极大概率出现一台向左、一台向右，队形瞬间被撕开，弹簧（编队力）会把它们往中间拽，越拽越卡。案例二用“任一机发起 + 轮次戳（stamp）+ 广播同步”解决这个问题——逃逸是团队事件，不是个体事件。

四、把队友放进同一力场，编队与避障才真正统一
如果障碍用斥力、队友用“位置伺服”，两套逻辑会在交叉场景里打架：机器人为了跟住队友而撞向障碍，或者为了避开障碍而顶开队友。把它们放进同一力场（案例二）之后，行为是一致的加权折中——离障碍近时避障权重自然占优，离障碍远时编队权重自然占优，中间过渡是连续的，没有模式跳变。但必须给队友斥力加死区（DEAD=0.28），否则正常转弯时内侧两机靠近就会被推开，队形精度反而崩掉。

五、逃逸必须有退出条件与升级链，队形在逃逸期优先级自动降低
“逃出去”之后如果不定义何时结束，机器人会一直带着切向力跑，永远回不到队形。案例一的退出条件是“距离目标缩短 0.3 m 或超时 3 s”，案例三更进一步给出升级链：VFF → 切向逃逸（3.5 s）→ 沿墙（12 s）→ 重构/放弃。这条链的每一级都比上一级更“重”、更慢、更耗资源，所以只在必要时升级；每一级也都有超时，避免无限停留。核心原则是：逃逸期间，保持队形的重要性自动降级——先活着出来，再谈站好队。

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