“Arduino BLDC 之基于 CAN 总线的位置/速度双环同步机器人”可以理解为：多个 BLDC 关节或轮组各自构成位置环、速度环和电流环，主控通过 CAN/CANopen 总线周期性下发目标位置或目标速度，并从各节点回传实际位置、速度和状态；主站以统一同步周期完成多轴插补、误差补偿和协同调度，从而实现多关节或多轮端的低延迟、高一致性运动。它既适用于机械臂、外骨骼、云台等多关节系统，也适用于差速底盘、全向底盘、AGV 等多轮协同平台。
一、主要特点

1. 分布式闭环架构，降低主控算力压力
   每个 BLDC 节点通常自带编码器、驱动器和局部闭环，能够在本机完成电流环、速度环甚至位置环计算。主控不再需要逐路处理所有底层 PWM 和电流采样，而是通过 CAN 总线下发目标值、读取反馈值，把算力留给轨迹规划、运动学解算和协同控制。
2. CAN/CANopen 提供高可靠实时通信
   CAN 总线采用差分传输、非破坏性仲裁、CRC 校验和自动重发，适合电机附近这种电磁干扰较强的环境。若使用 CANopen，还可利用 PDO 做周期性实时数据交换、SDO 做参数配置、NMT 管理节点状态、SYNC 报文统一多轴执行节拍。
3. 位置/速度双环通常以“外环位置、内环速度、最内环电流”运行
   典型结构是：位置环根据目标角度与实际角度误差输出期望速度；速度环根据期望速度与实际速度误差输出期望转矩或 q 轴电流；电流环由 FOC 完成三相电流闭环。
   如果驱动器支持 CiA 402，则可以使用 CSP 循环同步位置模式、CSV 循环同步速度模式，或者 PP/PV 轮廓模式，让主站与从站按固定周期协同更新。
4. 多轴同步依赖统一周期，而不是单纯提高波特率
   多关节机械臂、差速底盘或全向底盘都需要各轴“同时开始、同时结束、按比例运动”。仅靠高速 CAN 并不能保证同步，还需要主站以固定周期发送 SYNC 或周期性 PDO，并在每个周期内完成目标值下发和状态回传。
5. BLDC FOC 提升低速平稳性和抗扰能力
   BLDC 配合 FOC 可降低转矩脉动，使位置跟踪、低速转向和负载突变时的速度响应更平滑。对于需要频繁启停、微调和协同运动的机器人，FOC 比传统六步换向更适合。
6. 适合“一主多从”工程化扩展
   新增关节或轮组时，只需增加 CAN 节点并分配 ID，不必像脉冲+方向方案那样大量增加走线。对于 4 轴、6 轴甚至更多轴系统，CAN 总线在布线、维护和扩展性上优势明显。
   二、应用场景
7. 多关节机械臂
   主控通过逆运动学计算各关节目标角度，再经 CAN 总线同步下发。每个关节本地执行位置/速度/电流闭环，末端轨迹更平滑，适合示教再现、抓取、绘图和协作机械臂原型。
8. 移动机器人差速/全向底盘
   左右轮或麦克纳姆轮通过 CAN 接收速度指令，并回传编码器位置和转速。主控可根据里程计进行航位推算、直线修正和转向补偿，避免轮速不一致导致跑偏。
9. 外骨骼与康复设备
   髋、膝、踝等关节需要严格相位配合。CANopen SYNC 可用于统一步态周期，CiA 402 状态机可管理使能、故障复位和安全停机。
10. 云台、转台与光学指向平台
    这类场景对位置稳态误差、低速平稳性和抗扰动要求较高。位置环负责指向精度，速度环抑制振荡，电流环保证力矩输出连续。
11. AGV/AMR 工业搬运平台
    多轮驱动、多编码器反馈、多传感器节点都可通过 CAN 接入。主控负责路径跟踪和速度分配，底层节点负责电机闭环和故障上报。
12. 科研与竞赛平台
    例如基于 Arduino/ESP32/STM32 的多电机协同控制、CANopen 协议学习、CiA 402 状态机验证、FOC 三环整定、多轴插补实验等。
    三、需要注意的事项
13. 必须区分“CAN 物理层可靠”和“应用层同步可靠”
    CAN 总线能保证报文可靠传输，但多轴同步还需要主站周期稳定、从站响应一致、PDO 映射明确。若主控循环抖动，SYNC 间隔不稳定，多轴轨迹仍会出现相位差。
14. 控制周期要固定，避免阻塞延时
    位置/速度双环通常运行在 100 Hz–1 kHz 量级，电流环可能更高。主控应使用定时器中断或实时任务调度，不要在 CAN 收发、串口打印或传感器读取中插入阻塞延时。
15. 8 位 Arduino 更适合做轻量主站或单节点执行层
    ATmega328P 配合 MCP2515 可以完成基础 CAN 通信和简单双环控制，但多轴插补、逆运动学、CiA 402 完整协议栈和高频同步会明显吃紧。更建议将主控放在 STM32、ESP32-S3、Teensy 或上位机，Arduino 节点负责本地 FOC 和编码器闭环。
16. 总线负载必须控制
    CAN 2.0B 每帧最多 8 字节，多轴系统若每个周期都回传位置、速度、电流、状态字，总线负载会快速上升。应合理设置 PDO 周期、合并反馈字段、过滤无关 ID，并在高负载时降低非关键数据刷新率。
17. ID 分配和 PDO 映射要提前规划
    每个节点应有唯一 CAN ID，急停、状态异常等高优先级报文应使用更高优先级 ID。PDO 映射要统一字节序、单位、缩放系数和符号处理，避免多节点反馈含义不一致。
18. 位置环和速度环要分层整定
    位置环增益过高会引起超调和振荡，速度环增益过高会放大量化噪声和机械谐振。通常先整定电流环，再整定速度环，最后整定位置环；同时加入速度前馈、加速度限幅和积分抗饱和。
