Arduino BLDC CAN总线主从多轴同步——机器人位置/速度双环跟随，是以Arduino/ESP32为主控、BLDC无刷电机为执行器，通过CAN总线实现"一主多从"架构，主节点规划轨迹并广播同步指令，从节点运行"位置环+速度环"双闭环PID跟随主节点运动的多轴协同控制方案。 该方案具备CAN总线确定性同步、位置/速度双环级联控制、FOC低脉动驱动与主从柔性跟随四大特点，主要应用于多关节机械臂、多轴CNC/3D打印、双驱AGV/差速机器人、多机器人协同搬运及科研教学等场景；实际部署时需重点关注CAN总线拓扑与终端电阻、主控算力与实时性、双环PID调参顺序、编码器精度与分辨率、电源隔离与EMC、主从互搏与容错机制。
一、 技术架构与主要特点
CAN总线确定性同步：CAN总线采用差分信号传输，天然抗电磁干扰，且支持多主/主从广播通信。同步机制的核心在于"数据预存+SYNC触发"模式：主节点先广播各轴的目标位置/速度数据帧，从节点接收后暂存但不执行；主节点随后广播一帧无数据的SYNC同步帧，所有从节点在收到SYNC后立即执行预存指令。由于CAN的广播特性，SYNC帧到达所有节点的延迟仅在一个位时间（bit time）以内，同步抖动仅由各节点的中断延迟决定。若各从节点固件一致，同步精度可达微秒级。CAN标准帧最多携带8字节数据，可通过ID分段（如命令类/遥测类）区分优先级，命令帧ID更低、优先级更高。
位置/速度双环级联控制：这是该方案的核心控制架构，采用"外环定位置、内环定速度"的级联PID结构：
位置环（外环）：接收目标位置与编码器反馈的实际位置，计算位置误差，通过PID输出作为速度环的给定值（即"要跑多快才能到达目标"）。位置环更新频率通常≥100Hz。
速度环（内环）：接收位置环输出的速度给定值与编码器测得的实际速度，计算速度误差，通过PID输出PWM或电流指令驱动BLDC电机。速度环更新频率需≥500Hz（FOC场景建议≥1kHz），确保动态响应。
电流环（最内层）：通常由FOC算法在驱动板内部完成，输出三相正弦电流驱动电机，转矩纹波极低。
双环级联的关键在于内环带宽必须远大于外环，否则外环的速度给定值变化过快，内环来不及跟踪，会导致系统振荡。
FOC低脉动驱动：采用磁场定向控制（FOC）驱动BLDC电机，通过Park/Clarke变换将三相电流解耦为d轴（磁通）和q轴（转矩）分量，实现正弦电流驱动。相比传统六步换相，FOC的转矩纹波可降低70%以上，低速运行平滑无抖动，这对多轴同步至关重要——任何一轴的转矩脉动都会通过机械耦合传递到其他轴，破坏同步精度。开源库SimpleFOC已支持Arduino/ESP32/STM32平台的实时FOC运算。
主从柔性跟随与防互搏：对于刚性连接的多电机系统（如双驱AGV），若主从轴都设为速度模式，机械装配误差或负载差异会导致"互搏"（一个推一个拉，电流激增）。解决方案是采用"主轴速度环+从轴扭矩环"的主从策略：主轴负责速度跟踪，从轴通过扭矩环柔性跟随，利用扭矩差消除位置误差。进阶方案还会引入前馈控制——将轨迹规划得到的速度、加速度前馈到控制回路中，动态补偿系统惯性和摩擦力，显著减小跟踪误差。
二、 典型应用场景
多关节机械臂：这是最经典的应用。主控通过逆运动学实时解算各关节目标角度，经CAN总线同步下发至各关节从节点。每个从节点运行位置/速度双环控制，确保所有关节"同时开始、同时结束、按比例运动"，实现末端执行器在三维空间中精确、平滑的轨迹跟踪。控制频率通常≥500Hz，确保轨迹平滑与抗扰能力。
多轴CNC/3D打印：Arduino执行直线/圆弧插补算法，实时计算各轴在每个控制周期内的微小位移增量（ΔX, ΔY, ΔZ），通过CAN总线同步下发。双环控制确保各轴精确跟踪插补轨迹，避免加工圆变椭圆等轮廓误差。BLDC配合FOC驱动可实现极其平滑的转矩控制，减少低速爬行时的振动，提升加工表面光洁度。
双驱AGV/差速机器人：左右驱动轮分别由独立的BLDC从节点控制。主控通过里程计实时计算机器人位姿（X, Y, θ），根据路径规划算法计算出目标线速度和角速度，分解为左右轮速度指令，经CAN总线同步下发。双环控制确保左右轮精确跟踪速度指令，即使在负载变化或地面摩擦力不均的情况下也能保持直线行驶精度。
多机器人协同搬运：主机器人负责路径规划，通过CAN总线向从机器人广播自身位姿和队形参数。从机器人接收后，计算相对于主机器人的目标位姿偏移，运行双环跟随控制保持队形。CAN总线的高优先级帧可传递紧急停止指令，确保实时性（延迟<1ms）。
科研与教育平台：成本远低于商用多轴伺服系统，适合高校用于"多轴协同控制"“级联PID”"CAN总线通信"等课程的教学实训，也可用于RoboMaster等机器人竞赛中的多轴运动控制算法验证。
三、 关键注意事项
CAN总线拓扑与终端电阻：CAN总线必须采用总线型拓扑（菊花链），严禁星型布线。总线两端各需一个120Ω终端电阻，否则信号反射会导致通信错误。波特率建议500kbps或1Mbps，总线长度与波特率成反比（500kbps时建议≤100m）。各从节点CAN ID必须唯一，建议采用分段编码（如最高位区分命令/遥测，低位为设备编号），命令帧ID更低、优先级更高。
主控算力与实时性保障：多轴双环PID、FOC运算与CAN通信对算力要求较高。标准Arduino Uno（16MHz）难以胜任多轴同步，建议采用ESP32（双核240MHz）或STM32F4/F7等高算力板卡。控制回路必须使用硬件定时器中断保证确定性执行周期，严禁使用delay()函数。位置环频率≥100Hz，速度环频率≥500Hz（FOC场景建议≥1kHz）。
双环PID调参顺序：必须遵循"先内环后外环"的原则：
先断开位置环，单独调试速度环PID至响应快速且无振荡。
速度环稳定后，再接入位置环，调试位置环PID。
必须加入积分限幅（Anti-windup），防止位置误差长期累积导致积分饱和。
速度环的微分项需加低通滤波，防止编码器噪声被放大。
编码器精度与分辨率：双环控制的精度直接依赖于编码器分辨率。分辨率过低会导致位置环量化误差大，影响跟踪精度。建议选用每转≥1000线的增量式编码器或绝对值磁编码器（如TLE5012B，15bit分辨率）。编码器安装必须严格对中，机械间隙（backlash）会引入非线性误差，需通过预紧或软件补偿消除。
电源隔离与EMC防护：BLDC电机启停时电流冲击极大，严禁与Arduino及CAN收发器共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容（1000~4700μF）吸收反电动势。CAN信号线需使用屏蔽双绞线，与动力线间距≥50mm。电机三相线建议套屏蔽网，减少电磁辐射对编码器和CAN总线的干扰。
主从互搏与容错机制：
防互搏：刚性连接的多电机系统必须采用"主轴速度环+从轴扭矩环"策略，或引入虚拟弹簧-阻尼模型实现柔性跟随。
通讯超时保护：从节点设定超时阈值（如500ms未收到主节点SYNC帧），自动进入安全停机模式，防止失联后失控。
过流/过温保护：驱动板需具备硬件过流保护（阈值≤2倍额定电流），电机温度≥80℃时自动降额或停机。
急停机制：通过CAN总线高优先级帧或硬件急停按钮直接切断所有电机驱动电源，安全逻辑优先级最高。
时钟同步：主从控制器的内部时钟需同步，以确保状态信息的时间戳一致。可通过CAN总线周期性广播时间戳帧，从节点据此校准本地时钟，避免力与位置的融合计算产生误差。
前馈补偿与轨迹平滑：单纯的双环PID反馈控制在快速轨迹跟踪时存在滞后。建议引入前馈控制——将轨迹规划得到的速度、加速度前馈到控制回路中，动态补偿系统惯性。轨迹规划采用S形速度曲线（S-curve），确保加速度连续无突变，避免机械谐振。

