“Arduino BLDC 之机器人虚拟弹簧弹性队形保持 + 避障收缩”可以理解为：以 BLDC 差速/全向底盘为执行单元，把机器人之间的期望相对位置建模为虚拟弹簧-阻尼网络；正常行进时通过虚拟弹簧保持队形，遇到障碍物或狭窄通道时允许队形弹性收缩、绕行后再恢复；底层由 BLDC FOC 速度/位置闭环柔顺执行，上层通过局部通信和传感器反馈维持队形连贯性与安全性。
一、主要特点

1. 虚拟弹簧-阻尼网络提供物理可解释的队形约束
   队形保持不再依赖刚性轨迹绑定，而是把相邻机器人之间的期望距离、角度或相对位置抽象为虚拟弹簧。弹簧项负责把机器人拉回期望构型，阻尼项负责抑制相对速度振荡，使队形变化更平稳。
2. 队形具有弹性，而非刚性锁定
   传统虚拟结构法在障碍环境中容易“卡住”，因为队形形状被固定。虚拟弹簧模型允许机器人间距在一定范围内变化，遇到障碍时队形可以压缩、拉伸、旋转或局部重组，障碍清除后再恢复。
3. 避障收缩是队形保持与局部避障的耦合结果
   当机器人接近障碍物或通道变窄时，障碍物斥力场或环境约束会压缩虚拟弹簧，使队形整体收缩。收缩程度通常由最小安全间距、通信半径、传感器范围和机器人动力学能力共同决定。
4. 分布式或半分布式架构更适合扩展
   每台机器人只需获取领航者或邻居的位置、速度、航向信息，即可计算自身虚拟力并生成速度指令。这样即使领航者失效或通信拓扑变化，队形仍能部分维持。
5. BLDC FOC 提供柔顺的速度执行能力
   虚拟弹簧输出的是连续变化的力/速度指令，若电机执行层存在转矩脉动或响应滞后，队形容易出现抖动和相位差。BLDC 配合 FOC 电流环，可在频繁加减速、差速转向和小角度修正时保持较低执行误差。
6. 通信延迟和感知噪声会直接影响弹簧稳定性
   虚拟弹簧本质依赖相对位置和相对速度。如果位置更新延迟较大，弹簧力会基于过时状态计算，导致队形振荡、过冲甚至发散。因此需要固定周期通信、状态预测或轻量滤波。
   二、应用场景
7. 仓储物流 AGV 车队
   多台 AGV 以列车式或并排队形搬运大件货物，遇到货架、托盘或临时障碍时弹性收缩，通过后恢复队形。
8. 厂区/园区巡检集群
   多台巡检机器人沿走廊、管廊或道路行进，保持固定传感覆盖宽度；遇到行人、车辆或狭窄门洞时自动压缩队形。
9. 野外勘探与搜救编队
   在废墟、林地、沙地等非结构化环境中，编队需要根据地形宽度动态收缩，避免个体掉队或相互碰撞。
10. 农业巡检与环境监测
    多台机器人沿作物行间移动，保持覆盖间距；遇到田埂、水坑或设备时局部绕行并重新聚合。
11. 编队表演与教学演示
    适合展示虚拟弹簧、势场法、领航-跟随和分布式一致性等概念，BLDC 底盘可提供较好的低速平滑性和转向响应。
12. 协同搬运与围捕任务
    需要维持相对几何关系时，虚拟弹簧能保持机器人之间的协同间距；遇到障碍时又能避免刚性队形导致任务中断。
    三、需要注意的事项
13. 弹簧刚度与阻尼必须成对设计
    刚度过大会使队形僵硬、容易振荡；刚度过小会导致队形松散、恢复慢。阻尼不足会产生来回拉伸，阻尼过大会使队形响应迟钝。应结合机器人质量、最大速度和通信周期联合整定。
14. 避障收缩要设置最小安全距离
    队形可以压缩，但不能无限制靠近。必须保留机器人本体半径、传感器盲区、制动距离和控制延迟带来的安全裕度，防止收缩过程中发生互碰。
15. 通信拓扑变化需要平滑处理
    当机器人因障碍绕行导致邻居关系改变时，虚拟弹簧连接关系可能切换。切换瞬间应限制力突变，避免队形突然拉扯。
16. BLDC 速度指令必须限幅和斜坡化
    虚拟弹簧在队形误差较大时会输出较大修正量，若直接下发给电机，容易造成急加速、打滑或电流尖峰。应对线速度、角速度和加速度设置上限，并加入斜率限制。
17. Arduino 算力边界要提前评估
    ATmega328P 适合做单机器人 BLDC FOC 执行、简单虚拟力计算和局部通信节点；若同时运行多邻居势场、IMU 融合、里程计、通信调度和队形恢复逻辑，建议迁移到 ESP32、Teensy 4.1、STM32 或采用“上位机规划 + 下位机执行”的分层架构。
18. 相对定位误差会累积
    仅靠编码器和 IMU 进行多机相对位姿估计，误差会随时间增大。可结合 UWB、视觉标签、共享航点或外部定位系统校正，否则队形会逐渐漂移。