19. 编码器反馈需要滤波，但不能过度延迟
    速度通常由位置差分得到，容易受编码器量化噪声影响。可采用低通滤波、滑动平均或卡尔曼滤波，但滤波过强会带来相位滞后，影响速度环稳定性。
20. 必须处理节点失联、故障和急停
    CAN 节点可能出现掉线、编码器异常、过流、过温、堵转或驱动器故障。主控应检测心跳、状态字和错误码，触发时立即进入安全停机或减速保持，而不是继续下发位置指令。
21. 上电初始化要有明确状态机
    尤其是使用 CANopen/CiA 402 时，驱动器通常需要从初始化状态逐步进入使能状态，不能直接下发运动指令。应确认驱动器处于 Operation Enabled 后再进入位置/速度控制。
22. 机械标定不能省略
    轮径、轮距、减速比、编码器方向、机械零位、安装偏移都会影响位置/速度一致性。若未标定，即使 CAN 通信和闭环参数都正常，机器人仍可能出现直线跑偏、转向误差或关节零点漂移。

整体来看，这个架构的关键是：CAN/CANopen 解决“多节点可靠通信和同步”，位置/速度双环解决“单轴跟踪精度和动态响应”，FOC 电流环解决“力矩柔顺输出”，主控协同解决“多轴同时按比例运动”。做原型时，建议先实现“单节点 CAN 速度闭环 → 单节点位置闭环 → 双节点同步 → 多节点 PDO 周期调度 → CiA 402 状态机与安全降级”。

1、主节点广播 SYNC 触发 + 从节点双环跟随（位置/速度级联）
适用场景：双驱 AGV 或多关节机械臂的主从同步。主节点运行位置环+速度环双闭环，计算完成后广播目标值；从节点接收后暂存但不执行，收到 SYNC 帧后在同一时刻启动跟随，确保多轴“同时开始、同时到达”。

#include <SimpleFOC.h>
#include <SPI.h>
#include <mcp2515.h>

// ===== 主节点 BLDC（双环控制） =====
BLDCMotor motorMaster(7);
BLDCDriver3PWM driverMaster(9, 10, 11, 8);
Encoder encMaster(2, 3, 2048);

// CAN
MCP2515 canBus(10);  // CS 引脚

// 双环 PID
float targetPos = 0, targetVel = 0;
float Kp_pos = 8.0, Ki_pos = 0.0, Kd_pos = 0.1;
float Kp_vel = 0.5, Ki_vel = 2.0, Kd_vel = 0.0;

float posIntegral = 0, lastPosError = 0;
float velIntegral = 0, lastVelError = 0;

// 同步帧
struct SyncFrame {
  float pos;
  float vel;
  uint8_t seq;
};
uint8_t seqCounter = 0;

void setup() {
  Serial.begin(115200);
  
  // 电机初始化
  encMaster.init();
  motorMaster.linkSensor(&encMaster);
  driverMaster.voltage_power_supply = 24;
  driverMaster.init();
  motorMaster.linkDriver(&driverMaster);
  motorMaster.controller = MotionControlType::velocity;  // 底层用速度模式
  motorMaster.init();
  motorMaster.initFOC();
  
  // CAN 初始化（500kbps）
  canBus.reset();
  canBus.setBitrate(CAN_500KBPS);
  canBus.setNormalMode();
}

// 位置环：输出速度给定
float posLoop(float target, float actual) {
  float error = target - actual;
  posIntegral += error * 0.01;
  posIntegral = constrain(posIntegral, -5, 5);
  float derivative = (error - lastPosError) / 0.01;
  lastPosError = error;
  return Kp_pos * error + Ki_pos * posIntegral + Kd_pos * derivative;
}

// 速度环：输出力矩/电压给定
float velLoop(float target, float actual) {
  float error = target - actual;
  velIntegral += error * 0.01;
  velIntegral = constrain(velIntegral, -3, 3);
  float derivative = (error - lastVelError) / 0.01;
  lastVelError = error;
  return Kp_vel * error + Ki_vel * velIntegral + Kd_vel * derivative;
}

void loop() {
  motorMaster.loopFOC();
  
  // ===== 主节点双环计算 =====
  float actualPos = motorMaster.shaftAngle();
  float actualVel = motorMaster.shaftVelocity();
  
  // 外环：位置 → 速度给定
  float velCmd = posLoop(targetPos, actualPos);
  velCmd = constrain(velCmd, -10, 10);
  
  // 内环：速度 → 力矩给定
  float torqueCmd = velLoop(velCmd, actualVel);
  torqueCmd = constrain(torqueCmd, -3, 3);
  
  motorMaster.move(torqueCmd);
  
  // ===== 广播同步帧（数据预存） =====
  SyncFrame frame;
  frame.pos = targetPos;
  frame.vel = velCmd;
  frame.seq = seqCounter++;
  
  canBus.sendMessage(0x100, (uint8_t*)&frame, sizeof(frame));
  
  // ===== 广播 SYNC 触发帧 =====
  // 无数据帧，仅 ID 标识
  canBus.sendMessage(0x000, NULL, 0);
  
  delay(10);  // 100Hz 双环更新
}

从节点接收端（独立 Arduino）：

#include <SimpleFOC.h>
#include <SPI.h>
#include <mcp2515.h>

BLDCMotor motorSlave(7);
BLDCDriver3PWM driverSlave(9, 10, 11, 8);
Encoder encSlave(2, 3, 2048);

MCP2515 canBus(10);

struct SyncFrame { float pos; float vel; uint8_t seq; };
SyncFrame pendingFrame;
bool frameReady = false;

float Kp_vel = 0.5, Ki_vel = 2.0, Kd_vel = 0.0;
float velIntegral = 0, lastVelError = 0;