1、CAN指令转发式双轴同步（VESC方案）
场景：使用两块VESC驱动器分别控制两台BLDC电机，Arduino作为上位机通过UART连接主VESC，主VESC再通过CAN总线将指令转发给从VESC，实现双轴速度/电流同步。

核心逻辑：这是一种“指令转发”架构。Arduino将控制指令同时发送给主VESC和通过CAN连接的从VESC。主VESC直接执行，从VESC收到转发的指令后执行，从而实现同步。

#include <VescUart.h>

VescUart VESCUART;
#define SLAVE_CAN_ID 4  // 从VESC的CAN ID

const int potPin = A1;

void setup() {
  Serial1.begin(115200);  // UART与VESC通信
  VESCUART.setSerialPort(&Serial1);
}

void loop() {
  // 1. 读取输入（如电位器），映射为电流指令
  int raw = analogRead(potPin);
  float currentCmd = map(raw, 0, 1023, -21, 21); 

  // 2. 同时发送给主VESC和从VESC
  VESCUART.setCurrent(currentCmd);          // 发给主VESC（ID默认0）
  VESCUART.setCurrent(currentCmd, SLAVE_CAN_ID); // 通过CAN转发给从VESC

  delay(10);
}

要点说明：该方案的简洁性在于利用了VESC内置的CAN转发功能，Arduino无需直接处理CAN协议栈。但缺点是同步精度受限于“Arduino→主VESC→从VESC”的链路延迟。

2、基于MCP2515的自定义CAN同步帧（主从双环控制）
场景：使用Arduino Mega作为主站，多个Arduino Uno作为从站，通过MCP2515 CAN模块构建分布式控制系统。主站运行双环PID，从站接收同步帧后跟随主站的目标位置与速度。

核心逻辑：主站将自身的目标位置和目标速度封装为CAN数据帧广播出去；从站收到后将其作为自身PID控制器的设定值，从而实现跟随。这是典型的“设定值广播”模式。

参考代码框架：

主站（Arduino Mega）代码：

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

MCP2515 canBus(CAN_CS);
// 双环PID：位置环输出作为速度环目标
PID posPID(&posError, &speedTarget, &targetPos, Kp_p, Ki_p, Kd_p);
PID speedPID(&speedError, &pwmOut, &speedTarget, Kp_s, Ki_s, Kd_s);