19. 需处理局部极小值和死锁
    在 U 型障碍、狭窄门口或对称斥力环境下，机器人可能陷入振荡或停滞。应加入随机扰动、临时目标点、全局重规划或队形拓扑切换机制。
20. 队形恢复不能简单跳回原位置
    绕过障碍后，应基于当前位姿和期望构型重新计算目标相对位置，采用渐进恢复策略，避免所有机器人同时大幅修正导致二次碰撞。
21. 电源、EMC 和制动安全必须单独考虑
    多机协同时频繁加减速会引起母线波动，可能干扰 IMU、通信模块和编码器。控制电源应独立稳压，动力线与控制线分开布线，并设置欠压保护、看门狗和急停逻辑。
22. 仿真验证应先于实机编队测试
    虚拟弹簧参数、通信延迟、避障收缩阈值和队形恢复策略应先在仿真中验证稳定性，再逐步上机。实机测试应从双机开始，确认不会互碰后再扩展为多机编队。
    整体来看，这套系统的核心链路是：局部感知与通信获取邻居/障碍状态 → 虚拟弹簧-阻尼网络计算队形保持力 → 障碍斥力触发弹性收缩 → 生成各机器人速度/角速度指令 → BLDC FOC 柔顺执行 → 里程计/IMU 反馈修正 → 障碍通过后平滑恢复队形。做原型时，建议按“单机 BLDC 速度闭环 → 双机虚拟弹簧保持 → 加入障碍斥力 → 实现弹性收缩 → 引入通信延迟补偿 → 多机扩展与队形恢复”逐步推进。

1、领航-跟随虚拟弹簧 + 障碍物斥力压缩队形
适用场景：三台机器人（1领航+2跟随）在开阔区域保持菱形编队，遇到障碍物时跟随者因斥力偏离标准位置（弹簧被“压缩”），障碍消失后弹簧恢复力将跟随者拉回原始队形位置。核心逻辑是“弹簧力 + 障碍斥力”的合力驱动。

#include <SimpleFOC.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);

// 虚拟弹簧参数
const float SPRING_K = 1.2;        // 弹簧劲度系数
const float SPRING_DAMP = 0.6;     // 阻尼系数
const float FREE_LENGTH = 1.0;     // 弹簧自由长度 (m)

// 领航者位置（通过UWB/通信获取）
float leaderX = 0, leaderY = 0;
// 本机位置（简化：里程计推算）
float selfX = -1.0, selfY = 0.5;   // 初始在领航者左后方

// 障碍物斥力参数
const float OBSTACLE_K = 2.0;
const float OBSTACLE_RANGE = 0.8;  // 斥力作用距离 (m)
float obstacleDist = 999;

#define TRIG 22
#define ECHO 23

// 目标偏移（标准队形位置：左后方）
const float OFFSET_X = -1.0;
const float OFFSET_Y = 0.5;

float readDist() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long dur = pulseIn(ECHO, HIGH, 20000);
  return (dur == 0) ? 999 : dur * 0.0343 / 2.0;
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  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 loop() {
  motorL.loopFOC(); motorR.loopFOC();

  obstacleDist = readDist() / 100.0;  // 转米

  // ===== 虚拟弹簧力：拉向标准队形位置 =====
  float targetX = leaderX + OFFSET_X;
  float targetY = leaderY + OFFSET_Y;

  float dxSpring = targetX - selfX;
  float dySpring = targetY - selfY;
  float springDist = sqrt(dxSpring*dxSpring + dySpring*dySpring);

  // 弹簧力（胡克定律简化）：力 = K * 形变量，方向指向目标
  float springForceX = 0, springForceY = 0;
  if (springDist > 0.01) {
    float extension = springDist - FREE_LENGTH;
    // 形变量为负（距离小于自由长度）时产生斥力，为正时产生拉力
    float forceMag = SPRING_K * extension;
    springForceX = forceMag * dxSpring / springDist;
    springForceY = forceMag * dySpring / springDist;
  }

  // 阻尼项：抑制振荡
  float velX = 0;  // 简化：本机速度由里程计差分获取
  float velY = 0;
  springForceX -= SPRING_DAMP * velX;
  springForceY -= SPRING_DAMP * velY;

  // ===== 障碍物斥力：压缩队形 =====
  float repulseX = 0, repulseY = 0;
  if (obstacleDist > 0 && obstacleDist < OBSTACLE_RANGE) {
    // 斥力大小与距离成反比
    float repulseMag = OBSTACLE_K * (1.0 / obstacleDist - 1.0 / OBSTACLE_RANGE);
    // 方向：远离障碍物（简化：假设障碍物在正前方）
    repulseX = -repulseMag;
    repulseY = 0;
  }

  // ===== 合力 → 速度指令 =====
  float totalForceX = springForceX + repulseX;
  float totalForceY = springForceY + repulseY;

  float speedCmd = constrain(sqrt(totalForceX*totalForceX + totalForceY*totalForceY), 0, 0.8);
  float headingCmd = atan2(totalForceY, totalForceX);

  // 差速执行（简化：朝向合力方向）
  float turn = constrain(headingCmd * 0.5, -0.8, 0.8);
  motorL.move((speedCmd - turn * 0.15) / 0.05);
  motorR.move((speedCmd + turn * 0.15) / 0.05);

  // 更新本机位置（模拟）
  selfX += totalForceX * 0.02;
  selfY += totalForceY * 0.02;

  delay(20);
}