void setup() {
  encSlave.init();
  motorSlave.linkSensor(&encSlave);
  driverSlave.voltage_power_supply = 24;
  driverSlave.init();
  motorSlave.linkDriver(&driverSlave);
  motorSlave.controller = MotionControlType::velocity;
  motorSlave.init();
  motorSlave.initFOC();
  
  canBus.reset();
  canBus.setBitrate(CAN_500KBPS);
  canBus.setNormalMode();
}

float velLoop(float target, float actual) {
  float error = target - actual;
  velIntegral += error * 0.01;
  velIntegral = constrain(velIntegral, -3, 3);
  float derivative = (error - lastVelError) / 0.01;
  lastVelError = error;
  return Kp_vel * error + Ki_vel * velIntegral + Kd_vel * derivative;
}

void loop() {
  motorSlave.loopFOC();
  
  // 接收 CAN 帧
  can_frame rxFrame;
  if (canBus.readMessage(&rxFrame) == MCP2515::ERROR_OK) {
    if (rxFrame.can_id == 0x100) {
      // 数据帧：暂存但不执行
      memcpy(&pendingFrame, rxFrame.data, sizeof(SyncFrame));
      frameReady = true;
    } else if (rxFrame.can_id == 0x000 && frameReady) {
      // SYNC 帧：立即执行预存指令
      float torqueCmd = velLoop(pendingFrame.vel, motorSlave.shaftVelocity());
      torqueCmd = constrain(torqueCmd, -3, 3);
      motorSlave.move(torqueCmd);
      frameReady = false;
    }
  }
  
  delay(5);
}

解读：该案例直接体现了搜索结果中描述的“数据预存 + SYNC 触发”同步机制。主节点将双环计算后的目标值广播，从节点先存储，收到 SYNC 帧后才执行，使所有从节点的动作启动时刻对齐到微秒级。双环级联结构（外环位置→内环速度）确保了从节点在跟随过程中既保持位置精度，又具备速度环的动态响应能力。

2、SimpleFOC CANCommander 协议——基于寄存器映射的双环参数远程配置
适用场景：多轴系统中，主控需要远程调整各从节点的位置环/速度环 PID 参数，或在运行中切换控制模式。SimpleFOC 的 CANCommander 协议通过 29 位扩展帧的寄存器映射实现这一功能。

#include <SimpleFOC.h>
#include <SimpleCANio.h>
#include <CANCommander.h>

BLDCMotor motor(7);
BLDCDriver3PWM driver(9, 10, 11, 8);
Encoder enc(2, 3, 2048);

// CAN 控制器（以 STM32 内置 CAN 为例，需根据平台调整）
HardwareCAN can("can0");
CANCommander commander(can);

void setup() {
  Serial.begin(115200);
  
  enc.init();
  motor.linkSensor(&enc);
  driver.voltage_power_supply = 24;
  driver.init();
  motor.linkDriver(&driver);
  
  // 初始为位置模式
  motor.controller = MotionControlType::angle;
  motor.P_angle.P = 20.0;
  motor.PID_velocity.P = 0.5;
  motor.PID_velocity.I = 2.0;
  
  motor.init();
  motor.initFOC();
  
  // 注册为 CAN 节点地址 0x01
  commander.init(0x01);
  
  // 使能寄存器映射：主控可通过 CAN 读写这些参数
  commander.addMotor(&motor);
}

void loop() {
  motor.loopFOC();
  motor.move();
  
  // 处理 CAN 请求（非阻塞）
  commander.run();
  
  delay(1);
}
主控端（通过 CAN 发送写寄存器请求）：

cpp
#include <SimpleCANio.h>
#include <CANCommander.h>

HardwareCAN can("can0");
CANCommander commander(can);

void setup() {
  Serial.begin(115200);
  commander.init(0x00);  // 主控地址 0x00
}

void loop() {
  // 切换从节点到速度模式（寄存器 REG_CONTROL_MODE）
  // CAN ID 结构：地址(0x01) | 写请求 | 寄存器号
  uint8_t data = 1;  // 1 = velocity 模式
  commander.writeRegister(0x01, REG_CONTROL_MODE, &data, 1);
  
  delay(100);
  
  // 修改速度环 PID 参数（REG_VEL_PID_P）
  float newKp = 0.8;
  commander.writeRegister(0x01, REG_VEL_PID_P, (uint8_t*)&newKp, sizeof(float));
  
  delay(1000);
  
  // 读取当前位置（REG_ANGLE）
  float angle;
  commander.readRegister(0x01, REG_ANGLE, (uint8_t*)&angle, sizeof(float));
  Serial.print("从节点角度: "); Serial.println(angle);
  
  delay(1000);
}

解读：该案例基于 SimpleFOC 官方 CANCommander 协议的寄存器架构。29 位扩展帧的 ID 被划分为节点地址、包类型（读/写/响应/SYNC）、寄存器号和电机索引，主控无需预先约定每个参数的帧格式，而是通过寄存器号统一访问。这对于双环同步系统尤为重要：主控可以在运行中根据负载变化动态调整从节点的 REG_VEL_PID_P 或 REG_POS_PID_P，或通过 REG_CONTROL_MODE 在位置/速度模式间切换，而无需重新烧录固件。

3、双环同步的容错降级——CAN 通信超时后的独立安全模式
适用场景：主从同步系统中，CAN 总线因电磁干扰或节点故障中断。从节点不能无限等待同步帧，必须在超时后降级为本地安全模式（如保持当前位置或缓慢停车），避免失控。

#include <SimpleFOC.h>
#include <SPI.h>
#include <mcp2515.h>

BLDCMotor motor(7);
BLDCDriver3PWM driver(9, 10, 11, 8);
Encoder enc(2, 3, 2048);

MCP2515 canBus(10);

// 同步状态
struct SyncFrame { float pos; float vel; uint8_t seq; };
SyncFrame pendingFrame;
bool frameReady = false;
unsigned long lastSyncTime = 0;
const unsigned long SYNC_TIMEOUT = 100;  // 100ms 无 SYNC → 降级