void loop() {
  // 1. 双环控制计算
  posError = targetPos - currentPos;
  posPID.Compute();
  speedPID.Compute();

  // 2. 发送CAN同步帧：将目标位置和速度打包为8字节
  struct can_frame syncFrame;
  syncFrame.can_id  = 0x100;  // 同步帧ID
  syncFrame.can_dlc = 8;
  syncFrame.data[0] = (uint8_t)(targetPos >> 8);
  syncFrame.data[1] = (uint8_t)(targetPos & 0xFF);
  syncFrame.data[2] = (uint8_t)(speedTarget >> 8);
  syncFrame.data[3] = (uint8_t)(speedTarget & 0xFF);
  canBus.sendMessage(&syncFrame);

  // 3. 接收从站反馈（可选校验）
  if (canBus.readMessage(&rxFrame) == MCP2515::ERROR_OK) {
    // 解析从站实际位置，计算同步误差
  }
  delay(10);
}

从站（Arduino Uno）代码：

#include <mcp2515.h>
#include <PID_v2.h>

MCP2515 canBus(CAN_CS);
volatile float syncPosTarget = 0, syncSpeedTarget = 0;
PID slavePosPID(&slavePosErr, &slaveSpeedRef, &syncPosTarget, Kp_p, Ki_p, Kd_p);
PID slaveSpeedPID(&slaveSpeedErr, &pwmOut, &slaveSpeedRef, Kp_s, Ki_s, Kd_s);

void loop() {
  if (canBus.readMessage(&rxFrame) == MCP2515::ERROR_OK) {
    if (rxFrame.can_id == 0x100) {
      // 解析同步数据，更新PID设定值
      syncPosTarget = (rxFrame.data[0] << 8) | rxFrame.data[1];
      syncSpeedTarget = (rxFrame.data[2] << 8) | rxFrame.data[3];
    }
  }
  // 双环控制
  slavePosErr = syncPosTarget - currentPos;
  slavePosPID.Compute();  // 输出到速度环
  slaveSpeedPID.Compute();
  // 输出PWM驱动电机...
}

3、自定义CAN协议包（位置、速度、PID参数一体化下发）
场景：对同步精度和响应要求较高的场景（如机器人关节），需要在一个CAN帧中同时下发位置指令、速度前馈以及PID参数（Kp/Kd），让从站本地完成高带宽控制计算。

核心逻辑：借鉴高精度电机驱动器的CAN协议设计，将16位位置指令、12位速度指令、12位Kp和12位Kd打包进一个8字节CAN帧。从站收到后解码并立即执行。

参考代码框架：

CAN协议定义（位打包逻辑）：

/// CAN命令包结构 (8字节) ///
/// 0: 位置[15:8]    1: 位置[7:0]
/// 2: 速度[11:4]    3: 速度[3:0] + Kp[11:8]
/// 4: Kp[7:0]       5: Kd[11:4]
/// 6: Kd[3:0] + Torque_ff[11:8]  7: Torque_ff[7:0]
/// 来源: 参考AK80电机CAN协议和论文中的位打包方法 [citation:3][citation:9]

打包函数（主站使用）：

// 浮点数转定点整数（位数可配置）
uint32_t float_to_uint(float x, float x_min, float x_max, uint8_t bits) {
  float span = x_max - x_min;
  return (uint32_t)((x - x_min) * ((float)((1 << bits) - 1)) / span);
}

void packCANCommand(uint8_t* buf, float pos, float vel, float kp, float kd, float torque) {
  // 限幅与转换
  uint16_t p_int = float_to_uint(pos, -4*PI, 4*PI, 16);
  uint16_t v_int = float_to_uint(vel, -30, 30, 12);
  uint16_t kp_int = float_to_uint(kp, 0, 500, 12);
  uint16_t kd_int = float_to_uint(kd, 0, 100, 12);
  uint16_t t_int = float_to_uint(torque, -18, 18, 12);

  // 按位打包 [citation:9]
  buf[0] = p_int >> 8;
  buf[1] = p_int & 0xFF;
  buf[2] = v_int >> 4;
  buf[3] = ((v_int & 0xF) << 4) | (kp_int >> 8);
  buf[4] = kp_int & 0xFF;
  buf[5] = kd_int >> 4;
  buf[6] = ((kd_int & 0xF) << 4) | (t_int >> 8);
  buf[7] = t_int & 0xFF;
}

从站接收与执行：
从站收到CAN帧后，按相同规则拆包，将提取出的pos作为位置环设定值，vel作为速度前馈，kp/kd动态更新PID参数，然后运行本地双环控制。

要点解读
CAN总线是远距离多轴同步的“最优解”：对于电机间线缆长度超过50cm的场景，I2C和SPI因抗干扰能力差、线缆长度受限而不适用。CAN总线采用差分信号，支持1Mbps速率和数十米通信距离，且天然支持多主多从架构，非常适合分布式电机控制系统。

同步精度取决于通信架构，而非单纯速度：真正的同步需要“同时性”。简单的“指令转发”（案例一）存在链路延迟。更优的方案是“设定值广播”（案例二）——主站将目标值同时发给所有从站，从站本地闭环执行，这消除了串行通信带来的相位滞后。

打包效率决定控制带宽：CAN帧每次最多8字节有效负载。为了在有限的带宽内传递位置、速度、力矩及PID参数，必须采用位打包技术（案例三），将多个不同位宽的数据紧凑排列。例如16位位置+12位速度+12位Kp+12位Kd刚好塞满8字节。

通信与控制“松耦合”设计：CAN通信不应阻塞控制主循环。在从站设计中，CAN接收中断仅负责更新共享变量（如syncPosTarget），主循环则高速运行本地PID。这种松耦合设计保证了即使在通信抖动或丢包时，电机仍能按最后一帧指令稳定运行，提高了系统鲁棒性。

同步误差校验机制：主站通常采用“下发指令 + 周期读取反馈”的模式。通过比较下发位置与从站反馈位置，若误差超过阈值，主站可下发修正指令。此机制要求从站必须回复包含当前位置、速度、电流等状态信息的CAN帧。