解读：该案例直接采用了搜索结果中“虚拟弹簧-阻尼”的弹性编队逻辑。springForce 根据本机与标准队形位置的偏差计算拉力，当障碍物靠近时 repulseX 产生反向斥力，两者叠加后跟随者向领航者靠拢（队形“压缩”）。障碍消失后 repulseX = 0，弹簧力将跟随者拉回 OFFSET_X/OFFSET_Y 定义的标准位置（队形“恢复”）。BLDC 速度环确保合力驱动的速度指令被平滑执行，避免队形调整时的顿挫。

2、多方向超声波 + 自适应旋转方向选择（避障收缩的决策层）
适用场景：编队前方遇到障碍物时，需要决定向左还是向右收缩/旋转。传统固定规则（如“总是右转”）在对称障碍前会失效。通过比较左右两侧的开阔度，自适应选择斥力较小的方向，驱动编队整体旋转避让。

#include <SimpleFOC.h>
#include <NewPing.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);

// 三方向超声波
#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, 200);
NewPing sonarR(TRIG_R, ECHO_R, 200);

// 编队参数（环绕编队）
float selfAngle = 120;          // 从机环绕角度（度）
const float RADIUS = 1.0;
const float SPRING_K = 1.5;
const float AVOID_DIST = 0.6;   // 触发避障的距离

// 领航者位姿（UWB获取）
float leaderX = 0, leaderY = 0, leaderTheta = 0;
float selfX = 0, selfY = 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 loop() {
  motorL.loopFOC(); motorR.loopFOC();

  float distF = sonarF.ping_cm() / 100.0;
  float distL = sonarL.ping_cm() / 100.0;
  float distR = sonarR.ping_cm() / 100.0;

  // ===== 自适应旋转方向选择 =====
  bool avoiding = false;

  if (distF > 0 && distF < AVOID_DIST) {
    avoiding = true;
    if (distL > distR) {
      // 左侧更开阔：顺时针旋转（左轮反转，右轮正转）
      motorL.move(-0.8);
      motorR.move(0.8);
      selfAngle += 3;  // 环绕角度偏移，队形旋转
    } else if (distR > distL) {
      // 右侧更开阔：逆时针旋转
      motorL.move(0.8);
      motorR.move(-0.8);
      selfAngle -= 3;
    } else {
      // 对称障碍：随机选择方向打破死锁
      if (random(100) < 50) {
        motorL.move(-0.6);
        motorR.move(0.6);
        selfAngle += 2;
      } else {
        motorL.move(0.6);
        motorR.move(-0.6);
        selfAngle -= 2;
      }
    }
  }

  // 侧方障碍：单向避让
  if (!avoiding && distL > 0 && distL < 0.4) {
    motorL.move(0.6);
    motorR.move(-0.6);
    avoiding = true;
  } else if (!avoiding && distR > 0 && distR < 0.4) {
    motorL.move(-0.6);
    motorR.move(0.6);
    avoiding = true;
  }

  // ===== 无障碍：弹簧力回归环绕位置 =====
  if (!avoiding) {
    float rad = selfAngle * PI / 180;
    float targetX = leaderX + RADIUS * cos(rad + leaderTheta);
    float targetY = leaderY + RADIUS * sin(rad + leaderTheta);

    float dx = targetX - selfX;
    float dy = targetY - selfY;
    float dist = sqrt(dx*dx + dy*dy);
    float speed = constrain(SPRING_K * dist, 0, 0.8);
    float heading = atan2(dy, dx);
    float turn = constrain(heading * 0.4, -0.6, 0.6);

    motorL.move((speed - turn * 0.15) / 0.05);
    motorR.move((speed + turn * 0.15) / 0.05);
  }

  delay(20);
}

解读：该案例借鉴了搜索结果中“多方向超声波评估开阔度、自适应选择旋转方向”的逻辑。distL > distR 时选择顺时针旋转，反之逆时针，对称时随机打破死锁。selfAngle 的偏移同时改变了环绕编队的目标位置，使队形整体旋转而非单个机器人乱跑。避障结束后，弹簧力将机器人拉回 selfAngle 对应的新环绕位置——如果避障过程中 selfAngle 被持续调整，队形已经完成了“收缩/旋转”。

3、编队收缩状态机 + 平滑插值（避障前后队形参数过渡）
适用场景：编队从“宽间距菱形”收缩为“紧纵队”通过狭窄通道，通过后再展开。直接切换目标偏移会导致速度指令阶跃，BLDC 急加速产生机械冲击。用状态机管理“展开/收缩/恢复”三阶段，并对目标偏移做低通滤波平滑。

#include <SimpleFOC.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);

// 队形模式
enum FormationMode { WIDE, NARROW, TRANSITION };
FormationMode mode = WIDE;
unsigned long modeTimer = 0;

// 目标偏移（相对于领航者）
float targetOffsetX = -1.0, targetOffsetY = 0.5;   // 宽菱形
float narrowOffsetX = -0.5, narrowOffsetY = 0.2;   // 紧纵队