// 控制模式
enum SyncMode { FOLLOWER, SAFE_HOLD, FAULT };
SyncMode mode = FOLLOWER;

float lastValidPos = 0;

void setup() {
  enc.init();
  motor.linkSensor(&enc);
  driver.voltage_power_supply = 24;
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::angle;
  motor.P_angle.P = 15.0;
  motor.init();
  motor.initFOC();
  
  canBus.reset();
  canBus.setBitrate(CAN_500KBPS);
  canBus.setNormalMode();
  
  lastValidPos = motor.shaftAngle();
}

void loop() {
  motor.loopFOC();
  
  // 接收 CAN
  can_frame rxFrame;
  if (canBus.readMessage(&rxFrame) == MCP2515::ERROR_OK) {
    if (rxFrame.can_id == 0x100) {
      memcpy(&pendingFrame, rxFrame.data, sizeof(SyncFrame));
      frameReady = true;
      lastSyncTime = millis();
    } else if (rxFrame.can_id == 0x000 && frameReady) {
      // 正常同步执行
      motor.move(pendingFrame.pos);
      lastValidPos = pendingFrame.pos;
      frameReady = false;
      mode = FOLLOWER;
    }
  }
  
  // ===== 超时检测与降级 =====
  if (millis() - lastSyncTime > SYNC_TIMEOUT && mode == FOLLOWER) {
    mode = SAFE_HOLD;
    Serial.println("CAN 同步超时，进入安全保持模式");
  }
  
  // 安全模式：保持最后有效位置，不执行新指令
  if (mode == SAFE_HOLD) {
    motor.move(lastValidPos);
    
    // 若超过 2 秒仍无恢复，进入故障停车
    if (millis() - lastSyncTime > 2000) {
      mode = FAULT;
      motor.disable();
      Serial.println("故障：CAN 通信中断");
    }
  }
  
  // 恢复检测
  if (mode == SAFE_HOLD && frameReady) {
    mode = FOLLOWER;
    Serial.println("CAN 恢复，重新进入跟随");
  }
  
  delay(5);
}

解读：该案例将双环同步的可靠性从“正常路径”延伸到“故障路径”。CAN 总线的差分信号虽有一定抗干扰能力，但在电机驱动等强电磁环境中仍可能丢帧。从节点检测到 lastSyncTime 超过 100ms 未更新时，不再盲目等待，而是以最后有效位置为目标执行位置保持。超过 2 秒则 motor.disable() 彻底卸力。这一分层降级策略避免了“通信中断但电机继续执行过期指令”的危险场景。

要点解读

1. SYNC 帧的核心价值是“将执行时刻与数据到达解耦”
   CAN 总线本身是异步的，各从节点收到广播帧的时刻存在微秒级差异。如果从节点收到数据帧立即执行，先收到的节点先动、后收到的后动，多轴之间会产生累积相位差。SYNC 机制的思路是：数据帧携带“要做什么”，SYNC 帧携带“什么时候做”。从节点在收到数据帧时只存储不执行，收到 SYNC 帧后才在同一时刻启动。由于 SYNC 帧极短（0 字节数据），其在总线上的传播延迟几乎可以忽略，所有从节点的执行时刻对齐到各自的 CAN 中断响应延迟以内。

2. 位置/速度双环的调参顺序必须是“先内后外”
   速度环是内环，位置环是外环。如果先调位置环，其输出的速度给定值变化剧烈，而速度环尚未整定好，无法跟踪这一给定，系统会表现为位置响应超调或振荡，此时无法判断问题出在位置环还是速度环。正确的顺序是：先将电机设为纯速度模式，整定 Kp_vel/Ki_vel 使速度阶跃响应无超调、无稳态误差；再切换为位置模式，保持速度环参数不变，整定 Kp_pos/Kd_pos。搜索结果中同样强调了“内环带宽必须远大于外环”这一原则。

3. SimpleFOC 的 CANCommander 寄存器映射比自定义协议更适合多参数系统
   自定义 CAN 帧格式（如案例一的 SyncFrame）在参数少、结构固定时简洁高效。但双环同步系统涉及位置目标、速度给定、PID 参数、控制模式、传感器反馈等多个变量，如果每个变量都自定义帧 ID 和字节布局，主控端代码会迅速膨胀且难以维护。CANCommander 的寄存器架构将“读写哪个参数”编码在 CAN ID 中，数据区只放值本身，主控通过 writeRegister(addr, REG_VEL_PID_P, &val, 4) 即可完成任意参数的远程设置。代价是需要使用 29 位扩展帧，且节点端需要维护完整的寄存器表。

4. 同步超时降级是 CAN 总线系统不可省略的安全层
   CAN 的 CRC 校验和自动重传机制能检测并恢复偶发的位错误，但无法处理“节点掉电”或“总线物理断开”这类持续性故障。从节点如果仅依赖“收到新帧就执行”，在总线断开后会一直执行最后一帧的指令——如果最后一帧是“以最大速度前进”，后果是灾难性的。案例三的 SYNC_TIMEOUT = 100ms 和 SAFE_HOLD 模式提供了明确的行为边界：通信正常时跟随，通信中断时保持当前位置（位置模式）或按预设斜率减速（速度模式），彻底中断时卸力。这一逻辑必须在从节点固件中独立实现，不能依赖主控端的“心跳包”机制（因为主控本身也可能故障）。

5. Arduino 平台上实现双环同步需要 32 位 MCU 且 CAN 与 FOC 应分时处理
   8 位 Arduino Uno 的算力无法同时运行 SimpleFOC 的 FOC 环（需 ≥1kHz）和 CAN 通信协议栈。FOC 的 Park/Clarke 变换、PID 计算、SVPWM 生成已经占用了大部分 CPU 时间，CAN 帧的接收中断会打断 FOC 时序，导致转矩输出抖动。搜索结果中 SimpleFOC 对 STM32 系列的 CAN 支持表明，推荐使用 STM32F4/G4 或 ESP32 等带内置 CAN 控制器的 32 位平台。如果必须使用 Arduino 生态，ESP32 + MCP2515 是可行组合，但应将 FOC 环放在高优先级定时器中断中，CAN 收发放在主循环中轮询，避免中断嵌套破坏 FOC 的周期性。