4、工业流水线物料精准传送（主从多轴同步传送带）
适用场景：汽车零部件装配流水线中，3条传送带需同步运行，主传送带按预设位置曲线运动（如曲线上料），从传送带需实时跟踪主带的位置和速度，确保物料无偏差传递（例如发动机缸体在多段传送带间的精准交接）。

核心需求：
主节点（主传送带）：产生动态位置目标，通过CAN总线广播；
从节点（从传送带）：接收主节点的“位置目标+速度目标”，通过位置外环（PI控制，消除稳态误差）+ 速度内环（PI控制，快速响应速度波动），实现从带与主带的位置、速度完全同步；
通信保障：CAN总线传输位置、速度数据，从节点实时反馈自身位置给主节点，实现闭环监控。

/* ===== 工业流水线主从同步：CAN总线位置/速度双环跟随 =====
 * 主节点：产生位置/速度目标，广播至从节点，接收从节点反馈
 * 通信协议：CAN ID=0x10（广播目标），ID=0x20（从节点反馈）
 * 控制核心：主节点开环目标生成，从节点位置/速度双环PI控制
 */

// === 主节点代码（主传送带控制器） ===
#include <SPI.h>
#include <mcp_can.h>
#include <SimpleFOC.h>

// CAN控制器初始化
MCP_CAN CAN(SPI_CS);
const long CAN_BUS_SPEED = 500000; // 500kbps，工业标准速率

// BLDC主电机配置
BLDCMotor motor(3);  // 电机引脚按实际调整
BLDCDriver3PWM driver(9, 10, 11);
Encoder enc(2, 4); // 增量式编码器，用于计算主电机位置/速度

// 位置/速度目标参数
volatile float master_position_target = 0.0;
volatile float master_speed_target = 5.0; // 主带目标速度（rad/s）
float target_accel = 0.1; // 加速度（rad/s²）

// CAN数据帧格式：数据区8字节
// 字节0-3：位置目标（float，4字节）
// 字节4-7：速度目标（float，4字节）

void setup() {
  Serial.begin(115200);
  // CAN总线初始化
  if (CAN.begin(CAN_BUS_SPEED) == MCP2515_OK) {
    Serial.println("CAN初始化成功");
    CAN.setMode(MCP2515_MODE_NORMAL); // 正常模式
  } else {
    Serial.println("CAN初始化失败");
    while (1);
  }
  // BLDC电机初始化
  motor.linkDriver(&driver);
  motor.linkSensor(&enc);
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();
}

void loop() {
  motor.loopFOC();
  // 1. 生成位置/速度目标（正弦曲线运动，模拟流水线变速需求）
  static unsigned long timer = millis();
  master_speed_target = 5.0 * sin(millis() / 1000.0); // 正弦速度变化
  master_position_target += master_speed_target * 0.01; // 积分得位置

  // 2. 通过CAN总线广播目标（ID=0x10）
  byte can_data[8];
  memcpy(can_data, &master_position_target, sizeof(float));
  memcpy(can_data + 4, &master_speed_target, sizeof(float));
  CAN.sendMsgBuf(0x10, 0, 8, can_data); // 广播ID=0x10

  // 3. 监控从节点反馈（ID=0x20，从节点1的反馈）
  byte len;
  byte data[8];
  if (CAN.checkReceive() == MCP2515_OK) {
    CAN.readMsgBuf(&len, data);
    unsigned long id = CAN.getCanId();
    if (id == 0x20) {
      float slave1_position;
      float slave1_speed;
      memcpy(&slave1_position, data, 4);
      memcpy(&slave1_speed, data + 4, 4);
      Serial.print("从节点1反馈-位置:"); Serial.print(slave1_position);
      Serial.print(" 速度:"); Serial.println(slave1_speed);
    }
  }

  delay(10); // 10ms更新周期，满足实时性
}

// === 从节点代码（从传送带控制器，2台相同） ===
#include <SPI.h>
#include <mcp_can.h>
#include <SimpleFOC.h>

MCP_CAN CAN(SPI_CS);
const long CAN_BUS_SPEED = 500000;

// BLDC从电机配置
BLDCMotor motor(3);
BLDCDriver3PWM driver(9, 10, 11);
Encoder enc(2, 4);

// 双环控制参数（位置外环PI + 速度内环PI）
float pos_kp = 0.5;   // 位置环比例系数
float pos_ki = 0.1;   // 位置环积分系数
float speed_kp = 2.0; // 速度环比例系数
float speed_ki = 0.5; // 速度环积分系数
float pos_integral = 0.0;  // 位置环积分值
float speed_integral = 0.0;// 速度环积分值

// 目标值与反馈值
volatile float target_position = 0.0;
volatile float target_speed = 0.0;
volatile float current_position = 0.0;
volatile float current_speed = 0.0;

void setup() {
  Serial.begin(115200);
  if (CAN.begin(CAN_BUS_SPEED) != MCP2515_OK) {
    Serial.println("CAN初始化失败");
    while (1);
  }
  // 配置CAN接收过滤器：仅接收ID=0x10（主节点广播）
  CAN.setFilterMask(0, true, 0x1FFF0000, 0x1FFF0000);
  CAN.setFilter(0, true, 0x10, 0x1FFF0000);
  CAN.setMode(MCP2515_MODE_NORMAL);

  // BLDC电机初始化
  motor.linkDriver(&driver);
  motor.linkSensor(&enc);
  motor.controller = MotionControlType::velocity; // 速度内环控制，外环位置环输出速度给定
  motor.init();
  motor.initFOC();
}