// 平滑后的实际偏移
float smoothOffsetX = -1.0, smoothOffsetY = 0.5;
const float SMOOTH_ALPHA = 0.08;  // 平滑系数

// 领航者位置
float leaderX = 0, leaderY = 0;
float selfX = 0, selfY = 0;

#define TRIG 22
#define ECHO 23

float readDist() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long dur = pulseIn(ECHO, HIGH, 20000);
  return (dur == 0) ? 999 : dur * 0.0343 / 2.0;
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  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 loop() {
  motorL.loopFOC(); motorR.loopFOC();

  float dist = readDist() / 100.0;

  // ===== 状态机：根据障碍距离切换队形模式 =====
  if (mode == WIDE && dist > 0 && dist < 0.8) {
    mode = TRANSITION;
    targetOffsetX = narrowOffsetX;
    targetOffsetY = narrowOffsetY;
    modeTimer = millis();
    Serial.println("→ 收缩队形");
  } else if (mode == TRANSITION && millis() - modeTimer > 1500 && (dist > 1.2 || dist < 0.01)) {
    mode = NARROW;
  } else if (mode == NARROW && dist > 1.5) {
    mode = TRANSITION;
    targetOffsetX = -1.0;
    targetOffsetY = 0.5;
    modeTimer = millis();
    Serial.println("→ 恢复队形");
  } else if (mode == TRANSITION && targetOffsetX < -0.9 && millis() - modeTimer > 1500) {
    mode = WIDE;
  }

  // ===== 平滑插值：目标偏移缓慢逼近 =====
  smoothOffsetX += (targetOffsetX - smoothOffsetX) * SMOOTH_ALPHA;
  smoothOffsetY += (targetOffsetY - smoothOffsetY) * SMOOTH_ALPHA;

  // ===== 虚拟弹簧力 =====
  float targetX = leaderX + smoothOffsetX;
  float targetY = leaderY + smoothOffsetY;

  float dx = targetX - selfX;
  float dy = targetY - selfY;
  float distToTarget = sqrt(dx*dx + dy*dy);

  float speed = constrain(1.2 * distToTarget, 0, 0.8);
  float heading = atan2(dy, dx);
  float turn = constrain(heading * 0.5, -0.8, 0.8);

  motorL.move((speed - turn * 0.15) / 0.05);
  motorR.move((speed + turn * 0.15) / 0.05);

  selfX += dx * 0.1;
  selfY += dy * 0.1;

  delay(20);
}

解读：该案例将“避障收缩”从瞬时的力场响应升级为带平滑过渡的状态机。TRANSITION 状态下，smoothOffsetX/Y 以 SMOOTH_ALPHA = 0.08 的速率逼近 targetOffset，使跟随者的目标位置在约 1-2 秒内从宽间距平滑压缩到紧间距。搜索结果中编队切换的讨论同样强调“平滑插值避免硬切换冲击”。BLDC 速度环在此承担执行层角色：平滑的目标偏移变化转化为连续的速度指令，轮胎不会因目标突变而打滑或产生电流冲击。

要点解读

1. 虚拟弹簧的“自由长度”决定了编队的刚性与收缩幅度
   案例一中 FREE_LENGTH = 1.0 是弹簧的自然长度：当实际距离大于此值时产生拉力，小于时产生斥力（防止机器人挤在一起）。避障收缩的本质是障碍物斥力将跟随者推离标准位置，使弹簧被“压缩”到小于自由长度的状态。如果自由长度设置过小，编队在正常行驶时就会相互排斥；设置过大，则收缩后难以恢复。搜索结果中专利文献明确将虚拟弹簧的自由长度作为编队参数。

2. 避障收缩的触发依据应是“障碍物距离 + 开阔度评估”，而非单一距离阈值
   案例二的核心创新是 distL > distR 时选择顺时针旋转。如果只根据前方距离 < 0.6 就固定向某一侧避让，在障碍物偏左或偏右时会做出错误决策。搜索结果中“旋转力场方向自适应”的描述与此一致：系统实时评估左右两侧空间的开阔度，选择斥力最小的方向。对于虚拟弹簧编队，开阔度评估的结果应同时影响旋转方向和 selfAngle 的调整量，使队形整体旋转而非单机乱跑。

3. 队形参数（目标偏移）的平滑过渡比力场本身的平滑更重要
   案例三的 SMOOTH_ALPHA 低通滤波作用于 targetOffset，而非速度指令。这是因为：如果只对速度指令滤波，目标位置的阶跃仍然存在，机器人最终会以滤波后的速度“冲”向新目标；对目标偏移本身滤波，则目标位置是连续变化的，速度指令自然连续。搜索结果中编队切换的代码片段使用了 smoothFormationIdx += (target - current) * SMOOTH_FACTOR 的一阶滤波结构，与本案例思路一致。

4. BLDC FOC 的速度环是弹性编队“柔性”体感的物理保障
   虚拟弹簧计算出的合力是连续变化的模拟量，但如果底层使用开环 ESC 或有刷电机 PWM，低速段的死区和齿槽效应会将连续的速度指令“台阶化”，编队调整时出现顿挫。FOC 速度环的毫秒级响应使 motorL.move(continuous_speed) 被精确跟踪，弹簧力的渐变才能被复现为平滑的队形收缩/恢复。搜索结果中明确指出 BLDC 配合 FOC 的“低脉动驱动”是编队运动中各机器人速度高度同步的前提。