4、单节点位置+速度双环（CAN 收指令 / 回状态）
适用硬件：ESP32 + SimpleFOC BLDC + 编码器 + CAN 收发器（TJA1050/SN65HVD230）。
核心思路：位置环 100 Hz、速度环 1 kHz，两级都在节点内闭环；CAN 只负责“给目标位置/目标速度”和“回报实际位置/速度/电流/状态”，绝不参与实时环。位置环输出速度前馈 + 速度指令，速度环输出 Iq。

/*
 * 案例4：单关节双环 + CAN 指令/反馈
 * 报文：0x100+id  主机->节点 (目标位置 int32 mrad, 目标速度 int16 mrad/s, 模式)
 *      0x200+id  节点->主机 (实际位置 int32, 实际速度 int16, 电流 int16, 状态)
 */
#include <SimpleFOC.h>
#include "driver/twai.h"

BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 2048);
void onEncoder(){ encoder.handleA(); }

const uint8_t NODE_ID = 1;
const uint32_t ID_CMD  = 0x100 + NODE_ID;
const uint32_t ID_FB   = 0x200 + NODE_ID;

// ---- 指令与状态 ----
volatile int32_t cmdPos_mrad = 0;      // 目标位置
volatile int16_t cmdVel_mrads = 0;     // 速度前馈/限幅
volatile uint8_t cmdMode = 0;          // 0=失能 1=位置 2=速度 3=急停
volatile bool    canOnline = false;
unsigned long lastCmdMs = 0;

// ---- 双环 ----
const float KP_POS = 12.0f;     // 位置环 P (1/s) -> 输出速度
const float VEL_LIMIT = 20.0f;  // rad/s 位置环输出限幅
float posIntegrator = 0;        // 位置环积分（可选，抗静差）
const float KI_POS = 1.5f;

float targetVel = 0;            // 速度环给定
unsigned long lastPosMs = 0;

void setup() {
  encoder.init(); encoder.enableInterrupts(onEncoder);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;   // 我们用速度环做内环
  motor.PID_velocity.P = 0.35f;
  motor.PID_velocity.I = 4.0f;
  motor.PID_velocity.D = 0.0f;
  motor.PID_velocity.output_ramp = 300.0f;          // 速度环输出斜率限幅
  motor.LPF_velocity.Tf = 0.01f;                    // 速度反馈低通
  motor.voltage_limit = 12.0f;
  motor.current_limit = 5.0f;
  motor.init();
  driver.voltage_power_supply = 12.0f;
  driver.init();
  motor.linkDriver(&driver);
  motor.initFOC();

  // ---- TWAI 初始化 500kbps ----
  twai_general_config_t g = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)4, (gpio_num_t)5, TWAI_MODE_NORMAL);
  twai_timing_config_t  t = TWAI_TIMING_CONFIG_500KBITS();
  twai_filter_config_t  f = TWAI_FILTER_CONFIG_ACCEPT_ALL();
  twai_driver_install(&g, &t, &f);
  twai_start();
  Serial.begin(115200);
}

// ---- CAN 接收：只解析指令，不做控制 ----
void pollCan() {
  twai_message_t rx;
  while (twai_receive(&rx, 0) == ESP_OK) {
    if (rx.identifier != ID_CMD) continue;
    if (rx.data_length_code < 7) continue;
    int32_t p; int16_t v; uint8_t m;
    memcpy(&p, &rx.data[0], 4);
    memcpy(&v, &rx.data[4], 2);
    m = rx.data[6];
    cmdPos_mrad = p; cmdVel_mrads = v; cmdMode = m;
    lastCmdMs = millis();
    canOnline = true;
  }
  if (millis() - lastCmdMs > 200) { canOnline = false; cmdMode = 3; }  // 200ms 无指令 -> 急停
}

// ---- 位置环 100Hz：位置误差 -> 速度指令（含前馈）----
void posLoop() {
  if (millis() - lastPosMs < 10) return;
  float dt = (millis() - lastPosMs) / 1000.0f;
  lastPosMs = millis();

  float q = motor.shaft_angle;                       // rad
  float qd = cmdPos_mrad / 1000.0f;
  float e = qd - q;

  posIntegrator = constrain(posIntegrator + KI_POS * e * dt, -6.0f, 6.0f);
  float vCmd = KP_POS * e + posIntegrator + cmdVel_mrads / 1000.0f;  // 前馈
  targetVel = constrain(vCmd, -VEL_LIMIT, VEL_LIMIT);

  // 到位死区：抑制稳态抖动
  if (fabsf(e) < 0.002f && fabsf(cmdVel_mrads) < 10) { targetVel = 0; posIntegrator *= 0.9f; }
}

// ---- CAN 反馈 100Hz ----
void sendFeedback() {
  static unsigned long t = 0;
  if (millis() - t < 10) return;
  t = millis();
  twai_message_t tx = {};
  tx.identifier = ID_FB;
  tx.data_length_code = 8;
  int32_t p = (int32_t)(motor.shaft_angle * 1000.0f);
  int16_t v = (int16_t)(motor.shaft_velocity * 1000.0f);
  int16_t i = (int16_t)(motor.current * 1000.0f);
  uint8_t st = (cmdMode == 3) ? 0x03 : (cmdMode == 0) ? 0x00 : 0x01;
  memcpy(&tx.data[0], &p, 4);
  memcpy(&tx.data[4], &v, 2);
  memcpy(&tx.data[6], &i, 2);
  twai_transmit(&tx, 0);
  (void)st;
}

void setup1() {}   // 若双核可把速度环放到 core1