void loop() {
  motor.loopFOC();
  // 1. 读取主节点目标（ID=0x10）
  byte len;
  byte data[8];
  if (CAN.checkReceive() == MCP2515_OK) {
    CAN.readMsgBuf(&len, data);
    unsigned long id = CAN.getCanId();
    if (id == 0x10) {
      memcpy(&target_position, data, 4);
      memcpy(&target_speed, data + 4, 4);
    }
  }

  // 2. 计算反馈值（编码器获取当前位置、当前速度）
  current_position += enc.getPulses() * 360.0 / 1024.0; // 假设1024线编码器，转换为角度
  current_speed = enc.getVelocityPulses(); // 编码器测速（pulses/s）
  current_speed = current_speed * 60.0 / 1024.0 * 4; // 转换为rad/s（简化计算）

  // 3. 【核心】位置/速度双环控制
  // 位置外环：输出速度给定
  float pos_error = target_position - current_position;
  pos_integral += pos_error * 0.01; // 积分时间=0.01s（对应loop的10ms周期）
  float speed_given = pos_kp * pos_error + pos_ki * pos_integral;
  // 速度内环：PI控制输出占空比
  float speed_error = speed_given - current_speed;
  speed_integral += speed_error * 0.01;
  float duty_cycle = speed_kp * speed_error + speed_ki * speed_integral;
  // 限幅（避免PWM溢出）
  duty_cycle = constrain(duty_cycle, -100.0, 100.0);

  // 4. 输出控制信号到BLDC电机
  motor.move(duty_cycle);

  // 5. 从节点反馈自身位置/速度给主节点（ID=0x20，从节点1用0x20，从节点2用0x21）
  byte feedback_data[8];
  memcpy(feedback_data, &current_position, 4);
  memcpy(feedback_data + 4, &current_speed, 4);
  CAN.sendMsgBuf(0x20, 0, 8, feedback_data); // 从节点1反馈ID=0x20

  delay(10);
}

5、机械臂末端执行器同步（6轴机械臂末端从轴跟随）
适用场景：6轴协作机械臂的末端需安装打磨/焊接工具（由从轴BLDC驱动），主关节（如第6关节）的位置/速度即为末端从轴的目标，从轴需实时同步主关节的运动，确保末端工具与主关节姿态一致（例如航空发动机叶片的同步打磨，避免工具与工件错位）。

核心需求：
主节点（机械臂第6关节控制器）：输出末端的位置/速度目标（高精度，编码器分辨率达16位）；
从节点（末端工具驱动轴）：通过CAN总线接收目标，双环控制实现位置/速度同步，同步误差≤0.01°（避免打磨/焊接偏差）；
控制精度：编码器分辨率提升，参数调整（位置环kp/ki增大，提升跟踪精度）。

/* ===== 机械臂末端同步：CAN总线位置/速度双环跟随 =====
 * 核心优化：16位编码器分辨率+高速CAN(1Mbps)+高精度PI参数
 * 通信协议：ID=0x11（主节点广播末端目标），ID=0x21（从节点反馈）
 */

// === 主节点（机械臂第6关节控制器） ===
#include <SPI.h>
#include <mcp_can.h>
#include <SimpleFOC.h>

MCP_CAN CAN(SPI_CS);
const long CAN_BUS_SPEED = 1000000; // 1Mbps高速CAN，降低延迟

// 高精度BLDC（带16位绝对值编码器）
BLDCMotor motor(3);
BLDCDriver3PWM driver(9, 10, 11);
// 假设用AS5048绝对值编码器（16位，0-65535对应0-360°）
float current_pos = 0.0;
float current_speed = 0.0;

// 末端目标生成：基于关节运动学计算（此处简化为关节位置直接作为末端目标）
float calculate_end_effector_target(float joint6_pos) {
  return joint6_pos; // 简化：末端与关节6同角度
}

void setup() {
  Serial.begin(115200);
  if (CAN.begin(CAN_BUS_SPEED) != MCP2515_OK) while(1);
  CAN.setMode(MCP2515_MODE_NORMAL);

  // 电机初始化（绝对值编码器需额外初始化，此处简化为增量式接口兼容）
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();
}

void loop() {
  motor.loopFOC();

  // 1. 读取关节6位置（绝对值编码器，转换为角度）
  int enc_raw = analogRead(A0); // 假设AS5048接A0（实际用SPI接口）
  current_pos = (enc_raw / 65535.0) * 360.0; // 0-360°
  current_speed = 0.5; // 简化为恒定速度，实际从关节运动学获取

  // 2. 生成末端目标
  float end_pos_target = calculate_end_effector_target(current_pos);

  // 3. CAN广播目标（ID=0x11）
  byte data[8];
  memcpy(data, &end_pos_target, 4);
  memcpy(data+4, &current_speed, 4);
  CAN.sendMsgBuf(0x11, 0, 8, data);

  // 4. 接收从节点反馈（ID=0x21）
  byte len, recv_data[8];
  if (CAN.checkReceive() == MCP2515_OK) {
    CAN.readMsgBuf(&len, recv_data);
    if (CAN.getCanId() == 0x21) {
      float fb_pos, fb_speed;
      memcpy(&fb_pos, recv_data, 4);
      memcpy(&fb_speed, recv_data+4, 4);
      Serial.print("末端从轴同步误差:"); Serial.print(end_pos_target - fb_pos);
      Serial.println("°");
    }
  }

  delay(5); // 5ms周期，满足机械臂高速运动需求
}

// === 从节点（末端工具驱动轴） ===
#include <SPI.h>
#include <mcp_can.h>
#include <SimpleFOC.h>

MCP_CAN CAN(SPI_CS);
const long CAN_BUS_SPEED = 1000000;

