在基于 Arduino + BLDC（无刷直流电机）​ 的多机器人系统中，动态变换编队（虚拟结构切换 / Dynamic Virtual Structure Reconfiguration），是指多台机器人原本保持某一虚拟几何结构（如直线、三角形、矩形），在任务需要时可平滑切换为另一种虚拟结构或重组成员关系，并通过协调过渡策略防止切换瞬间 BLDC 输出突变导致撞车或失步。与固定虚拟结构不同，此方案强调结构形态可变、成员可重分配、过渡过程受控。

一、主要特点：形态切换 + 成员重绑 + 平滑过渡
1、动态切换类型
形态切换（Shape Change）
例：三角形 → 直线（通过窄通道）
仅改偏移量，成员编号不变
成员重分配（Role Reassignment）
某机电量低 / 故障 → 其余机按新偏移重绑，保持原形态
常配合“ID 优先级”或“距质心最近者替补”
拆分 / 合并
一大编队拆为两个子虚拟结构（各带子质心）
任务完成后重新合并
2、切换过渡与平滑（关键）
直接瞬间改偏移会导致目标点跳变 → BLDC 急加速/急转。
常用处理方式：
渐变偏移插值（LERP）
在 N 个控制周期（如 0.5~1s）内，将旧偏移线性插值到新偏移
机器人目标点连续变化，BLDC 速度环可跟踪
虚拟结构暂停/减速过渡
切换前虚拟质心速度斜坡降至较低值（或暂停）
完成偏移切换后，再加速恢复
最短路径旋转（Heading Blend）
若新编队要求不同航向，可让虚拟结构航向也渐变，避免侧向滑移过大（差速车）
3、通信与状态同步
通常 Leader / 协调机​ 广播：
新编队类型 ID
新的相对偏移表（或索引引用预设表）
切换开始时间戳 / 过渡时长
Follower 本地查表应用，不需每帧传全坐标
可用 ESP‑NOW / LoRa / WiFi UDP，带 序列号 + ACK​ 防重复应用

二、应用场景
仓储 AGV 变通道编队
三台 AGV 以三角编队巡视 → 遇到窄通道 → 动态切换为一字纵队
通过后自动切回三角，全程 BLDC 不停车急转，只减速平滑变形
安保/巡逻哨兵重组
发现可疑区域 → 前二机呈 V 形包抄，后一机留守观察（形态 + 成员角色变化）
威胁解除 → 恢复三角环绕
灾难救援集群
开阔地用扇形展开搜索（大间距 V/Diamond）
进入废墟门洞 → 切换紧凑直线
某机卡住 → 剩余机重分配偏移保持形态继续搜索

三、关键注意事项（设计红线）

1. 偏移跳变与 BLDC 动态响应
   最大位移突变限幅
   即便做插值，单周期目标点位移增量也应 ≤ 电机最大速度 × Δt
   若过渡时间太短，实际跟不上 → 跟踪误差大、震荡
   差速车旋转中心突变
   切换前后若某机从“内侧”变“外侧”，需角速度限幅，防原地急旋
   建议：过渡期适当降虚拟质心速度上限（如 50% 正常）
2. 成员失效与编号一致性
   所有机必须：
   存有同一套预设偏移表索引
   明确自身 Slot ID（0/1/2…）
   若成员掉线：
   Leader 广播新有效 ID 列表 → 其余机重映射 Slot
   若无法重组（如三角变直线但仅剩 1 台），降级为单机巡线/待命
   防重应用：切换命令带递增 SeqNum，旧包丢弃
3. 若定位置信度低（如 UWB 多径严重），暂缓形态切换或只允许“缩小间距/锁轴”
   新成员加入后首次应用偏移前，应先完成自身位姿初始化对齐
4. 嵌入式实现要点（Arduino / ESP32）
   偏移表用 const PROGMEM（Flash 存预设），RAM 只存当前激活偏移
   插值可用整型或 Q15 定点：
   cur_off = old_off + (new_off - old_off) * step / total_steps
   AVR（Uno/Mega）：
   若三角函数用于航向旋转 → 查表（LUT）
   或限定航向离散档（0°/90°/180°…）
   ESP32 / STM32：
   可 float + sin/cos，双核分工（Core0 FOC/电机，Core1 编队+通信）
5. 安全与故障接管
   切换中若：
   无线心跳丢失
   编码器失效
   碰撞/悬崖触发
   → 立即中止切换，进入 ERROR / 缓停 / 锁轴
   急停具最高优先级，可硬拉 BLDC EN pins

1、串口指令触发编队切换（状态机 + 差速映射）
适合遥控操作，通过串口发送编队代号实时切换，核心是状态机确保切换安全。

#include <ESP32Servo.h>

#define PWM_FL   12  // Front Left
#define PWM_FR   13  // Front Right
#define PWM_RL   14  // Rear Left
#define PWM_RR   27  // Rear Right

Servo escFL, escFR, escRL, escRR;

enum Formation { V_FORM, PARA_FORM, TRI_FORM, STOP_FORM };
Formation currentForm = STOP_FORM;

float targetV  = 0.0;   // 纵向速度 -100~100
float targetW  = 0.0;   // 横摆角速度 -100~100

void setup() {
  Serial.begin(115200);
  
  for (int p : {PWM_FL, PWM_FR, PWM_RL, PWM_RR}) {
    pinMode(p, OUTPUT);
  }
  
  escFL.setPeriodHertz(500); escFR.setPeriodHertz(500);
  escRL.setPeriodHertz(500); escRR.setPeriodHertz(500);
  escFL.attach(PWM_FL, 1500, 2000);
  escFR.attach(PWM_FR, 1500, 2000);
  escRL.attach(PWM_RL, 1500, 2000);
  escRR.attach(PWM_RR, 1500, 2000);
  
  brakeAll();
  delay(1000);
  Serial.println("Ready. Cmd: V/P/T/S");
}