void loop() {
  pollCan();
  posLoop();

  // 速度环：1kHz，纯执行
  static unsigned long tl = 0;
  if (micros() - tl >= 1000) {
    tl = micros();
    motor.loopFOC();
    float v = (cmdMode == 3 || cmdMode == 0) ? 0.0f : targetVel;
    motor.move(v);
  }
  sendFeedback();

  static unsigned long pr = 0;
  if (millis() - pr > 100) {
    pr = millis();
    Serial.print("q="); Serial.print(motor.shaft_angle, 2);
    Serial.print(" v="); Serial.print(motor.shaft_velocity, 2);
    Serial.print(" tgt="); Serial.print(targetVel, 2);
    Serial.print(" online="); Serial.println(canOnline);
  }
}

关键：cmdMode == 3 是总线级急停，直接旁路所有环路把速度给零；lastCmdMs > 200ms 自动进急停，这是 CAN 双环机器人必须有的第一道失效保护。

5、主从多轴同步 —— SYNC 帧 + CSP 插值 + 速度前馈
适用硬件：1×主控（ESP32）+ N×关节节点（ESP32/STM32 + SimpleFOC + 编码器），CAN 总线菊花链。
核心思路：同步不是“让所有节点同时收到位置”，而是所有节点在同一个时间基准上按同一个节拍插值。主机每 10 ms 广播一次 SYNC，各节点收到后锁存本周期目标位置与上周期位置，用线性插值 + 速度前馈在两帧之间走完，位置连续、速度连续。

/*
 * 案例5（节点侧）：CSP 周期同步位置模式
 * 主机: 0x080 SYNC(每10ms, 数据=周期计数)
 *      0x100+id 目标位置 (int32 mrad)，在 SYNC 前发出
 * 节点：收到 SYNC -> 用 (pPrev,pNext) 线性插值 + 速度前馈
 */
#include <SimpleFOC.h>
#include "driver/twai.h"

BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9,10,11,8);
Encoder encoder = Encoder(2,3,2048);
void onEncoder(){ encoder.handleA(); }

const uint8_t NODE_ID = 2;
const uint32_t ID_SYNC = 0x080;
const uint32_t ID_CMD  = 0x100 + NODE_ID;
const uint32_t ID_FB   = 0x200 + NODE_ID;

// ---- 同步状态 ----
volatile int32_t pPrev = 0, pNext = 0;     // mrad
volatile bool    haveNew = false;
volatile uint32_t syncSeq = 0;
uint32_t syncSeqLocal = 0;
unsigned long syncT0 = 0;
const float TS = 0.010f;                   // 周期 10ms

// ---- 双环参数 ----
const float KP_POS = 18.0f, KI_POS = 2.0f;
float posI = 0;
const float VEL_LIM = 30.0f;
float vFF = 0;                              // 速度前馈（由插值斜率得到）

// ---- 同步质量监控 ----
uint32_t lostSync = 0, badOrder = 0;

void setup() {
  encoder.init(); encoder.enableInterrupts(onEncoder);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.4f; motor.PID_velocity.I = 5.0f;
  motor.PID_velocity.output_ramp = 500.0f;
  motor.LPF_velocity.Tf = 0.008f;
  motor.voltage_limit = 12.0f; motor.current_limit = 5.0f;
  motor.init(); driver.voltage_power_supply = 12.0f; driver.init();
  motor.linkDriver(&driver); motor.initFOC();

  twai_general_config_t g = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)4,(gpio_num_t)5,TWAI_MODE_NORMAL);
  twai_timing_config_t  t = TWAI_TIMING_CONFIG_500KBITS();
  twai_filter_config_t  f = TWAI_FILTER_CONFIG_ACCEPT_ALL();
  twai_driver_install(&g,&t,&f); twai_start();
  Serial.begin(115200);
}

// ---- 收帧：先存目标，SYNC 到来才生效（保证所有节点同一时刻切换）----
void pollCan() {
  twai_message_t rx;
  while (twai_receive(&rx, 0) == ESP_OK) {
    if (rx.identifier == ID_CMD && rx.data_length_code >= 4) {
      int32_t p; memcpy(&p, &rx.data[0], 4);
      pNext = p; haveNew = true;
    } else if (rx.identifier == ID_SYNC) {
      uint32_t seq = 0;
      if (rx.data_length_code >= 4) memcpy(&seq, &rx.data[0], 4);
      // 顺序性检查：允许 ±1 以内，跳变说明丢帧
      if (syncSeqLocal != 0 && seq != syncSeqLocal + 1) lostSync++;
      if (seq < syncSeqLocal) badOrder++;
      syncSeqLocal = seq; syncSeq = seq;
      if (haveNew) { pPrev = (int32_t)(motor.shaft_angle*1000.0f); pNext = pNext; haveNew = false; }
      else { pPrev = pNext; }         // 没新目标：保持不动（速度为 0）
      syncT0 = micros();
    }
  }
}

// ---- 位置插值 + 双环 ----
float interpPos() {
  float t = (micros() - syncT0) / 1e6f;
  t = constrain(t, 0.0f, TS);
  float a = t / TS;
  float p = pPrev + (pNext - pPrev) * a;                 // mrad
  vFF = (pNext - pPrev) / 1000.0f / TS;                  // rad/s 前馈
  return p / 1000.0f;
}