BLDCMotor motor(3);
BLDCDriver3PWM driver(9, 10, 11);
// 16位绝对值编码器（AS5048）
int enc_raw = 0;
float current_pos = 0.0;
float current_speed = 0.0;

// 高精度双环参数（提升跟踪精度）
float pos_kp = 2.0;   // 位置环比例系数增大，提升响应
float pos_ki = 0.5;   // 积分系数增大，消除微小稳态误差
float speed_kp = 5.0; // 速度环比例系数增大，快速响应速度波动
float speed_ki = 1.0;
float pos_integral = 0.0;
float speed_integral = 0.0;

void setup() {
  Serial.begin(115200);
  if (CAN.begin(CAN_BUS_SPEED) != MCP2515_OK) while(1);
  // 接收ID=0x11的主节点数据
  CAN.setFilter(0, true, 0x11, 0x1FFF0000);
  CAN.setMode(MCP2515_MODE_NORMAL);

  motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();
}

void loop() {
  motor.loopFOC();

  // 1. 接收主节点目标（ID=0x11）
  byte len, data[8];
  if (CAN.checkReceive() == MCP2515_OK) {
    CAN.readMsgBuf(&len, data);
    if (CAN.getCanId() == 0x11) {
      memcpy(&target_position, data, 4);
      memcpy(&target_speed, data+4, 4);
    }
  }

  // 2. 高精度反馈计算（16位编码器）
  enc_raw = analogRead(A0); // 实际用SPI读取AS5048，此处简化
  current_pos = (enc_raw / 65535.0) * 360.0;
  // 速度计算：连续两次位置差/时间
  static float last_pos = 0.0;
  static unsigned long last_time = millis();
  current_speed = (current_pos - last_pos) / ((millis() - last_time) / 1000.0);
  last_pos = current_pos;
  last_time = millis();

  // 3. 高精度双环控制
  float pos_error = target_position - current_pos;
  // 防止积分饱和
  if (pos_error > 10.0) pos_integral = 0.0;
  if (pos_error < -10.0) pos_integral = 0.0;
  pos_integral += pos_error * 0.005; // 5ms积分时间
  float speed_given = pos_kp * pos_error + pos_ki * pos_integral;

  float speed_error = speed_given - current_speed;
  if (speed_error > 5.0) speed_integral = 0.0;
  if (speed_error < -5.0) speed_integral = 0.0;
  speed_integral += speed_error * 0.005;
  float duty_cycle = speed_kp * speed_error + speed_ki * speed_integral;
  duty_cycle = constrain(duty_cycle, -100.0, 100.0);

  motor.move(duty_cycle);

  // 4. 反馈（ID=0x21）
  byte fb_data[8];
  memcpy(fb_data, &current_pos, 4);
  memcpy(fb_data+4, &current_speed, 4);
  CAN.sendMsgBuf(0x21, 0, 8, fb_data);

  delay(5);
}

6、物流AGV集群分拣协同（主从多AGV同步分拣）
适用场景：物流仓库分拣系统中，1台主AGV负责定位分拣目标（如包裹位置），3台从AGV负责跟随主AGV的位置/速度，同步到达指定分拣位，协同完成分拣（例如主AGV定位包裹，从AGV抓取包裹并移至分拣格）。

核心需求：
主节点（主AGV）：融合GPS/激光SLAM定位，输出自身位置/速度作为目标，通过CAN总线广播；
从节点（从AGV）：接收主节点目标，双环控制实现同步跟随，且支持越界保护（当主节点数据丢失或从AGV偏离过大时，自动减速停车）；
协同逻辑：主AGV到达分拣点时，发送“触发指令”，从AGV同步触发抓取动作。

/* ===== 物流AGV集群分拣：CAN总线位置/速度双环跟随 =====
 * 核心功能：主AGV定位广播+从AGV同步跟随+越界保护+分拣触发
 * 通信协议：ID=0x12（主AGV目标），ID=0x22-0x24（从AGV反馈），ID=0x13（触发指令）
 */

// === 主AGV节点（定位+目标广播+触发指令） ===
#include <SPI.h>
#include <mcp_can.h>
#include <SimpleFOC.h>

MCP_CAN CAN(SPI_CS);
const long CAN_BUS_SPEED = 500000;

BLDCMotor motor(3);
BLDCDriver3PWM driver(9, 10, 11);
Encoder enc(2, 4);

// 定位参数（简化：用编码器模拟GPS/SLAM定位，实际替换为真实定位数据）
float main_agv_x = 0.0; // 主AGV X坐标（m）
float main_agv_y = 0.0; // 主AGV Y坐标（m）
float main_agv_speed = 0.0; // 主AGV速度（m/s）

// 分拣点触发条件：主AGV到达目标分拣点（X=10.0m, Y=5.0m）
const float target_x = 10.0;
const float target_y = 5.0;

void setup() {
  Serial.begin(115200);
  if (CAN.begin(CAN_BUS_SPEED) != MCP2515_OK) while(1);
  CAN.setMode(MCP2515_MODE_NORMAL);

  motor.linkDriver(&driver);
  motor.linkSensor(&enc);
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();
}