void loop() {
  // 串口接收编队指令
  if (Serial.available()) {
    char c = Serial.read();
    switch (c) {
      case 'V': currentForm = V_FORM;    Serial.println("-> V-Form");    break;
      case 'P': currentForm = PARA_FORM;  Serial.println("-> Para-Form");  break;
      case 'T': currentForm = TRI_FORM;   Serial.println("-> Tri-Form");   break;
      case 'S': currentForm = STOP_FORM;  Serial.println("-> STOP");       break;
      default:  break;
    }
    brakeAll();
    delay(500);  // 切换安全延时
  }
  
  // 读取遥控输入
  if (Serial.available() >= 6) {
    String s = Serial.readStringUntil('\n');
    if (s.startsWith("CTRL:")) {
      targetV = s.substring(5, s.indexOf(',')).toFloat();
      targetW = s.substring(s.indexOf(',') + 1).toFloat();
    }
  }
  
  // 状态机：根据编队计算各轮PWM
  applyFormation();
  delay(20);  // 50Hz
}

void applyFormation() {
  uint16_t pFL, pFR, pRL, pRR;
  
  switch (currentForm) {
    case V_FORM:
      // V形：前轮差速，后轮同步跟随
      pFL = speedToPWM(targetV + targetW);
      pFR = speedToPWM(targetV - targetW);
      pRL = speedToPWM(targetV * 0.7);
      pRR = speedToPWM(targetV * 0.7);
      break;
      
    case PARA_FORM:
      // 平行：四轮同速，纯纵向
      pFL = pFR = pRL = pRR = speedToPWM(targetV);
      break;
      
    case TRI_FORM:
      // 三角：前驱主导，后轮随动补偿
      pFL = speedToPWM(targetV + targetW * 0.5);
      pFR = speedToPWM(targetV - targetW * 0.5);
      pRL = speedToPWM(targetV * 0.4 + targetW * 0.3);
      pRR = speedToPWM(targetV * 0.4 - targetW * 0.3);
      break;
      
    case STOP_FORM:
    default:
      pFL = pFR = pRL = pRR = 1500;
      break;
  }
  
  escFL.writeMicroseconds(pFL);
  escFR.writeMicroseconds(pFR);
  escRL.writeMicroseconds(pRL);
  escRR.writeMicroseconds(pRR);
}

uint16_t speedToPWM(float speed) {
  speed = constrain(speed, -100, 100);
  if (speed >= 0) return map((int)speed, 0, 100, 1500, 2000);
  else          return map((int)abs(speed), 0, 100, 1500, 1000);
}

void brakeAll() {
  escFL.writeMicroseconds(1500);
  escFR.writeMicroseconds(1500);
  escRL.writeMicroseconds(1500);
  escRR.writeMicroseconds(1500);
}

2、时间轴自动编队切换（定时任务 + 平滑插值）
适合预设路径自动变换，用 ESP32-S3 双核：Core 0 跑运动控制，Core 1 跑编队调度器，切换时 PWM 线性插值避免突变。

#include <ESP32Servo.h>
#include <esp_task_wdt.h>

#define PWM_FL 12
#define PWM_FR 13
#define PWM_RL 14
#define PWM_RR 27

Servo escFL, escFR, escRL, escRR;

struct FormationCfg {
  const char* name;
  float fl, fr, rl, rr;  // 各轮速度权重 0~1
};

FormationCfg formations[] = {
  {"V-Form",   1.0, 1.0, 0.6, 0.6},
  {"Para-Form", 1.0, 1.0, 1.0, 1.0},
  {"Tri-Form",  1.0, 1.0, 0.5, 0.5}
};

int currentIdx = 0;
int nextIdx    = 0;
float interpolT = 0.0;  // 插值因子 0→1
bool switching  = false;

float currentPWM[4] = {1500, 1500, 1500, 1500};
float targetPWM[4]  = {1500, 1500, 1500, 1500};

float throttle = 80.0;  // 基础油门 0~100
float steering = 0.0;   // 转向 -100~100

TaskHandle_t taskControl;
TaskHandle_t taskScheduler;

void setup() {
  Serial.begin(115200);
  
  escFL.setPeriodHertz(500); escFR.setPeriodHertz(500);
  escRL.setPeriodHertz(500); escRR.setPeriodHertz(500);
  escFL.attach(PWM_FL, 1500, 2000);
  escFR.attach(PWM_FR, 1500, 2000);
  escRL.attach(PWM_RL, 1500, 2000);
  escRR.attach(PWM_RR, 1500, 2000);
  
  // Core 1: 编队调度器
  xTaskCreatePinnedToCore(schedulerTask, "scheduler", 8192, NULL, 1, &taskScheduler, 1);
  
  // Core 0: 运动控制
  xTaskCreatePinnedToCore(controlTask, "control", 8192, NULL, 1, &taskControl, 0);
  
  delay(1000);
  Serial.println("Auto-switch enabled. Cycle: V->P->T->V...");
}

void loop() {
  // 主循环空闲，核心逻辑在双核任务中
  vTaskDelay(pdMS_TO_TICKS(100));
}

// Core 0: 高频运动控制 200Hz
void controlTask(void* pv) {
  for (;;) {
    // 线性插值 PWM 过渡
    if (switching) {
      interpolT += 0.05;  // 20步完成切换，约100ms
      if (interpolT >= 1.0) {
        interpolT = 1.0;
        switching = false;
        currentIdx = nextIdx;
        Serial.printf("Switched to: %s\n", formations[currentIdx].name);
      }
      for (int i = 0; i < 4; i++) {
        currentPWM[i] = currentPWM[i] + (targetPWM[i] - currentPWM[i]) * 0.05;
      }
    }
    
    // 应用当前 PWM
    escFL.writeMicroseconds((uint16_t)currentPWM[0]);
    escFR.writeMicroseconds((uint16_t)currentPWM[1]);
    escRL.writeMicroseconds((uint16_t)currentPWM[2]);
    escRR.writeMicroseconds((uint16_t)currentPWM[3]);
    
    vTaskDelay(pdMS_TO_TICKS(5));  // 200Hz
  }
}