void loop() {
  pollCan();

  static unsigned long tl = 0;
  if (micros() - tl >= 1000) {
    tl = micros();
    // 外环：位置（每 10 次即 100Hz）
    static int cnt = 0;
    if (++cnt >= 10) {
      cnt = 0;
      float qd = interpPos();
      float e = qd - motor.shaft_angle;
      posI = constrain(posI + KI_POS*e*0.01f, -8.0f, 8.0f);
      float vCmd = KP_POS*e + posI + vFF;               // 前馈 + 反馈
      motor.target = constrain(vCmd, -VEL_LIM, VEL_LIM);
    }
    motor.loopFOC();
    motor.move(motor.target);
  }

  // 反馈 100Hz
  static unsigned long tf = 0;
  if (millis() - tf >= 10) {
    tf = millis();
    twai_message_t tx = {}; tx.identifier = ID_FB; tx.data_length_code = 8;
    int32_t p = (int32_t)(motor.shaft_angle*1000.0f);
    int16_t v = (int16_t)(motor.shaft_velocity*1000.0f);
    memcpy(&tx.data[0], &p, 4); memcpy(&tx.data[4], &v, 2);
    memcpy(&tx.data[6], &lostSync, 2);      // 回报同步质量
    twai_transmit(&tx, 0);
  }
}

同步的三层保障：① SYNC 用 0x080 高优先级 ID，保证准时；② 节点“先存目标、SYNC 再切换”，消除报文到达抖动；③ 速度前馈 (pNext-pPrev)/T 让内环不必靠误差积分追赶，同步误差可降到毫秒级。

6、FreeRTOS 三任务 + 节点状态机 + 掉线/超差故障响应
适用硬件：ESP32 双核 + SimpleFOC BLDC + 编码器 + CAN 收发器（多节点同构）。
核心思路：工业级 CAN 双环必须回答“同步坏了怎么办”。节点状态机 INIT→READY→ENABLED→FAULT，CAN 任务只做收发与超时判定，控制任务做双环，监控任务比对“指令位置 vs 实际位置”，超差或丢 SYNC 立即进 FAULT 并按规定斜率停机。

/*
 * 案例6：CAN 三任务 + 状态机 + 故障响应
 *   taskCan   50Hz : 收指令/SYNC、发心跳与反馈、超时判定
 *   taskCtrl 100Hz : 位置环（速度环在 1kHz 定时器/高优先级环内）
 *   taskMon   20Hz : 跟随误差、同步丢失、温度/电流异常 -> FAULT
 */
#include <SimpleFOC.h>
#include "driver/twai.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9,10,11,8);
Encoder encoder = Encoder(2,3,2048);
void onEncoder(){ encoder.handleA(); }

const uint8_t  NODE_ID  = 3;
const uint32_t ID_SYNC  = 0x080;
const uint32_t ID_CMD   = 0x100 + NODE_ID;
const uint32_t ID_FB    = 0x200 + NODE_ID;
const uint32_t ID_HB    = 0x700 + NODE_ID;   // 心跳

SemaphoreHandle_t mux;

enum St { ST_INIT, ST_READY, ST_ENABLED, ST_FAULT };
volatile St st = ST_INIT;

volatile int32_t pCmd = 0, pPrev = 0, pNext = 0;
volatile bool haveNew = false;
unsigned long lastCmdMs = 0, lastSyncMs = 0;
volatile uint32_t syncSeq = 0, syncSeqLocal = 0;
volatile uint32_t lostSync = 0;
volatile float followErr = 0;

const float TS = 0.010f;
const float KP = 18.0f, KI = 2.0f, VLIM = 30.0f;
float posI = 0;

void enterFault(const char* why) {
  st = ST_FAULT;
  Serial.print("FAULT: "); Serial.println(why);
}

void setup() {
  encoder.init(); encoder.enableInterrupts(onEncoder);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.4f; motor.PID_velocity.I = 5.0f;
  motor.PID_velocity.output_ramp = 500.0f;
  motor.LPF_velocity.Tf = 0.008f;
  motor.voltage_limit = 12.0f; motor.current_limit = 5.0f;
  motor.init(); driver.voltage_power_supply = 12.0f; driver.init();
  motor.linkDriver(&driver); motor.initFOC();

  twai_general_config_t g = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)4,(gpio_num_t)5,TWAI_MODE_NORMAL);
  twai_timing_config_t  t = TWAI_TIMING_CONFIG_500KBITS();
  twai_filter_config_t  f = TWAI_FILTER_CONFIG_ACCEPT_ALL();
  twai_driver_install(&g,&t,&f); twai_start();

  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskCan,  "can",  4096, NULL, 3, NULL, 0);
  xTaskCreatePinnedToCore(taskCtrl, "ctrl", 4096, NULL, 3, NULL, 1);
  xTaskCreatePinnedToCore(taskMon,  "mon",  4096, NULL, 2, NULL, 0);

  st = ST_READY;
  Serial.begin(115200);
}

// ---------------- CAN 任务 ----------------
void taskCan(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    twai_message_t rx;
    while (twai_receive(&rx, 0) == ESP_OK) {
      if (rx.identifier == ID_CMD && rx.data_length_code >= 4) {
        int32_t p; memcpy(&p, &rx.data[0], 4);
        xSemaphoreTake(mux, portMAX_DELAY);
        pNext = p; haveNew = true;
        lastCmdMs = millis();
        xSemaphoreGive(mux);
      } else if (rx.identifier == ID_SYNC) {
        uint32_t seq = 0;
        if (rx.data_length_code >= 4) memcpy(&seq, &rx.data[0], 4);
        xSemaphoreTake(mux, portMAX_DELAY);
        if (syncSeqLocal && seq != syncSeqLocal + 1) lostSync++;
        syncSeqLocal = seq;
        pPrev = haveNew ? (int32_t)(motor.shaft_angle*1000.0f) : pNext;
        haveNew = false;
        lastSyncMs = millis();
        xSemaphoreGive(mux);
      }
    }

    // 超时判定：指令 200ms / SYNC 100ms
    if (millis() - lastCmdMs > 200 || millis() - lastSyncMs > 100) {
      if (st == ST_ENABLED) enterFault("cmd/sync timeout");
    }

    // 心跳 5Hz
    static unsigned long th = 0;
    if (millis() - th >= 200) {
      th = millis();
      twai_message_t tx = {}; tx.identifier = ID_HB; tx.data_length_code = 2;
      tx.data[0] = (uint8_t)st; tx.data[1] = NODE_ID;
      twai_transmit(&tx, 0);
    }
    vTaskDelayUntil(&last, pdMS_TO_TICKS(20));
  }
}