void loop() {
  motor.loopFOC();

  // 1. 模拟定位数据（实际替换为GPS/SLAM解析的坐标）
  static unsigned long t = millis();
  main_agv_x = 0.1 * t / 1000.0; // 沿X轴匀速移动，速度0.1m/s
  main_agv_speed = 0.1;

  // 2. 广播目标（ID=0x12，坐标+速度，8字节：X(4)+Y(4)，速度通过编码器间接获取）
  byte data[8];
  memcpy(data, &main_agv_x, 4);
  memcpy(data+4, &main_agv_y, 4);
  CAN.sendMsgBuf(0x12, 0, 8, data);

  // 3. 检测是否到达分拣点，发送触发指令（ID=0x13）
  if (abs(main_agv_x - target_x) < 0.1 && abs(main_agv_y - target_y) < 0.1) {
    Serial.println("到达分拣点，发送触发指令");
    CAN.sendMsgBuf(0x13, 0, 0, NULL); // 触发指令：无数据，仅ID标识
    delay(1000); // 保持触发指令1秒
  }

  // 4. 接收从AGV反馈（ID=0x22/0x23/0x24）
  byte len, recv_data[8];
  while (CAN.checkReceive() == MCP2515_OK) {
    CAN.readMsgBuf(&len, recv_data);
    unsigned long id = CAN.getCanId();
    float fb_x, fb_y;
    memcpy(&fb_x, recv_data, 4);
    memcpy(&fb_y, recv_data+4, 4);
    Serial.print("从AGV"); Serial.print(id - 0x20); 
    Serial.print("反馈坐标:("); Serial.print(fb_x); Serial.print(","); Serial.print(fb_y); Serial.println(")");
  }

  delay(10);
}

// === 从AGV节点（同步跟随+越界保护+触发执行） ===
#include <SPI.h>
#include <mcp_can.h>
#include <SimpleFOC.h>

MCP_CAN CAN(SPI_CS);
const long CAN_BUS_SPEED = 500000;

BLDCMotor motor(3);
BLDCDriver3PWM driver(9, 10, 11);
Encoder enc(2, 4);

// 双环控制参数
float pos_kp = 0.3;
float pos_ki = 0.05;
float speed_kp = 1.5;
float speed_ki = 0.3;
float pos_integral = 0.0;
float speed_integral = 0.0;

// 同步与保护参数
volatile float target_x = 0.0, target_y = 0.0;
volatile float current_x = 0.0, current_y = 0.0;
volatile float current_speed = 0.0;
bool trigger_active = false; // 分拣触发标志
const float max_deviation = 0.5; // 最大允许偏差（m），超过则触发保护

void setup() {
  Serial.begin(115200);
  if (CAN.begin(CAN_BUS_SPEED) != MCP2515_OK) while(1);
  // 接收主AGV目标（ID=0x12）和触发指令（ID=0x13）
  CAN.setFilter(0, true, 0x12, 0x1FFF0000);
  CAN.setFilter(1, true, 0x13, 0x1FFF0000);
  CAN.setMode(MCP2515_MODE_NORMAL);

  motor.linkDriver(&driver);
  motor.linkSensor(&enc);
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();
  pinMode(12, OUTPUT); // 分拣执行器（如机械爪）
}

void loop() {
  motor.loopFOC();

  // 1. 接收CAN数据
  byte len, data[8];
  if (CAN.checkReceive() == MCP2515_OK) {
    CAN.readMsgBuf(&len, data);
    unsigned long id = CAN.getCanId();
    if (id == 0x12) { // 目标坐标
      memcpy(&target_x, data, 4);
      memcpy(&target_y, data+4, 4);
    } else if (id == 0x13) { // 分拣触发指令
      trigger_active = true;
      Serial.println("收到分拣触发指令");
    }
  }

  // 2. 从AGV定位（编码器+简单积分，实际结合激光雷达）
  float delta_pulses = enc.getPulses();
  current_x += delta_pulses * 0.01; // 假设1脉冲=0.01m，沿X轴
  current_speed = delta_pulses / 0.01; // 0.01s采样周期，计算速度（m/s）

  // 3. 越界保护：检测是否偏离主AGV超过阈值
  float deviation = sqrt(pow(target_x - current_x, 2) + pow(target_y - current_y, 2));
  if (deviation > max_deviation) {
    Serial.println("偏差过大，启动保护：减速停车");
    motor.move(0.0); // 停车
    // 可加入蜂鸣器报警
    delay(1000);
    return; // 停止后续控制，等待人工干预或数据恢复
  }

  // 4. 双环控制：计算目标速度（跟踪主AGV的X方向速度）
  float pos_error = target_x - current_x;
  pos_integral += pos_error * 0.01;
  float speed_given = pos_kp * pos_error + pos_ki * pos_integral;

  float speed_error = speed_given - current_speed;
  speed_integral += speed_error * 0.01;
  float duty_cycle = speed_kp * speed_error + speed_ki * speed_integral;
  duty_cycle = constrain(duty_cycle, -50.0, 50.0); // 限幅，适应AGV低速场景
  motor.move(duty_cycle);

  // 5. 执行分拣动作
  if (trigger_active) {
    digitalWrite(12, HIGH); // 启动机械爪抓取
    Serial.println("执行分拣动作");
    delay(500);
    digitalWrite(12, LOW);
    trigger_active = false;
  }

  // 6. 反馈自身坐标（ID=0x22，从AGV1用0x22，从AGV2用0x23，从AGV3用0x24）
  byte fb_data[8];
  memcpy(fb_data, &current_x, 4);
  memcpy(fb_data+4, &current_y, 4);
  CAN.sendMsgBuf(0x22, 0, 8, fb_data); // 从AGV1反馈ID=0x22

  delay(10);
}

要点解读