5. Arduino 算力边界要求“势场计算”与“FOC执行”分层
   案例中的 delay(20) 对应约 50Hz 的势场计算与决策频率。虚拟弹簧、障碍斥力、开阔度比较涉及平方根、反正切和条件判断，在 Arduino 上以 50Hz 运行已接近负载。合理的架构是：FOC 环尽可能高频运行（去掉 delay 或使用定时器），势场与队形决策降频到 30-50Hz。搜索结果中同样指出“经典 8 位 Arduino 难以胜任三机器人系统的势场实时计算”，建议使用 ESP32 或 STM32，并采用分层架构（势场计算与 BLDC 高频控制分离）。

4、虚拟弹簧跟随 + 障碍压力驱动的队形收缩（双机最小闭环）
适用硬件：ESP32 + SimpleFOC 差速底盘 + 编码器 + 前向/侧向超声 + ESP-NOW（机间通信）。

核心思路：队形不是硬性位置伺服，而是一根"虚拟弹簧"：期望位与当前位置的偏差产生弹性力，速度项提供阻尼。障碍靠近时不改弹簧刚度，而是缩小期望间距（收缩因子 s），弹簧自然把队伍"收拢"；障碍离开后 s 缓慢恢复，队伍重新展开。

/*
 * 案例4：虚拟弹簧 + 距离驱动收缩
 * 期望位 = Leader + 旋转(偏置向量 × s)；s 由障碍压力 p 连续生成
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onL(){ encL.handleA(); } void onR(){ encR.handleA(); }

const float R_W = 0.065f, TRACK = 0.34f;

// ---- 虚拟弹簧参数 ----
const float K_SPR = 1.9f;      // 弹性增益 (1/s)
const float B_DMP = 2.7f;      // 阻尼增益
const float D0    = 0.90f;     // 标称间距 m
const float S_MIN = 0.55f;     // 收缩下限（再小就碰撞）
const float S_MAX = 1.00f;

// ---- 收缩因子（快收慢放）----
float s = 1.0f;
const float SHRINK_RATE = 1.6f;   // /s
const float EXPAND_RATE = 0.35f;  // /s

// ---- 障碍感知 ----
const int TRIG_F = 6, ECHO_F = 7;
float dObs = 9.0f;
const float R_SAFE = 0.75f;       // 障碍影响半径

float readUS(int t, int e) {
  digitalWrite(t, LOW); delayMicroseconds(2);
  digitalWrite(t, HIGH); delayMicroseconds(10); digitalWrite(t, LOW);
  long d = pulseIn(e, HIGH, 20000);
  return (d > 0) ? d * 0.034f / 2.0f / 100.0f : 9.0f;
}

// ---- 机间通信：Leader 广播位姿与速度 ----
struct Msg { 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* data, int len) {
  if (len != sizeof(Msg)) return;
  memcpy(&leader, data, sizeof(Msg));
  hasLeader = (millis() - leader.t < 400);
}

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

void setup() {
  encL.init(); encL.enableInterrupts(onL);
  encR.init(); encR.enableInterrupts(onR);
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.35f; wheelL.PID_velocity.I = 4.0f;
  wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 4.0f;
  wheelL.voltage_limit = wheelR.voltage_limit = 12.0f;
  wheelL.current_limit = wheelR.current_limit = 4.0f;
  wheelL.init(); wheelR.init();
  drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
  wheelL.initFOC(); wheelR.initFOC();

  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, 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);
  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.15f) dt = 0.02f;

  // ---- 1. 感知：障碍距离 ----
  dObs = readUS(TRIG_F, ECHO_F);

  // ---- 2. 收缩因子：障碍压力 -> s（快收慢放 + 下限）----
  float p = constrain((R_SAFE - dObs) / R_SAFE, 0.0f, 1.0f);  // 压力 0..1
  float sTgt = 1.0f - (1.0f - S_MIN) * p;                     // 压力满 -> S_MIN
  sTgt = constrain(sTgt, S_MIN, S_MAX);
  float rate = (sTgt < s) ? SHRINK_RATE : EXPAND_RATE;
  s += constrain(sTgt - s, -rate*dt, rate*dt);

  // ---- 3. 期望位：Leader 位姿 + 偏置 × s ----
  float offX = -D0 * s;          // 跟在正后方
  float offY = 0.0f;
  float tx = leader.x + offX*cosf(leader.th) - offY*sinf(leader.th);
  float ty = leader.y + offX*sinf(leader.th) + offY*cosf(leader.th);

  // ---- 4. 虚拟弹簧：弹性力 -> 期望速度 ----
  float ex = tx - px, ey = ty - py;
  // 世界系力 -> 机体系
  float fwd =  ex*cosf(pth) + ey*sinf(pth);
  float lat = -ex*sinf(pth) + ey*cosf(pth);

  float vCmd = K_SPR*fwd - B_DMP*vCur;          // 纵向弹簧
  vCmd = constrain(vCmd, -0.25f, 0.85f);
  // 横向偏差转为航向修正（差速车不能横移）
  float wCmd = constrain(1.6f*lat, -0.9f, 0.9f) - 0.4f*wCur;

  // ---- 5. 失联降级：停在原地等 Leader ----
  if (!hasLeader) { vCmd = 0; wCmd = 0; }

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

  float vl = wheelL.shaft_velocity*R_W, vr = wheelR.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("p="); Serial.print(p, 2);
  Serial.print(" s="); Serial.print(s, 2);
  Serial.print(" v="); Serial.println(vCmd, 2);
  delay(20);
}

关键点：收缩只改期望间距，不改弹簧刚度。若靠增大刚度来"挤紧"队伍，一旦障碍消失刚度不会自动回退，队形会持续发僵；而改期望位是"目标层面的变化"，物理上等价于整条弹簧被压缩，恢复也自然。

5、FreeRTOS 三层 + 全队统一收缩因子（多机协同）
适用硬件：ESP32 双核 + 差速 BLDC + 超声 + ESP-NOW（N 台，本次以 4 台示意）。

核心思路：收缩必须是全队一致的单一标量，而不是每台各算各的。否则 A 收缩、B 不收缩，队形会被撕开。做法是：每台广播自己的最近障碍距离，全队取最小值算压力 p，由此得到统一的 s；每台用同一份 (center, s, heading) 计算自己的期望位，弹簧只负责跟随。

/*
 * 案例5：感知(10Hz) - 队形规划(20Hz) - 执行(1kHz)
 * 全队共享：center(虚拟队形中心)、s(收缩因子)、th(队形朝向)
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
SemaphoreHandle_t mux;

const float R_W = 0.065f, TRACK = 0.34f;
const float K_SPR = 2.0f, B_DMP = 2.8f;
const float S_MIN = 0.55f;

// ---- 队形定义：4 机菱形，各机相对中心的偏置（标称间距 1.0m）----
const float OFF[4][2] = {{-0.9f, 0.0f}, {0.0f, 0.8f}, {0.9f, 0.0f}, {0.0f,-0.8f}};
const uint8_t MY_ID = 1;

// ---- 广播内容 ----
struct Bcast {
  uint8_t id;
  float x, y, th;        // 位姿
  float dObs;            // 本机最近障碍
  float cx, cy, cth;     // 队形中心与朝向（由 Leader 填）
  float s;               // 收缩因子（由 Leader 统一发布）
  unsigned long t;
};
Bcast gPeers[4]; int gN = 0;
volatile float sLocal = 1.0f;
volatile float cx = 0, cy = 0, cth = 0;
volatile bool  haveCenter = false;

// ---- 共享量 ----
volatile float s_dObs = 9.0f;
volatile float s_v = 0, s_w = 0;
volatile float c_v = 0, c_w = 0;
volatile bool  c_stop = false;
float px = 0, py = 0, pth = 0;

void onRecv(const uint8_t*, const uint8_t* data, int len) {
  if (len != sizeof(Bcast)) return;
  Bcast m; memcpy(&m, data, sizeof(m));
  if (m.id == MY_ID) return;
  for (int i = 0; i < gN; i++) if (gPeers[i].id == m.id) { gPeers[i] = m; return; }
  if (gN < 4) gPeers[gN++] = m;

  // Leader 发布中心与收缩因子 -> 全队跟随
  if (m.s > 0.0f) sLocal = m.s;
  if (m.cx != 0 || m.cy != 0) { cx = m.cx; cy = m.cy; cth = m.cth; haveCenter = true; }
}

// ---- 任务1：感知 10Hz ----
void taskSense(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float d = readFrontUS();      // 本机前向最近障碍
    xSemaphoreTake(mux, portMAX_DELAY);
    s_dObs = d;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(100));
  }
}

// ---- 任务2：队形规划 20Hz ----
void taskForm(void*) {
  TickType_t last = xTaskGetTickCount();
  float s = 1.0f;
  for (;;) {
    // ---- 全队障碍：取所有已知机的最小值 ----
    float dMin = s_dObs;
    for (int i = 0; i < gN; i++) dMin = min(dMin, gPeers[i].dObs);

    // ---- 统一收缩因子（快收慢放）----
    const float R_SAFE = 0.85f;
    float p = constrain((R_SAFE - dMin) / R_SAFE, 0.0f, 1.0f);
    float sTgt = 1.0f - (1.0f - S_MIN) * p;
    float rate = (sTgt < s) ? 1.5f : 0.30f;
    s += constrain(sTgt - s, -rate*0.05f, rate*0.05f);
    sLocal = s;

    // ---- 期望位：中心 + 偏置 × s（旋转到队形朝向）----
    bool haveC = haveCenter;
    float ccx = cx, ccy = cy, ccth = cth;
    if (!haveC) { ccx = px; ccy = py; ccth = pth; }

    float ox = OFF[MY_ID][0] * s, oy = OFF[MY_ID][1] * s;
    float tx = ccx + ox*cosf(ccth) - oy*sinf(ccth);
    float ty = ccy + ox*sinf(ccth) + oy*cosf(ccth);

    // ---- 虚拟弹簧 -> 期望速度 ----
    float ex = tx - px, ey = ty - py;
    float fwd =  ex*cosf(pth) + ey*sinf(pth);
    float lat = -ex*sinf(pth) + ey*cosf(pth);

    float v = K_SPR*fwd - B_DMP*s_v;
    v = constrain(v, -0.25f, 0.90f);
    float w = constrain(1.7f*lat, -0.9f, 0.9f) - 0.35f*s_w;

    // ---- 安全兜底：收缩到极限且障碍极近 -> 停车待命 ----
    bool block = (s <= S_MIN + 0.02f) && (dMin < 0.35f);

    xSemaphoreTake(mux, portMAX_DELAY);
    c_v = block ? 0.0f : v;
    c_w = block ? 0.0f : w;
    c_stop = block;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
  }
}

// ---- 任务3：执行 1kHz ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float v, w;
    xSemaphoreTake(mux, portMAX_DELAY);
    v = c_v; w = c_w;
    xSemaphoreGive(mux);
    float vL = v - w*TRACK*0.5f, vR = v + w*TRACK*0.5f;
    wheelL.loopFOC(); wheelR.loopFOC();
    wheelL.move(vL/R_W); wheelR.move(vR/R_W);

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

void setup() {
  // 电机初始化同案例一
  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);
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskSense, "sens", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskForm,  "form", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,  "exec", 4096, NULL, 3, NULL, 1);
  Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }

关键点：dMin 取全队最小值，而不是本机值——这是"统一收缩"的物理依据。若各机自算，靠近障碍的一台收缩、其余不变，队形会变成"一头紧一头松"，弹簧反而被拉长、误差持续存在。

6、多边形队形 + 各顶点独立收缩 + 恢复滞回与解散兜底
适用硬件：ESP32 + 差速 BLDC + 侧向超声 ×2 + ESP-NOW（N=5 示意）。

核心思路：全队统一收缩在窄通道里过于保守（一侧有墙也要全体收缩）。更实用的做法是各顶点独立收缩：每个顶点只看自己那一侧的障碍，按方向生成各自的 sᵢ；再加两层保护——恢复滞回（慢展开）与解散兜底（收缩到极限仍不可行时，退化为单列或就地停靠）。

/*
 * 案例6：多边形队形，各顶点独立收缩 + 滞回 + 解散兜底
 * 顶点期望位 = center + R(th) · (r_i · s_i)
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

BLDCMotor wheelL(7), wheelR(7);
// 初始化同案例一（略）
const float R_W = 0.065f, TRACK = 0.34f;
const float K_SPR = 1.9f, B_DMP = 2.7f;

// ---- 5 机多边形：半径 1.1m，均匀分布 ----
#define N 5
const float R_FORM = 1.1f;
const float S_MIN  = 0.50f;      // 顶点收缩下限
const float S_ABORT = 0.45f;     // 低于此 -> 解散

// ---- 各顶点收缩因子（本机只关心自己）----
float sLocal = 1.0f;
unsigned long sHoldT = 0;

// ---- 侧向/前向障碍 ----
const int TRIG_L = 6, ECHO_L = 7, TRIG_R = 8, ECHO_R = 9;
float dL = 9, dR = 9;

// ---- 队形中心（Leader 广播）----
struct Center { float cx, cy, cth; unsigned long t; };
Center gC = {0,0,0,0};
bool haveC = false;

void onRecv(const uint8_t*, const uint8_t* d, int len) {
  if (len != sizeof(Center)) return;
  Center c; memcpy(&c, d, sizeof(c));
  if (millis() - c.t < 400) { gC = c; haveC = true; }
}

float readUS(int t, int e) {
  digitalWrite(t, LOW); delayMicroseconds(2);
  digitalWrite(t, HIGH); delayMicroseconds(10); digitalWrite(t, LOW);
  long us = pulseIn(e, HIGH, 20000);
  return (us > 0) ? us * 0.034f / 2.0f / 100.0f : 9.0f;
}

float px = 0, py = 0, pth = 0, vCur = 0, wCur = 0;
bool disbanded = false;

void setup() {
  // 电机初始化同案例一
  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, 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);
  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.15f) dt = 0.02f;

  // ---- 1. 侧向感知：本顶点只看自己这一侧 ----
  dL = readUS(TRIG_L, ECHO_L);
  dR = readUS(TRIG_R, ECHO_R);
  float dSide = min(dL, dR);          // 最近侧向障碍

  // ---- 2. 独立收缩：本顶点按自身障碍算 ----
  const float R_SAFE = 0.70f;
  float p = constrain((R_SAFE - dSide) / R_SAFE, 0.0f, 1.0f);
  float sTgt = 1.0f - (1.0f - S_MIN) * p;

  // 恢复滞回：收缩快、展开慢，且需持续 1.5s 无障碍才展开
  if (sTgt < sLocal) {
    sLocal += constrain(sTgt - sLocal, -1.5f*dt, 0.0f);
    sHoldT = now;
  } else {
    if (now - sHoldT > 1500) {
      sLocal += constrain(sTgt - sLocal, 0.0f, 0.30f*dt);
    }
  }
  sLocal = constrain(sLocal, S_ABORT, 1.0f);

  // ---- 3. 解散兜底：收缩到极限仍不可行 -> 就地停靠 ----
  if (sLocal <= S_ABORT + 0.01f && dSide < 0.30f) disbanded = true;
  if (disbanded && dSide > 0.60f) disbanded = false;   // 通道变宽 -> 归队

  // ---- 4. 期望位 ----
  float ang = (2.0f * PI * 1) / N;          // 本机所在顶点角（示意：1 号顶点）
  float r = R_FORM * sLocal;
  float ccx = haveC ? gC.cx : px;
  float ccy = haveC ? gC.cy : py;
  float cth = haveC ? gC.cth : pth;

  float tx = ccx + r*cosf(cth + ang);
  float ty = ccy + r*sinf(cth + ang);

  // ---- 5. 虚拟弹簧 ----
  float ex = tx - px, ey = ty - py;
  float fwd =  ex*cosf(pth) + ey*sinf(pth);
  float lat = -ex*sinf(pth) + ey*cosf(pth);

  float v = K_SPR*fwd - B_DMP*vCur;
  v = constrain(v, -0.25f, 0.90f);
  float w = constrain(1.7f*lat, -0.9f, 0.9f) - 0.35f*wCur;

  if (disbanded) { v = 0; w = 0; }          // 解散：原地待命

  // ---- 6. 执行 + 里程 ----
  float vL = v - w*TRACK*0.5f, vR = v + w*TRACK*0.5f;
  wheelL.loopFOC(); wheelR.loopFOC();
  wheelL.move(vL/R_W); wheelR.move(vR/R_W);

  float vl = wheelL.shaft_velocity*R_W, vr = wheelR.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("dSide="); Serial.print(dSide, 2);
  Serial.print(" s="); Serial.print(sLocal, 2);
  Serial.print(" disb="); Serial.println(disbanded);
  delay(20);
}

关键点：disbanded 是必须存在的出口。收缩因子只能"把队伍压小"，压到物理极限（车宽 + 安全余量）就无解了；此时正确行为是解散队形、就地停靠等指令，而不是继续压缩导致碰撞。工程上"允许队形解体"比"强守队形"安全得多。

要点解读
一、虚拟弹簧本质是 PD 控制，队形误差就是"形变"，不是"违规"
把期望位与当前位的偏差当作弹簧形变量 F = K·e - B·v，是编队控制里最经典也最鲁棒的做法。它的价值在于允许误差存在：队友慢半拍时队伍短暂拉长、路面不平时局部错位，都会被弹簧"吸收"而不是触发急停或报警。相比硬性位置伺服（误差超阈值就停），弹簧模型在真实地面上的连续性和乘坐感都好得多。K 决定"多硬"，B 决定"多稳"，两者比例失调就会出现画龙或拖沓。

二、收缩要改"期望位"，不要改"刚度"
最常见的错误做法是：遇到障碍就加大 K，把队伍"挤紧"。这会带来两个问题——① 障碍消失后刚度不会自动回退，队伍持续发僵、耗电增加、轮胎磨损加快；② 高刚度下任何位置噪声都会被放大成力矩抖动。正确做法（案例一、二）是保持 K 不变、把期望间距乘上一个收缩因子 s，物理上等价于整条弹簧被压缩，恢复时 s 回到 1，队伍自然展开，参数无需二次调整。这是"目标层调整"优于"增益层调整"的典型例子。

三、收缩因子必须全队唯一，且收缩快、展开慢
两个反向的坑：各算各的 → 靠近障碍的一台收缩、其余不动，队形被撕开，弹簧持续被拉长；收缩慢 → 障碍已经贴脸了队伍还没收拢。所以案例二用"全队障碍最小值"生成统一的 s，案例三虽然各顶点独立，但恢复仍需 1.5 s 持续无障碍才展开。这条"快收慢放"的不对称性与之前多个主题（力矩卸载、避障降速）完全同构：安全方向的动作要快，恢复方向的动作要慢，否则会在边界处反复抖动。

四、收缩有物理下限，压到极限必须允许"解散"
收缩因子不是可以无限小的。下限由车体宽度 + 最小安全间距决定（案例一 S_MIN=0.55、案例三 S_ABORT=0.45），低于这个值，收缩本身就变成了碰撞源。所以架构上必须有第三个状态：收缩（SHRINK）→ 极限（LIMIT）→ 解散（DISBAND）。解散不是失败，而是"承认当前队形不可行"的正确响应——原地停靠、退化为单列、或等待指令，都比强行压缩安全。编队系统的成熟度，往往就体现在它敢不敢解散。

五、分层与失联降级：队形是"协同的产物"，不能假设通信永远在
编队依赖机间通信，而通信必然有丢包。三层结构（感知 10 Hz / 规划 20 Hz / 执行 1 kHz）保证即使规划层被阻塞，执行层仍能把当前指令走完；而失联降级必须显式定义：案例一 !hasLeader 就地停车，案例二 !haveCenter 用自身位姿做临时中心。绝不能做的是"通信断了还按最后的期望位猛冲"——那是编队系统最常见的事故原因。任何依赖他人信息的控制器，都必须有一个"没有他人信息时也安全"的默认行为。

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