// ---------------- 控制任务 ----------------
void taskCtrl(void*) {
  TickType_t last = xTaskGetTickCount();
  float vFF = 0;
  for (;;) {
    int32_t pv, pn;
    xSemaphoreTake(mux, portMAX_DELAY);
    pv = pPrev; pn = pNext;
    xSemaphoreGive(mux);

    if (st == ST_ENABLED) {
      float qd = pn / 1000.0f;
      vFF = (pn - pv) / 1000.0f / TS;
      float e = qd - motor.shaft_angle;
      posI = constrain(posI + KI*e*0.01f, -8.0f, 8.0f);
      float v = constrain(KP*e + posI + vFF, -VLIM, VLIM);
      motor.target = v;
    } else {
      motor.target = 0;         // READY/FAULT 都输出零速
      posI *= 0.95f;
    }

    // 速度环 1kHz（此任务 100Hz 触发外环，内环用独立高频节拍）
    static unsigned long tl = 0;
    if (micros() - tl >= 1000) {
      tl = micros();
      motor.loopFOC();
      motor.move(motor.target);
    }
    vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
  }
}

// ---------------- 监控任务 ----------------
void taskMon(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float pe = fabsf(pNext/1000.0f - motor.shaft_angle);
    followErr = pe;

    if (st == ST_ENABLED) {
      if (pe > 0.30f)              enterFault("follow error > 0.3 rad");
      if (lostSync > 5)            enterFault("sync lost");
      if (fabsf(motor.current) > 6.0f) enterFault("over current");
    }
    // FAULT 自恢复：错误消失且持续 2s，回 READY 等主机使能
    static unsigned long ft = 0;
    if (st == ST_FAULT) {
      if (ft == 0) ft = millis();
      if (millis() - ft > 2000 && pe < 0.05f && lostSync <= 5) {
        st = ST_READY; ft = 0;
      }
    } else ft = 0;

    // 使能条件：主机下发模式 1 且已收到 SYNC
    if (st == ST_READY && millis() - lastSyncMs < 100 && millis() - lastCmdMs < 200)
      st = ST_ENABLED;

    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
  }
}

void loop() { vTaskDelay(portMAX_DELAY); }

状态机是这类系统的骨架：READY 与 ENABLED 分离，保证上电不会因为残留目标位置而突然动作；FAULT 有明确的自恢复条件（误差收敛 + 同步正常 + 2s 稳定），避免“抖一下就好、抖一下又坏”的反复启停。

要点解读
一、双环的带宽必须分离，位置环绝不能跑在 1kHz
位置环和速度环不是“两个 PID 并联”，而是带宽差 3~5 倍的串级：速度环（含电流环）1 kHz，位置环 100 Hz。若把位置环也提到 1 kHz，位置环输出会随编码器噪声高频抖动，经速度环放大后直接表现为电机啸叫与电流尖峰。反过来说，位置环的积分项（案例一 KI_POS、案例三 posI）必须比速度环积分慢一个数量级，否则两个积分器会互相“抢活”，出现低频振荡。这条带宽规则是 CAN 双环系统能否安静运行的第一前提。

二、CAN 只承载“指令与状态”，实时环必须留在节点内
总线是共享介质、有仲裁延迟、有排队——500 kbps 下一帧 8 字节约 200 μs，加上排队可到毫秒级。所以正确分工是：主机下发目标（10~100 Hz）、节点回报状态（100 Hz），双环全部在节点内闭环（案例一、二）。任何“把编码器读数发到主机、主机算完再发回来”的架构，都会引入一个数量级的额外延迟，且总线一拥堵就抖。这与前面所有案例的“感知/调度/执行分离”同构：CAN 是调度通道，不是控制通道。

三、真正的同步靠 SYNC 帧 + 本地插值，不靠“同时发送”
“同时发”在总线上不可能实现：报文有先后、节点处理有快慢。所以标准做法是周期同步位置模式（CSP）：主机先下发各节点目标位置，再广播一帧 SYNC（用最高优先级 ID，如 0x080），所有节点收到 SYNC 的瞬间锁存 (pPrev, pNext)，然后各自用本地时钟在 10 ms 内线性插值走完（案例二 interpPos）。这样各轴的位置-时间曲线严格同相，误差只取决于各节点晶振差异（通常 <100 ppm，10 ms 内 <1 μs 级），与报文到达抖动无关。这就是“同步”二字的技术含义：同步的是时间基准，不是报文。

四、速度前馈是双环同步的“隐形主力”，不能只靠反馈
纯反馈的位置环在跟随斜坡目标时必然有稳态滞后（滞后量 ≈ v/Kp），多轴联动时表现为“后面的轴永远慢半拍”，走圆弧会变成多边形。案例二用 vFF = (pNext - pPrev)/T 做速度前馈，等于把“这段路该走多快”直接告诉内环，位置环只需修正残差。效果是：位置误差降低一个数量级、各轴滞后一致、轨迹形状保真。前馈的系数应可标定（0.8~1.0），过大会引起超调。

五、故障安全要分级，且必须“自动进、有条件出”
CAN 双环系统的失效模式很具体：指令超时、SYNC 丢失、跟随误差超差、过流/过温、总线 Bus-Off。每一条都必须有独立判定和统一出口（案例三 enterFault）。三个原则：① 失效即零速，绝不保持最后一条指令（那是撞车的最常见原因）；② 进入快、退出慢——故障立即进 FAULT，恢复需误差收敛 + 同步正常 + 持续 2 s（案例三），防止反复启停；③ Bus-Off 要能自恢复：TWAI/CAN 控制器进入 Bus-Off 后需按协议做恢复序列（twai_initiate_recovery 或重新 start），并在恢复后先回 READY 而非直接 ENABLED。

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