1. CAN总线通信的“实时性与可靠性”设计：多轴同步的通信基础
   CAN总线是多轴同步的“神经中枢”，核心要点在于解决低延迟通信与高可靠传输的平衡，直接决定同步精度：
   波特率与延迟匹配：根据场景需求选择波特率——工业场景（传送带）用500kbps，高速场景（机械臂）用1Mbps，通信延迟可控制在1ms以内，远优于串口（10ms级），满足位置/速度的实时跟踪需求；
   数据帧格式精简：采用8字节数据帧，避免大数据量传输（如仅传位置/速度2个float，刚好8字节），减少单帧传输时间，同时避免总线拥堵；
   通信容错机制：①硬件过滤：用CAN控制器的过滤器（如MCP2515）仅接收目标ID数据，避免无关数据干扰；②数据校验：虽案例未显式添加，实际需在数据帧末尾加入CRC校验（或自定义奇偶校验），防止数据传输错误；③超时处理：从节点若连续3个周期未收到主节点数据，自动触发保护（如案例3的越界保护），防止失控。
2. 位置/速度双环控制的核心逻辑：“外环精度+内环响应”的协同
   双环控制是实现高精度同步的关键，核心是内外环分工明确、参数匹配，避免单一环路的缺陷：
   环路分工：①位置外环：以目标位置与当前位置的偏差为输入，输出速度给定值，核心作用是消除稳态误差，确保从节点最终跟踪到主节点的目标位置（案例4/5/6均用PI控制，积分项消除静差）；②速度内环：以速度给定与当前速度的偏差为输入，输出电机驱动占空比，核心作用是快速响应速度波动（如负载突变、地面阻力变化），提升动态跟踪能力，采用PI控制可实现速度的无静差跟踪；
   参数匹配原则：速度内环的带宽需远高于位置外环（一般3-5倍），确保速度环能快速跟随外环给定，因此速度环的kp/ki需大于位置环（如案例1中速度kp=2、位置kp=0.5），避免外环输出的变化速度超过内环响应能力，导致超调或振荡；
   积分饱和抑制：在双环的积分项中加入限幅（如案例2/3中对积分值清零的条件），避免偏差过大时积分项累积导致输出饱和，无法及时响应反向偏差，提升控制的动态性能。
3. 主从同步的“一致性与鲁棒性”：应对复杂环境的保障
   主从同步的核心是主节点目标可靠、从节点跟踪鲁棒，同时应对环境干扰（如负载波动、通信丢包）：
   主节点目标生成逻辑：主节点的目标需贴合场景需求——案例4的正弦曲线（模拟流水线变速）、案例5的关节运动学解算（模拟机械臂末端姿态）、案例6的SLAM定位（模拟AGV定位），需保证目标的连续性与合理性，避免突变目标导致从节点跟踪超调；
   从节点反馈与同步修正：从节点需实时反馈自身位置/速度给主节点，主节点可通过反馈判断同步误差，调整目标生成（如案例2中根据反馈误差调整机械臂关节运动），形成全局闭环，提升同步精度；
   鲁棒性保护机制：①越界保护：案例6中设置最大允许偏差，超过则停车报警，避免从AGV因通信丢失或定位误差失控；②数据有效性校验：从节点接收目标时，判断偏差是否在合理范围（如目标位置与当前位置的差值在0-100cm），过滤无效目标；③负载自适应：可通过编码器实时监测电流，当电流突增（负载突变）时，自动增大速度环kp，提升抗扰能力。
4. BLDC电机的“闭环驱动适配”：同步控制的执行保障
   BLDC电机是执行单元，其驱动的精度与闭环适配能力直接决定双环控制的效果，核心要点围绕传感器反馈、控制模式匹配：
   传感器与位置/速度计算匹配：①位置反馈：采用增量式编码器（案例4/6）或绝对值编码器（案例5），增量式编码器通过累计脉冲计算位置，成本低；绝对值编码器直接输出当前位置，断电不丢失，适合高精度场景（如机械臂）；②速度计算：用编码器的脉冲频率法（单位时间内的脉冲数），避免用位置差分的噪声干扰，提升速度反馈的平滑性；
   控制模式与双环匹配：BLDC的控制模式需匹配双环架构——案例中均设置为MotionControlType::velocity（速度模式），因为速度内环的输出直接对应速度给定，电机在该模式下快速跟踪速度给定，而位置外环的输出作为速度给定，实现“位置→速度→执行”的闭环链路；若用位置模式，需电机内置位置环，灵活性较差，难以适配主从同步的动态目标变化；
   驱动参数整定：电机的PI参数需通过实际调试优化，如先整定速度内环（仅速度闭环，给阶跃速度信号，调整kp使响应快且无超调，再调整ki消除静差），再整定位置外环（将位置环输出接入速度环，给阶跃位置信号，调整kp/ki使位置跟踪无静差、无超调），避免参数盲目套用。
5. 多场景的“参数与逻辑适配”：从理论到落地的关键
   不同场景的环境约束、精度需求差异大，核心是参数与逻辑的场景化调整，而非直接复用代码：
   工业流水线（案例4）：负载平稳，同步误差允许±5mm，参数无需过高，速度内环kp=2、位置环kp=0.5即可，重点是通信的实时性（500kbps）；
   机械臂末端（案例5）：负载波动大、精度要求高（±0.01°），需采用16位绝对值编码器提升反馈精度，同时增大双环PI参数（位置kp=2、速度kp=5），提升跟踪精度，通信采用1Mbps高速CAN，降低延迟；
   物流AGV（案例6）：环境动态性强（有障碍物、地面不平），需加入越界保护、超时保护，双环参数适当减小（位置kp=0.3、速度kp=1.5），避免因环境干扰导致超调，同时加入分拣触发逻辑，实现任务协同；
   核心适配逻辑：场景的精度需求决定编码器选型与PI参数，场景的动态性决定保护逻辑，场景的协同需求决定通信协议与触发机制，代码需“按需裁剪”，而非机械复用。

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