// Core 1: 编队调度器，每 8 秒切换一次
void schedulerTask(void* pv) {
  TickType_t lastSwitch = xTaskGetTickCount();
  
  for (;;) {
    if (xTaskGetTickCount() - lastSwitch > pdMS_TO_TICKS(8000)) {
      lastSwitch = xTaskGetTickCount();
      
      nextIdx = (currentIdx + 1) % 3;
      
      // 计算目标 PWM（基于当前油门/转向）
      FormationCfg* cfg = &formations[nextIdx];
      float baseV = map((int)throttle, 0, 100, 1000, 2000);
      float baseR = map((int)steering, -100, 100, -500, 500);
      
      targetPWM[0] = constrain(baseV + baseR * cfg->fl, 1000, 2000);
      targetPWM[1] = constrain(baseV - baseR * cfg->fr, 1000, 2000);
      targetPWM[2] = constrain(baseV * cfg->rl, 1000, 2000);
      targetPWM[3] = constrain(baseV * cfg->rr, 1000, 2000);
      
      switching = true;
      interpolT  = 0.0;
    }
    
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

3、CAN 总线多机协同编队（SimpleFOC + 虚拟结构）
多台机器人通过 CAN 共享编队状态，任意一台发出切换指令，全队同步变换。核心是虚拟结构算法：将多台机器人视为一个刚体，计算每台的目标速度。

#include <SimpleFOC.h>
#include <CAN.h>

#define PWM_FL 12
#define PWM_FR 13

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(5, 6, 7, 8);
BLDCDriver3PWM driverR(9, 10, 11, 12);

// 编码器
Encoder encoderL(22, 23, 500);
Encoder encoderR(25, 26, 500);

// CAN 编队数据
struct FormationMsg {
  uint8_t  formationId;  // 0=V, 1=Para, 2=Tri
  float    globalV;       // 全局纵向速度
  float    globalW;       // 全局横摆
  uint8_t  robotId;       // 本机ID 0~7
  uint8_t  totalRobots;   // 总机器人数
};

FormationMsg myFormation = {0, 0, 0, 0, 1};

float Kp = 3.0, Ki = 0.8, Kd = 1.5;
float intL = 0, intR = 0, lastErrL = 0, lastErrR = 0;

hw_timer_t* canTimer;

void setup() {
  Serial.begin(115200);
  
  CAN.setPins(4, 5);
  if (!CAN.begin(500E3)) {
    Serial.println("CAN init fail!");
    while (1);
  }
  
  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  driverL.init(); driverR.init();
  
  motorL.linkDriver(&driverL);
  motorR.linkDriver(&driverR);
  motorL.linkSensor(&encoderL);
  motorR.linkSensor(&encoderR);
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  
  canTimer = timerBegin(0, 80, true);
  timerAttachInterrupt(canTimer, &canISR, true);
  timerAlarmWrite(canTimer, 10000, true);  // 10kHz CAN发送
  timerAlarmEnable(canTimer);
  
  Serial.println("CAN formation ready");
}

void loop() {
  // FOC 闭环
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 读取 CAN 编队数据
  if (CAN.parsePacket()) {
    FormationMsg msg;
    CAN.readBytes((uint8_t*)&msg, sizeof(msg));
    myFormation = msg;
  }
  
  // 虚拟结构算法：根据编队计算本机目标速度
  float targetL, targetR;
  calcVirtualStructure(targetL, targetR);
  
  // PID 速度闭环
  float velL = encoderL.getVelocity();
  float velR = encoderR.getVelocity();
  
  float errL = targetL - velL;
  float errR = targetR - velR;
  
  intL += errL; intR += errR;
  intL = constrain(intL, -300, 300);
  intR = constrain(intR, -300, 300);
  
  float outL = Kp * errL + Ki * intL + Kd * (errL - lastErrL);
  float outR = Kp * errR + Ki * intR + Kd * (errR - lastErrR);
  
  lastErrL = errL; lastErrR = errR;
  
  if (abs(outL) < 0.3) outL = 0;
  if (abs(outR) < 0.3) outR = 0;
  
  motorL.move(outL);
  motorR.move(outR);
  
  delay(10);  // 100Hz
}

void calcVirtualStructure(float& vL, float& vR) {
  float V = myFormation.globalV;
  float W = myFormation.globalW;
  float L = 0.3;  // 轮距 30cm
  
  switch (myFormation.formationId) {
    case 0:  // V形
      vL = V + W * L;
      vR = V - W * L;
      break;
    case 1:  // 平行
      vL = V;
      vR = V;
      break;
    case 2:  // 三角
      vL = V + W * L * 0.6;
      vR = V - W * L * 0.4;
      break;
    default:
      vL = V; vR = V;
  }
}

void canISR() {
  FormationMsg msg = myFormation;
  msg.robotId = 0;  // 本机ID
  msg.totalRobots = 4;
  
  CAN.beginPacket(0x100, sizeof(msg));
  CAN.write((uint8_t*)&msg, sizeof(msg));
  CAN.endPacket();
}

要点解读
一、切换瞬间必须归零——这不是建议，是物理铁律
编队切换本质是动力分配的突变。V形切平行时，后轮从 0.6V 跳到 1.0V，前轮从差速跳到同速——若不先刹停，电机反电动势与新指令叠加，瞬时电流可达额定值 5~8 倍，直接烧毁 ESC MOSFET。案例一用 brakeAll() + 500ms 延时，案例二用 20 步线性插值，案例三靠 CAN 同步全队同时切换——三种方案殊途同归：切之前，必须停。

二、虚拟结构算法是多机协同的唯一正确解法
多台机器人编队不是"各跑各的"，而是视为一个刚体。案例三的 calcVirtualStructure() 就是核心：给定全局速度 V 和角速度 W，根据每台机器人在结构中的位置（轮距 L、前后偏置），算出各自目标轮速。V形时外侧轮快、内侧轮慢；平行时四轮同速；三角时前驱主导、后轮补偿。没有这个算法，多机编队就是一盘散沙。

三、插值切换 vs 硬切——100ms 的差距决定机器人会不会翻车
案例二的线性插值是实战中最容易被忽略的细节。硬切（案例一）简单但冲击大，适合低速场景；插值切换用 20 步、每步 5ms 完成 PWM 过渡，总耗时约 100ms，加速度曲线平滑，底盘不会" jerk "（急抖）。竞技级机器人必须用插值，搬运机器人硬切也能凑合用。 关键参数：插值步数 ≥ 15 步，单步 ≤ 10ms。

四、CAN 总线是多机编队的必然选择，串口做不到
案例三用 CAN 而非串口，原因只有一个：实时性。串口是点对点，4 台机器人需要 6 条串口线或轮询，延迟不可控。CAN 总线 500kbps 下，4 台机器人广播 + 接收总延迟 < 2ms，且所有节点同时收到切换指令，真正做到"全队同步变阵"。超过 2 台机器人，请直接上 CAN，不要犹豫。

五、编码器闭环是编队精度的生命线，开环等于赌博
案例一没有编码器，靠开环 PWM 控制——电池电压一降、负载一变，实际转速和指令偏差可达 15~20%，编队两秒就散。案例二、三都用编码器做速度闭环（SimpleFOC），精度控制在 ±2% 以内。多机编队时，如果一台快 10%、一台慢 10%，V形两秒就变成梯形，平行队形直接撕裂。开环能跑，闭环才能编队。这是"能动"和"能用"的分水岭。

4、工业协作场景——多机器人流水线动态矩形/菱形编队切换（基于主从CAN通信）
场景定位：适用于汽车装配、电子制造等流水线协作场景，需根据工序变化实时切换编队结构（如矩形队列用于物料搬运、菱形队列用于避障绕行），核心需求是主控统一调度、从机毫秒级响应、BLDC电机精准同步，保障流水线节拍与协作效率。

核心逻辑：采用主从式CAN通信架构，主控（ESP32-S3）作为编队中心，实时下发编队切换指令与目标轨迹；从机（Arduino BLDC控制器）接收指令后，通过PID闭环控制BLDC电机，跟踪预设轨迹，实现矩形→菱形编队的动态切换，同时通过编码器反馈保证速度同步，避免编队散架。

// 主控代码（ESP32-S3）：负责编队结构计算与指令下发
#include <SimpleFOC.h>
#include <CAN.h>

// 编队结构参数（单位：米）
#define FORMATION_RECT_X1 0.5
#define FORMATION_RECT_X2 1.5
#define FORMATION_RECT_Y1 0.5
#define FORMATION_RECT_Y2 0.5
#define FORMATION_DIAMOND_X1 1.0
#define FORMATION_DIAMOND_Y1 0.0
#define FORMATION_DIAMOND_X2 1.0
#define FORMATION_DIAMOND_Y2 1.0

// CAN通信参数
#define CAN_ID_MASTER 0x00
#define CAN_ID_SLAVE1 0x01
#define CAN_ID_SLAVE2 0x02
#define CAN_SPEED 1000000 // 1Mbps

// BLDC电机初始化（主控仅需发送指令，无需直接控制）
void setup() {
  Serial.begin(115200);
  CAN.setPins(18, 19); // ESP32-S3 CAN引脚
  if (!CAN.begin(CAN_SPEED)) {
    Serial.println("CAN初始化失败!");
    while (1);
  }
  Serial.println("主控初始化完成");
}

// 计算目标位置与速度（根据编队结构）
void calcFormationTarget(float currentTime, uint8_t formationType, float& x, float& y, float& vx, float& vy) {
  float cycleTime = 10.0; // 编队切换周期
  if (formationType == 0) { // 矩形编队
    x = FORMATION_RECT_X1 + (FORMATION_RECT_X2 - FORMATION_RECT_X1) * (currentTime % cycleTime) / cycleTime;
    y = FORMATION_RECT_Y1;
  } else if (formationType == 1) { // 菱形编队
    float t = (currentTime % cycleTime) / cycleTime;
    x = FORMATION_DIAMOND_X1 + 0.5 * sin(2 * M_PI * t);
    y = FORMATION_DIAMOND_Y1 + 0.5 * cos(2 * M_PI * t);
  }
  vx = 0.2; // 固定线速度（m/s）
  vy = 0.0;
}

void loop() {
  static uint8_t currentFormation = 0;
  float x1, y1, vx1, vy1;
  float x2, y2, vx2, vy2;
  float currentTime = millis() / 1000.0;

  // 1. 计算两台从机的目标参数
  calcFormationTarget(currentTime, currentFormation, x1, y1, vx1, vy1);
  calcFormationTarget(currentTime, currentFormation, x2, y2, vx2, vy2);

  // 2. 封装CAN指令（4字节：x高4位+x低4位+y高4位+y低4位，简化处理）
  uint8_t canData1[4], canData2[4];
  uint16_t x1Int = (uint16_t)(x1 * 100);
  uint16_t y1Int = (uint16_t)(y1 * 100);
  uint16_t x2Int = (uint16_t)(x2 * 100);
  uint16_t y2Int = (uint16_t)(y2 * 100);
  canData1[0] = (x1Int >> 8) & 0xFF; canData1[1] = x1Int & 0xFF;
  canData1[2] = (y1Int >> 8) & 0xFF; canData1[3] = y1Int & 0xFF;
  canData2[0] = (x2Int >> 8) & 0xFF; canData2[1] = x2Int & 0xFF;
  canData2[2] = (y2Int >> 8) & 0xFF; canData2[3] = y2Int & 0xFF;

  // 3. 发送编队指令（每50ms刷新一次）
  if (millis() % 50 == 0) {
    CAN.beginPacket(CAN_ID_SLAVE1, 4);
    CAN.write(canData1, 4);
    CAN.endPacket();

    CAN.beginPacket(CAN_ID_SLAVE2, 4);
    CAN.write(canData2, 4);
    CAN.endPacket();

    // 每5秒切换编队结构
    if (millis() % 5000 == 0) {
      currentFormation = (currentFormation + 1) % 2;
      Serial.print("切换至编队："); Serial.println(currentFormation == 0 ? "矩形" : "菱形");
    }
  }
}

// 从机代码（Arduino + BLDC控制器）：接收指令并控制电机跟踪目标
#include <SimpleFOC.h>

// BLDC电机参数
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 6, 7, 8);

// 目标参数
float targetX = 0, targetY = 0;
float currentX = 0, currentY = 0;
float wheelRadius = 0.05; // 轮半径（米）
float encoderCountsPerRev = 1024; // 编码器线数

// PID控制参数
PID posPID(&currentX, &targetX, &OutputX, 2.0, 0.5, 1.0, DIRECT);
PID posPID_Y(&currentY, &targetY, &OutputY, 2.0, 0.5, 1.0, DIRECT);

// CAN通信参数
#define CAN_ID_SLAVE 0x01
uint8_t canData[4];

void setup() {
  Serial.begin(115200);
  // CAN初始化（需根据硬件配置调整引脚）
  // CAN.setPins(2, 3);
  // CAN.begin(1000000);

  // BLDC电机初始化
  motor.linkDriver(&driver);
  driver.voltage_power_supply = 12;
  driver.init();
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();

  // PID初始化
  posPID.SetMode(AUTOMATIC);
  posPID_Y.SetMode(AUTOMATIC);
  Serial.println("从机初始化完成");
}

// 编码器角度转线位移
float encoderToDisplacement(int encoderCount) {
  float angle = (encoderCount / (float)encoderCountsPerRev) * 2 * M_PI;
  return angle * wheelRadius;
}

void loop() {
  // 1. 接收CAN指令（模拟，实际需用CAN.endPacket()接收）
  // CAN.beginPacket(CAN_ID_SLAVE, 4);
  // while(CAN.availablePacket()) CAN.readPacket(canData, 4);
  // 简化：每100ms模拟接收一次
  if (millis() % 100 == 0) {
    targetX = ((canData[0] << 8) | canData[1]) / 100.0;
    targetY = ((canData[2] << 8) | canData[3]) / 100.0;
  }

  // 2. 获取当前位置（编码器反馈）
  currentX = encoderToDisplacement(motor.sensor.getAngle());
  currentY = encoderToDisplacement(motor.sensor.getAngle()); // 简化处理，实际需双编码器

  // 3. PID控制计算目标速度
  posPID.Compute();
  posPID_Y.Compute();
  float targetVelocity = sqrt(OutputX*OutputX + OutputY*OutputY);

  // 4. 电机速度闭环控制
  motor.target = targetVelocity;
  motor.loopFOC();

  // 调试输出
  Serial.print("目标X:"); Serial.print(targetX);
  Serial.print(" 当前X:"); Serial.print(currentX);
  Serial.print(" 目标速度:"); Serial.println(targetVelocity);
}

5、应急救援场景——多机器人避障编队动态切换（基于去中心化一致性算法）
场景定位：适用于地震废墟搜救、火灾现场物资转运等应急场景，特点是通信不稳定、障碍物动态变化、无固定主控，需机器人自主切换编队结构（如线性队列穿越狭窄通道、圆形编队围堵泄漏源），核心需求是去中心化拓扑重构、动态避障、BLDC电机高响应。

核心逻辑：采用基于一致性的去中心化编队控制，每个机器人通过蓝牙Mesh获取邻居状态，自主调整编队拓扑；结合超声波+IMU感知障碍物，触发编队切换（线性→圆形），通过BLDC电机的再生制动实现快速减速与姿态调整，保障应急场景的机动性与安全性。

// 机器人单机代码（Arduino + BLDC）：支持去中心化编队切换与避障
#include <SimpleFOC.h>
#include <BLEDevice.h>

// BLDC电机与驱动
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(5, 6, 7, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(9, 10, 11, 12);

// 传感器参数
#define ULTRASONIC_FRONT 2
#define ULTRASONIC_LEFT 3
#define ULTRASONIC_RIGHT 4
#define IMU_ADDR 0x68

// 编队参数
enum FormationType { LINEAR = 0, CIRCULAR = 1 };
FormationType currentFormation = LINEAR;
float formationRadius = 1.0; // 圆形编队半径
float neighborDist = 0.8; // 邻居距离（米）

// 避障与编队切换参数
float obstacleDistFront = 0, obstacleDistLeft = 0, obstacleDistRight = 0;
float switchThreshold = 0.5; // 避障触发编队切换的距离阈值

// 去中心化一致性参数
float localStateX = 0, localStateY = 0; // 自身状态
float neighborStateX[4] = {0}, neighborStateY[4] = {0};
int neighborCount = 0;

// BLE通信参数（模拟去中心化通信）
#define BLE_SERVICE_UUID "12345678-1234-5678-1234-567812345678"
#define BLE_CHAR_UUID "87654321-4321-8765-4321-876543214321"
BLECharacteristic *pCharacteristic;

// 初始化传感器
void initSensors() {
  pinMode(ULTRASONIC_FRONT, OUTPUT);
  pinMode(ULTRASONIC_LEFT, OUTPUT);
  pinMode(ULTRASONIC_RIGHT, OUTPUT);
  pinMode(ULTRASONIC_FRONT + 1, INPUT);
  pinMode(ULTRASONIC_LEFT + 1, INPUT);
  pinMode(ULTRASONIC_RIGHT + 1, INPUT);
}

// 读取超声波距离
float getUltrasonicDist(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  float duration = pulseIn(echoPin, HIGH);
  return (duration * 343 / 2) / 100;
}

// 去中心化编队一致性更新（简化）
void updateConsensus() {
  // 模拟接收邻居状态（实际通过BLE Mesh）
  for (int i = 0; i < neighborCount; i++) {
    // 一致性算法：调整自身状态与邻居同步
    localStateX = (localStateX + neighborStateX[i]) / 2;
    localStateY = (localStateY + neighborStateY[i]) / 2;
  }
}

// 动态编队切换逻辑
void switchFormation() {
  obstacleDistFront = getUltrasonicDist(ULTRASONIC_FRONT, ULTRASONIC_FRONT + 1);
  
  // 前方障碍物近，切换至圆形编队（绕障）
  if (obstacleDistFront < switchThreshold && currentFormation == LINEAR) {
    currentFormation = CIRCULAR;
    formationRadius = 0.8; // 缩小编队半径便于绕障
    Serial.println("切换至圆形编队");
  }
  
  // 无障碍物，切换回线性编队（高效移动）
  else if (obstacleDistFront > 2.0 && currentFormation == CIRCULAR) {
    currentFormation = LINEAR;
    Serial.println("切换至线性编队");
  }
}

// 计算目标速度（基于编队结构）
void calcTargetSpeed(float& targetVx, float& targetVy) {
  if (currentFormation == LINEAR) {
    // 线性编队：保持固定间距，匀速前进
    targetVx = 0.3;
    targetVy = 0.0;
  } else if (currentFormation == CIRCULAR) {
    // 圆形编队：绕圆心匀速圆周运动
    float omega = 0.2; // 角速度（rad/s）
    targetVx = -formationRadius * omega * sin(localStateX);
    targetVy = formationRadius * omega * cos(localStateY);
  }
}

// BLE数据接收回调
class MyBLECallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    std::string value = pCharacteristic->getValue();
    // 解析邻居状态（简化：接收4个字节，对应X1,Y1,X2,Y2）
    if (value.length() >= 8) {
      neighborStateX[0] = value[0] << 8 | value[1];
      neighborStateY[0] = value[2] << 8 | value[3];
      neighborCount = 1;
    }
  }
};

void setup() {
  Serial.begin(115200);
  initSensors();
  
  // BLDC电机初始化
  motorL.linkDriver(&driverL);
  motorR.linkDriver(&driverR);
  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  driverL.init(); driverR.init();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();

  // BLE初始化
  BLEDevice::init("RobotNode");
  BLEServer *pServer = BLEDevice::createServer();
  BLEService *pService = pServer->createService(BLE_SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(BLE_CHAR_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
  pServer->getAdvertising()->addServiceUUID(BLE_SERVICE_UUID);
  BLEDevice::startAdvertising();
  pCharacteristic->setCallbacks(new MyBLECallbacks());

  Serial.println("去中心化编队机器人初始化完成");
}

void loop() {
  // 1. 更新一致性状态
  updateConsensus();
  
  // 2. 动态编队切换
  switchFormation();
  
  // 3. 计算目标速度
  float targetVx, targetVy;
  calcTargetSpeed(targetVx, targetVy);
  
  // 4. 差速控制BLDC电机（双轮差速底盘）
  float vL = targetVx - targetVy * 0.2; // 轮距系数简化
  float vR = targetVx + targetVy * 0.2;
  vL = constrain(vL, -0.5, 0.5);
  vR = constrain(vR, -0.5, 0.5);
  
  motorL.move(vL);
  motorR.move(vR);
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 5. 发送自身状态（广播给邻居）
  unsigned char data[8];
  data[0] = (unsigned char)((localStateX * 100) >> 8);
  data[1] = (unsigned char)((localStateX * 100) & 0xFF);
  data[2] = (unsigned char)((localStateY * 100) >> 8);
  data[3] = (unsigned char)((localStateY * 100) & 0xFF);
  pCharacteristic->setValue(data, 4);
  
  // 6. 调试输出
  Serial.print("编队类型:"); Serial.print(currentFormation == LINEAR ? "线性" : "圆形");
  Serial.print(" 前方障碍:"); Serial.print(obstacleDistFront);
  Serial.print(" 左轮速度:"); Serial.print(vL);
  Serial.print(" 右轮速度:"); Serial.println(vR);
  
  delay(100);
}

6、阵列展示场景——多机器人灯光与队形同步切换（基于红外/光通信同步）
场景定位：适用于大型展会、舞台表演、科普展示等场景，需多机器人同步切换灯光与队形（如方阵→花朵→箭头），核心需求是无有线通信、高精度同步、BLDC电机静音控制、灯光与队形联动，保障视觉效果与同步性。

核心逻辑：采用红外/光通信实现同步触发，主机器人发送切换指令，从机接收后同步切换编队结构与灯光模式；通过BLDC电机的平滑PWM控制实现静音驱动，结合PID闭环保证队形整齐度，同时通过WS2812灯带实现灯光与队形的动态联动。

// 阵列展示机器人代码（Arduino + BLDC + 灯带）：支持同步编队与灯光切换
#include <SimpleFOC.h>
#include <Adafruit_NeoPixel.h>

// 硬件定义
#define LED_PIN 6
#define NUM_LEDS 16 // 灯带长度
Adafruit_NeoPixel strip(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);

BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(5, 6, 7, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(9, 10, 11, 12);

// 红外通信参数
#define IR_RX_PIN 2
#define IR_TX_PIN 3
#define IR_COMMAND_SWITCH 0x01 // 编队切换指令
#define IR_COMMAND_SYNC 0x02 // 同步指令

// 编队与灯光参数
enum FormationType { SQUARE = 0, FLOWER = 1, ARROW = 2 };
FormationType currentFormation = SQUARE;
uint8_t formationCount = 3;
uint8_t syncCount = 0; // 同步计数
uint8_t switchFlag = 0; // 切换标志

// 队形参数
float squarePos[4][2] = {{0,0}, {1,0}, {1,1}, {0,1}};
float flowerPos[8][2] = {{0.5,0.5}, {0.5+0.5,0.5}, {0.5+0.5*0.707,0.5+0.5*0.707}, 
                        {0.5,0.5+0.5}, {0.5-0.5*0.707,0.5+0.5*0.707}, {0.5-0.5,0.5},
                        {0.5-0.5*0.707,0.5-0.5*0.707}, {0.5+0.5*0.707,0.5-0.5*0.707}};
float arrowPos[5][2] = {{0,0.5}, {0.5,1.0}, {0.5,0.5}, {0.5,0.0}, {1.0,0.5}};

// 灯光参数
uint32_t squareColor = strip.Color(255, 0, 0); // 红色
uint32_t flowerColor = strip.Color(0, 255, 0); // 绿色
uint32_t arrowColor = strip.Color(0, 0, 255); // 蓝色

// 红外通信函数
void sendIRCommand(uint8_t cmd) {
  digitalWrite(IR_TX_PIN, HIGH);
  delayMicroseconds(100);
  digitalWrite(IR_TX_PIN, LOW);
  delayMicroseconds(100);
  // 简化：发送1ms脉冲表示指令
}

// 接收红外指令（中断回调）
void IR_Received() {
  uint8_t cmd = analogRead(IR_RX_PIN);
  if (cmd == IR_COMMAND_SWITCH) {
    switchFlag = 1;
  } else if (cmd == IR_COMMAND_SYNC) {
    syncCount++;
  }
}

// 设置灯光模式
void setLightMode(FormationType formation) {
  uint32_t color;
  if (formation == SQUARE) color = squareColor;
  else if (formation == FLOWER) color = flowerColor;
  else color = arrowColor;
  
  for (int i = 0; i < NUM_LEDS; i++) {
    strip.setPixelColor(i, color);
  }
  strip.show();
}

// 计算目标轨迹（基于队形）
void calcTargetTrajectory(FormationType formation, float time, float& x, float& y) {
  float cycleTime = 5.0;
  float t = (time % cycleTime) / cycleTime;
  
  if (formation == SQUARE) {
    int idx = floor(t * 4) % 4;
    x = squarePos[idx][0] + 0.5 * sin(2 * M_PI * t);
    y = squarePos[idx][1] + 0.5 * cos(2 * M_PI * t);
  } else if (formation == FLOWER) {
    for (int i = 0; i < 8; i++) {
      x = flowerPos[i][0] + 0.3 * cos(2 * M_PI * t + i * M_PI/4);
      y = flowerPos[i][1] + 0.3 * sin(2 * M_PI * t + i * M_PI/4);
    }
  } else if (formation == ARROW) {
    int idx = floor(t * 5) % 5;
    x = arrowPos[idx][0] + 0.4 * t;
    y = arrowPos[idx][1] + 0.2 * sin(2 * M_PI * t);
  }
}

// 初始化
void setup() {
  Serial.begin(115200);
  
  // 灯带初始化
  strip.begin();
  strip.show();
  setLightMode(currentFormation);
  
  // BLDC电机初始化（静音模式：降低PWM频率）
  motorL.linkDriver(&driverL);
  motorR.linkDriver(&driverR);
  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  driverL.init(); driverR.init();
  // 降低PWM频率，减少噪音
  driverL.pwm_frequency = 8000;
  driverR.pwm_frequency = 8000;
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  
  // 红外中断初始化
  attachInterrupt(digitalPinToInterrupt(IR_RX_PIN), IR_Received, RISING);
  
  Serial.println("阵列展示机器人初始化完成");
}

void loop() {
  // 1. 检测切换指令
  if (switchFlag == 1) {
    currentFormation = (FormationType)((currentFormation + 1) % formationCount);
    switchFlag = 0;
    Serial.print("切换至队形：");
    Serial.println(currentFormation == SQUARE ? "方阵" : currentFormation == FLOWER ? "花朵" : "箭头");
    setLightMode(currentFormation);
    // 发送同步指令（通知其他机器人切换）
    sendIRCommand(IR_COMMAND_SYNC);
  }
  
  // 2. 同步检测（确保编队同步）
  if (syncCount >= 3) { // 收到3次同步指令后切换
    syncCount = 0;
    currentFormation = (FormationType)((currentFormation + 1) % formationCount);
    setLightMode(currentFormation);
  }
  
  // 3. 计算目标轨迹
  float time = millis() / 1000.0;
  float targetX, targetY;
  calcTargetTrajectory(currentFormation, time, targetX, targetY);
  
  // 4. 差速控制电机（静音、平滑）
  float baseSpeed = 0.2;
  float vL = baseSpeed - 0.1 * sin(2 * M_PI * time);
  float vR = baseSpeed + 0.1 * cos(2 * M_PI * time);
  vL = constrain(vL, 0.1, 0.3);
  vR = constrain(vR, 0.1, 0.3);
  
  motorL.move(vL);
  motorR.move(vR);
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 5. 灯光动态效果（与队形联动）
  for (int i = 0; i < NUM_LEDS; i++) {
    float brightness = 128 + 127 * sin(2 * M_PI * time + i * 0.1);
    uint32_t color = strip.getPixelColor(i);
    // 提取RGB分量，调整亮度
    uint8_t r = (color >> 16) & 255;
    uint8_t g = (color >> 8) & 255;
    uint8_t b = color & 255;
    r = (r * brightness) / 255;
    g = (g * brightness) / 255;
    b = (b * brightness) / 255;
    strip.setPixelColor(i, strip.Color(r, g, b));
  }
  strip.show();
  
  // 6. 定时自动切换（备用）
  if (millis() % 10000 == 0) {
    sendIRCommand(IR_COMMAND_SWITCH);
  }
  
  delay(100);
}

要点解读

1. 编队拓扑重构：虚拟结构的动态切换逻辑设计
   虚拟结构切换的核心是拓扑规则的可配置性与快速切换能力，直接决定编队的灵活性与适用性：

结构抽象与参数化：将编队结构抽象为数学模型（如线性、圆形、花朵等），通过参数数组（如坐标点、半径、角度）定义拓扑，案例1的矩形/菱形、案例3的方阵/花朵均通过参数化实现快速切换，无需修改核心代码，适配多场景需求。

切换触发机制：根据场景需求设计触发逻辑，工业场景依赖主控指令（案例1），应急场景依赖避障触发（案例2），展示场景依赖外部指令（案例3）；同时加入防抖逻辑，避免频繁切换，保障编队稳定性。

拓扑一致性保障：切换前后需保证机器人状态连续，通过轨迹规划实现平滑过渡，如案例1中矩形→菱形切换时，采用渐进式目标位置变化，避免电机突变导致编队散架，提升切换流畅度。

2. 多机同步机制：高实时性通信与时钟对齐
   编队切换的核心瓶颈是多机器人的同步性，需从通信方式与时钟对齐两方面保障：

通信方式适配场景：

工业场景：优先CAN总线（高带宽、低延迟、抗干扰），支持多节点同步，案例1的CAN通信确保从机毫秒级响应；
应急场景：采用去中心化通信（蓝牙Mesh、红外、光通信），无单点故障，案例2的BLE Mesh、案例3的红外通信，适配通信不稳定环境；
展示场景：采用光/红外同步，实现无有线连接的高精度同步，案例3的红外指令确保多机器人灯光与队形联动。
时钟对齐与同步补偿：通过主从时钟对齐或去中心化一致性算法，消除机器人间的时钟漂移，案例2的一致性算法让机器人自主同步状态；同时加入同步计数（案例3的syncCount），确保所有机器人在同一时刻切换，避免编队错位。

通信容错与重传：设计指令重传与校验机制，案例1的CAN数据校验、案例3的红外指令重复发送，避免因通信丢包导致个别机器人未切换，保障编队整体同步性。

3. BLDC电机控制：高精度与动态响应的平衡
   编队切换的执行核心是BLDC电机的动态响应与精度，需满足轨迹跟踪、快速启停、姿态调整等需求：

闭环控制策略：采用FOC矢量控制实现速度/位置闭环，案例1、2、3均通过编码器反馈实现精准轨迹跟踪，避免开环控制的速度波动导致编队散架；同时引入PID控制补偿轨迹误差，案例1的PID控制确保机器人精准跟踪目标位置。

动态响应优化：编队切换需快速启停与转向，双向ESC需支持正反转与再生制动，案例2的再生制动实现快速减速，减少切换时间；电机参数（如PWM频率、PID系数）需根据场景调整，展示场景降低PWM频率实现静音，工业场景提升响应速度保障效率。

差速转向与轨迹跟踪：双轮差速底盘是主流方案，通过左右轮速度差实现转向，案例1-3均采用差速控制实现编队转向；结合轨迹规划算法，将虚拟结构坐标转化为左右轮目标速度，确保机器人沿编队轨迹移动，避免偏离拓扑。

4. 感知与决策融合：环境适配与自主切换的协同
   动态编队需适配复杂环境，需将传感器感知与编队决策深度融合，实现自主切换与避障：

多传感器融合感知：采用超声波、IMU、红外等传感器感知环境与自身状态，案例2的超声波检测障碍物，案例3的红外接收同步指令，感知数据为编队切换提供决策依据，避免盲目切换。

环境-编队联动决策：根据环境特征触发编队切换，如案例2中前方障碍物近时自动切换为圆形编队绕障，障碍消失后切换回线性编队提升效率；这种“感知→决策→切换”的闭环，让编队具备环境适应性，适配应急、工业等动态场景。

多目标优先级排序：应急场景中，安全优先级高于效率，避障优先于编队保持；展示场景中，视觉效果优先级高于速度，灯光与队形联动优先于避障，需根据场景设计优先级逻辑，避免冲突，案例2的优先级设计确保应急安全，案例3的联动设计提升展示效果。

5. 容错与鲁棒性：通信中断与故障自恢复
   复杂场景下，通信中断、电机故障等问题难以避免，需设计容错机制保障编队稳定性：

通信中断后的自治模式：主从架构中，主控故障后，从机切换为自治模式，采用预设的编队结构运行，案例1可预留应急自治逻辑；去中心化架构中，单个机器人故障不影响其他机器人，案例2的一致性算法具备容错性，部分节点故障后仍能维持编队。

电机故障应急处理：检测到电机过流、堵转或编码器失效时，立即触发再生制动或安全停车，同时通过灯光、无线报警，案例中可加入电流监测逻辑，避免电机故障导致编队失控。

参数自适应调整：环境变化时自动调整控制参数，如应急场景路面湿滑时，降低电机速度与加速度，提升稳定性；展示场景地面不平整时，通过IMU补偿姿态误差，避免队形歪斜，提升编队鲁棒性。

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