基于 Arduino 的无刷直流电机（BLDC）关节伺服系统，通过引入编码器进行位置反馈，是实现高精度运动控制的核心技术。该系统构建了一个闭环控制回路，使电机能够精确、稳定地响应控制指令，广泛应用于对位置和速度有严苛要求的场合。

🛠️ 主要特点
高精度位置感知与全速域稳定性
编码器作为系统的“眼睛”，提供了远超传统传感器的观测精度。
高分辨率： 相较于仅提供粗略位置的霍尔传感器，编码器（尤其是增量式光学或磁性编码器）能提供每转数百至数千脉冲（PPR）的高分辨率信号。通过 Arduino 对信号进行四倍频处理，可进一步提升角度分辨率，实现亚度级甚至更高精度的位置闭环控制。
全速域稳定： 编码器直接测量转子的机械位置，不受电机反电动势大小的限制。这意味着系统在零速或极低速下仍能准确获取位置信息，实现平稳启动和精确停位，克服了无感控制在低速时性能下降的固有缺陷。
支持先进控制算法与多模态运行
编码器反馈不仅是位置闭环的基础，更是实现高级控制策略的前提。
磁场定向控制（FOC）： 高精度的角度信息是实现 FOC 算法的必要条件。FOC 通过坐标变换，将电机电流解耦为控制磁场和控制转矩的两个分量，从而实现像直流电机一样平滑、安静、高效的控制，极大降低了转矩脉动，这对于需要平稳输出的关节伺服至关重要。
多模态控制： 基于同一套编码器反馈信号，系统可灵活切换控制模式。通过对位置信号求导可实现速度闭环控制，或在位置控制模式下实现精确的轨迹跟踪。
强大的抗干扰能力与动态响应
闭环架构赋予了系统对外部扰动的抵抗能力。
动态纠偏： 当系统受到外部负载扰动导致实际位置偏离目标时，控制器（Arduino）能根据实时反馈的位置误差，迅速调整输出（如 PWM 占空比或目标电流），驱动电机消除误差，保证了系统的稳定性和动态响应速度。
处理器选型： 为了处理高频的编码器信号和复杂的控制算法（如 FOC），通常需要选用性能更强的 Arduino 平台，如 Teensy 4.x 或 Arduino Due，它们具备更高的主频、硬件浮点运算单元和专用的编码器接口库。

🏭 应用场景
该技术方案凭借其高精度和高可靠性，成为众多精密机电系统的核心驱动单元：
机器人关节驱动：
在工业机械臂、人形机器人或四足机器人中，每个关节都需要精确的位置和力矩控制，以保证末端执行器的定位精度和运动的柔顺性。
精密云台与稳定系统：
用于无人机、车载或手持相机的云台，需要极低延迟和高精度的角度控制来实时抵消抖动，保持画面稳定。
数控机床与 3D 打印：
在 CNC 机床的进给轴或 3D 打印机的喷头运动机构中，该系统能确保加工路径和打印轨迹的精确度，提升成品质量。
医疗设备与自动化仪器：
在手术机器人、诊断设备或自动化移液工作站中，高可靠性的关节伺服是保障操作精准和患者安全的关键。

⚠️ 注意事项
实现一个高性能的伺服系统是一项复杂的系统工程，需重点关注以下挑战：
控制算法的复杂性与算力需求
FOC 算法门槛： 实现高性能的 FOC 控制算法复杂，对处理器的算力要求高。开发者需要深入理解坐标变换（Clarke/Park 变换）和 PID 调节技术。
硬件选型： 必须根据控制环路的更新频率要求，选择具备足够处理能力和定时器资源的 Arduino 开发板，避免因算力不足导致控制延迟或失稳。
编码器信号处理与抗干扰
信号完整性： 在电机运行产生的强电磁干扰环境下，高频的编码器信号极易受到噪声影响。必须使用屏蔽线，并尽量缩短信号线长度，必要时进行硬件滤波。
中断资源管理： 为了不丢失高速脉冲，Arduino 通常需要利用外部中断或输入捕获功能来读取编码器信号，这会占用宝贵的硬件资源，需在程序设计时合理规划。
电源管理与热设计
电源隔离： BLDC 电机启停时的大电流会对 Arduino 控制板供电造成冲击。务必使用独立的隔离电源模块为控制电路和电机电路供电，并做好共地处理，防止电机噪声导致控制板复位。
散热设计： 在长时间高负载运行或电机堵转情况下，驱动器和电机本体都会产生大量热量。必须设计足够的散热片，并在软件中加入过温保护逻辑。
机械安装与系统安全
安装同轴度： 编码器通常安装在电机轴或负载端，其安装的同轴度和牢固性直接影响反馈精度，微小的机械偏差都会被反馈回路放大，导致系统振荡。
安全冗余： 必须设计硬件限位开关和软件限位保护，防止程序错误导致关节超程损坏机械结构。同时应设置电流或力矩上限，防止电机堵转烧毁。

1、基础位置闭环控制（PID算法）

#include <Encoder.h>
#define ENC_A 2    // 编码器A相
#define ENC_B 3    // 编码器B相
#define PWM_PIN 9  // 电机PWM输出
#define DIR_PIN 8  // 电机方向控制

Encoder myEnc(ENC_A, ENC_B);
long targetPos = 0;  // 目标位置（编码器脉冲数）
float Kp = 1.2, Ki = 0.5, Kd = 0.1;  // PID参数
float integral = 0, prevError = 0;
unsigned long lastTime = 0;

void setup() {
  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  Serial.begin(115200);
}

void loop() {
  unsigned long now = millis();
  if (now - lastTime >= 10) {  // 100Hz控制周期
    long currentPos = myEnc.read();  // 读取编码器值
    float error = targetPos - currentPos;
    
    // PID计算
    integral += error * 0.1;  // 积分项（简化时间单位）
    float derivative = (error - prevError) / 0.1;
    float output = Kp * error + Ki * integral + Kd * derivative;
    prevError = error;

    // 输出限幅与方向控制
    output = constrain(output, -255, 255);
    analogWrite(PWM_PIN, abs(output));
    digitalWrite(DIR_PIN, (output > 0) ? HIGH : LOW);

    Serial.print("Pos: "); Serial.print(currentPos);
    Serial.print(" Output: "); Serial.println(output);
    lastTime = now;
  }
}

2、速度-位置双环控制（FOC简化版）

#include <SimpleFOC.h>
BLDCMotor motor(7);  // 电机极对数
BLDCDriver3PWM driver(9, 10, 11, 8);  // PWM引脚 + 使能
Encoder encoder(2, 3, 500);  // 500 PPR编码器

float targetVel = 0.0;  // 目标速度（rad/s）
float targetPos = 0.0;  // 目标位置（rad）

void setup() {
  Serial.begin(115200);
  encoder.init();
  motor.linkSensor(&encoder);
  driver.init();
  motor.linkDriver(&driver);

  // 速度环PID
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 1.0;
  motor.PID_velocity.D = 0.01;
  motor.controller = MotionControlType::velocity_position;  // 双环模式

  motor.initFOC();
}

void loop() {
  motor.loopFOC();  // FOC电流环（1kHz）
  motor.move(targetVel);  // 速度环（100Hz）

  // 位置环（外层，10Hz）
  static float lastPos = 0;
  float currentPos = encoder.getMechanicalAngle();
  float error = targetPos - currentPos;
  targetVel = error * 5.0;  // 简单比例前馈
  targetVel = constrain(targetVel, -10.0, 10.0);

  Serial.print("Pos: "); Serial.print(currentPos);
  Serial.print(" Vel: "); Serial.println(targetVel);
  delay(100);
}

3、多关节协同控制（时间序列触发）

#include <SimpleFOC.h>
#define NUM_JOINTS 3
BLDCMotor joints[NUM_JOINTS] = {BLDCMotor(7), BLDCMotor(8), BLDCMotor(9)};
Encoder encoders[NUM_JOINTS] = {Encoder(2, 3, 1000), Encoder(4, 5, 1000), Encoder(6, 7, 1000)};

struct JointCommand {
  int id;
  float targetPos;
  unsigned long startTime;
  unsigned long duration;
};

JointCommand trajectory[] = {
  {0, 1.57, 0, 1000},    // 关节0：90°用时1秒
  {1, -0.785, 500, 800}, // 关节1：-45°延迟0.5秒，用时0.8秒
  {2, 3.14, 1000, 1200}  // 关节2：180°延迟1秒，用时1.2秒
};

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < NUM_JOINTS; i++) {
    encoders[i].init();
    joints[i].linkSensor(&encoders[i]);
    joints[i].init();
  }
}

void loop() {
  unsigned long now = millis();
  for (int i = 0; i < NUM_JOINTS; i++) {
    JointCommand cmd = trajectory[i];
    if (now >= cmd.startTime && now < cmd.startTime + cmd.duration) {
      float progress = (now - cmd.startTime) / (float)cmd.duration;
      float currentPos = encoders[cmd.id].getMechanicalAngle();
      float targetPos = cmd.targetPos * progress;  // 线性插值
      
      // 简单P控制
      float error = targetPos - currentPos;
      float pwm = error * 50.0;
      pwm = constrain(pwm, -255, 255);
      // 实际需通过驱动器输出（此处简化）
      Serial.print("Joint "); Serial.print(cmd.id);
      Serial.print(" Pos: "); Serial.print(currentPos);
      Serial.print(" Target: "); Serial.println(targetPos);
    }
  }
  delay(10);
}

要点解读
编码器分辨率与信号处理
高分辨率编码器（如1000 PPR）需通过四倍频技术提升至4000计数/转，配合硬件计数器（如Arduino Due的QDEC模块）避免脉冲丢失。
信号线需采用屏蔽双绞线，远离电机动力线，必要时加施密特触发器整形。
PID参数整定策略
P项：过大导致超调，过小响应迟缓。建议从Kp=0.5开始逐步增加。
I项：消除静差，但需限制积分饱和（如添加if (error < threshold) integral += error）。
D项：抑制振荡，但对编码器噪声敏感，可加入低通滤波（如derivative *= 0.7）。
实时性保障
控制周期需稳定（如1-10ms），使用硬件定时器中断（如Timer1.initialize(1000)）替代delay()。
避免在中断服务程序（ISR）中执行复杂计算，仅更新计数器，主循环处理PID逻辑。
多关节协同控制
通过时间序列或运动学逆解分配各关节目标位置，需考虑机械耦合效应。
示例3中采用线性插值生成平滑轨迹，实际应用可替换为S曲线或贝塞尔曲线。
硬件选型与电气设计
主控板：标准Arduino Uno仅适合低速场景，推荐使用Teensy 4.1（Cortex-M7）或ESP32。
驱动器：需支持FOC算法（如SimpleFOC兼容板卡），或自行设计三相逆变电路。
电源隔离：电机电源与逻辑电源共地但独立稳压，编码器VCC加10μF+100nF电容滤波。
应用场景扩展
工业机械臂：案例2的双环控制可扩展至6轴，结合逆运动学实现末端轨迹跟踪。
人形机器人：案例3的协同控制可用于腿部摆动相与支撑相的切换。
CNC机床：通过编码器反馈实现主轴速度闭环与进给轴位置闭环的同步控制。

4、工业机械臂关节位置伺服（点到点高精度定位）
场景定位
工业机械臂的关节级点到点（PTP）定位，例如流水线物料抓取工位，需BLDC关节快速响应指令，精准停在目标角度，重复定位精度要求±0.1°以内，需实现位置闭环伺服控制。

核心控制逻辑
Arduino接收上位机下发的目标角度指令，通过CAN总线发送给VESC驱动板；
VESC驱动板读取编码器反馈，在底层执行电流环+速度环，Arduino从VESC读取实际角度，在上层执行位置环PID，动态调整位置误差，实现闭环伺服；
实时判断误差小于阈值，稳定定位后输出到位信号，供上位机触发抓取动作。

// 工业机械臂关节位置伺服代码（Arduino Mega + VESC CAN通信）
#include <CAN.h>                 // CAN通信库
#include <PID_v1.h>              // PID控制库

// ========== 硬件与通信配置 ==========
#define CAN_BAUD_RATE 500000      // CAN通信波特率（VESC默认500k）
#define VESC_ID 0x01              // VESC驱动板的CAN节点ID（需提前配置）
#define MOTOR_ENCODER_CPR 8000     // 编码器每转脉冲数（VESC解码后等效值，VESC默认将2000线编码器等效为8000CPR）
#define MAX_ANGLE 360.0            // 关节最大旋转角度（度）

// ========== PID参数（位置环，需现场调试） ==========
double Kp = 2.5, Ki = 0.8, Kd = 0.3;  // 比例、积分、微分系数
double targetAngle = 0.0;            // 目标角度（度）
double actualAngle = 0.0;            // 实际反馈角度（度）
double positionError = 0.0;          // 位置误差
PID positionPID(&targetAngle, &actualAngle, &positionError, Kp, Ki, Kd, DIRECT);

// ========== 全局变量 ==========
unsigned long lastCANUpdate = 0;
const unsigned long CAN_PERIOD = 20; // 20ms更新一次CAN数据（50Hz频率）
bool isTargetReached = false;        // 是否到达目标位置

// ========== 函数声明 ==========
void setupCAN();
void sendTargetAngle(float angle);
void readActualAngle();
void pidControl();
void checkReachedCondition();

void setup() {
  Serial.begin(115200);
  Serial.println("机械臂关节位置伺服初始化...");

  setupCAN();
  positionPID.SetMode(AUTOMATIC);  // 开启PID自动模式
  positionPID.SetOutputLimits(-50, 50); // PID输出为速度指令，范围-50~50rad/s（需匹配VESC参数）

  Serial.println("初始化完成，等待目标角度指令（串口输入，格式：目标角度，单位：度）");
}

void loop() {
  1. 串口接收目标角度（上位机/手动输入）
  if (Serial.available() > 0) {
    targetAngle = Serial.parseFloat();
    isTargetReached = false;
    Serial.print("接收目标角度：");
    Serial.println(targetAngle);
  }

  2. 执行PID控制，更新指令
  pidControl();

  3. 读取实际角度反馈
  if (millis() - lastCANUpdate >= CAN_PERIOD) {
    lastCANUpdate = millis();
    readActualAngle();
  }

  4. 判断是否到达目标位置
  checkReachedCondition();
}

// ========== 初始化CAN通信 ==========
void setupCAN() {
  CAN.begin(CAN_BAUD_RATE);
  while (!CAN.setup(CAN_BAUD_RATE)) {
    Serial.println("CAN初始化失败，重试...");
    delay(1000);
  }
  Serial.println("CAN通信初始化成功（波特率500k）");
}

// ========== 发送目标角度到VESC（简化CAN指令，实际需遵循VESC CAN协议） ==========
void sendTargetAngle(float angle) {
  // VESC CAN协议：位置指令为「目标位置」，单位：脉冲数（需转换为角度对应的脉冲）
  int anglePulse = (int)(angle / MAX_ANGLE * MOTOR_ENCODER_CPR);
  anglePulse = constrain(anglePulse, 0, MOTOR_ENCODER_CPR);

  // CAN数据帧：ID=VESC_ID，数据为4字节脉冲数（高字节在前）
  byte data[4] = {
    (byte)(anglePulse >> 24),
    (byte)(anglePulse >> 16),
    (byte)(anglePulse >> 8),
    (byte)anglePulse
  };
  CAN.send(VESC_ID, data, 4);
}

// ========== 读取VESC反馈的实际角度 ==========
void readActualAngle() {
  if (CAN.available()) {
    byte id = CAN.getReceivedId();
    byte len = CAN.getReceivedLength();
    byte* data = CAN.getReceivedData();

    if (id == VESC_ID && len >= 4) {
      int anglePulse = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
      actualAngle = (anglePulse / (float)MOTOR_ENCODER_CPR) * MAX_ANGLE;
      actualAngle = fmod(actualAngle, MAX_ANGLE); // 处理角度溢出（0~360循环）
    }
  }
}

// ========== PID闭环控制（核心：计算输出并发送目标速度） ==========
void pidControl() {
  // 1. 计算PID误差并输出
  positionPID.Compute();

  // 2. 将PID输出（位置修正后的速度指令）发送给VESC（实际为速度环输入）
  // 注：VESC速度环指令单位为rpm，需转换：rpm = (速度rad/s * 60) / (2*PI)
  float targetSpeedRPM = (positionError * 60) / (2 * PI);
  targetSpeedRPM = constrain(targetSpeedRPM, -3000, 3000); // 限幅

  // 发送速度指令（简化CAN速度指令，实际需按VESC协议）
  byte speedData[4] = {
    (byte)(targetSpeedRPM >> 24),
    (byte)(targetSpeedRPM >> 16),
    (byte)(targetSpeedRPM >> 8),
    (byte)targetSpeedRPM
  };
  CAN.send(VESC_ID, speedData, 4);
}

// ========== 判断是否到达目标位置（误差<0.1°视为到位） ==========
void checkReachedCondition() {
  float absError = fabs(targetAngle - actualAngle);
  // 处理角度跨越0°/360°的误差（例如目标359°，实际1°，误差为2°）
  if (absError > 180) {
    absError = 360 - absError;
  }

  if (absError <= 0.1 && !isTargetReached) {
    isTargetReached = true;
    positionPID.SetMode(MANUAL); // 到达后关闭PID，停止输出
    Serial.println("目标位置到达！当前实际角度：" + String(actualAngle) + "°");
    // 发送到位信号给上位机（可选，通过CAN或串口）
  }
}

5、AGV舵轮速度伺服（稳定跟踪路径速度）
场景定位
AGV（自动导引车）的舵轮速度伺服控制，需BLDC舵轮按上位机下发的目标速度稳定转动，配合编码器反馈实时调节速度，确保AGV在转弯、直行时速度平稳，速度跟踪误差≤±2%，满足路径跟踪精度要求。

核心控制逻辑
AGV车载控制器通过CAN总线下发目标速度指令（单位：rpm）；
Arduino从CAN读取目标速度，同时从C620驱动板读取编码器反馈的实际转速；
执行速度环PID，计算输出电流/电压指令，修正速度误差，实现速度稳定跟踪；
实时监控速度误差，超差时输出报警，确保AGV行驶平稳。

// AGV舵轮速度伺服代码（Arduino Uno + C620驱动板）
#include <CAN.h>
#include <PID_v1.h>

// ========== 硬件与通信配置 ==========
#define CAN_BAUD_RATE 250000      // C620默认CAN波特率250k
#define DRIVER_ID 0x02            // C620驱动板的CAN节点ID
#define TARGET_SPEED_MAX 1000     // 最大目标速度（rpm）

// ========== 速度PID参数（需根据负载调试） ==========
double Kp = 1.2, Ki = 0.3, Kd = 0.05;
double targetSpeed = 0.0;        // 目标速度（rpm）
double actualSpeed = 0.0;        // 实际速度（rpm）
double speedError = 0.0;         // 速度误差
PID speedPID(&targetSpeed, &actualSpeed, &speedError, Kp, Ki, Kd, DIRECT);

// ========== 全局变量 ==========
unsigned long lastCANLoop = 0;
const unsigned long CONTROL_PERIOD = 10; // 10ms控制周期（100Hz）
bool isSpeedStable = false;
float currentSpeedError = 0.0;

// ========== 函数声明 ==========
void setupCAN();
void readTargetSpeed();
void readActualSpeed();
void pidSpeedControl();
void checkSpeedError();

void setup() {
  Serial.begin(115200);
  Serial.println("AGV舵轮速度伺服初始化...");

  setupCAN();
  speedPID.SetMode(AUTOMATIC);
  speedPID.SetOutputLimits(-20, 20); // PID输出为电流指令，范围-20~20A（匹配C620参数）

  Serial.println("初始化完成，等待速度指令（CAN总线接收，单位：rpm）");
}

void loop() {
  if (millis() - lastCANLoop >= CONTROL_PERIOD) {
    lastCANLoop = millis();

    1. 读取目标速度（AGV控制器下发）
    readTargetSpeed();

    2. 读取实际速度反馈（C620读取编码器数据）
    readActualSpeed();

    3. 执行速度PID控制
    pidSpeedControl();

    4. 监控速度误差，超差报警
    checkSpeedError();
  }
}

// ========== 初始化CAN（适配C620驱动板） ==========
void setupCAN() {
  CAN.begin(CAN_BAUD_RATE);
  while (!CAN.setup(CAN_BAUD_RATE)) {
    Serial.println("C620 CAN初始化失败，重试...");
    delay(500);
  }
  Serial.println("C620驱动板CAN通信初始化成功");
}

// ========== 从CAN读取目标速度（AGV控制器下发，2字节整数，单位rpm） ==========
void readTargetSpeed() {
  if (CAN.available()) {
    byte id = CAN.getReceivedId();
    byte len = CAN.getReceivedLength();
    byte* data = CAN.getReceivedData();

    if (id == DRIVER_ID && len >= 2) {
      targetSpeed = (data[0] << 8) | data[1]; // 高字节在前，转换为rpm
      targetSpeed = constrain(targetSpeed, -TARGET_SPEED_MAX, TARGET_SPEED_MAX);
      isSpeedStable = false;
    }
  }
}

// ========== 从C620读取实际速度（编码器反馈，2字节整数，单位rpm） ==========
void readActualSpeed() {
  // 发送请求实际速度的CAN指令（简化：按C620协议请求速度反馈）
  byte reqData[2] = {0x01, 0x03}; // 模拟请求指令，具体按C620手册
  CAN.send(DRIVER_ID, reqData, 2);
  delay(2); // 等待驱动板响应

  if (CAN.available()) {
    byte id = CAN.getReceivedId();
    byte len = CAN.getReceivedLength();
    byte* data = CAN.getReceivedData();

    if (id == DRIVER_ID && len >= 2) {
      actualSpeed = (data[0] << 8) | data[1];
      currentSpeedError = fabs(targetSpeed - actualSpeed);
    }
  }
}

// ========== 速度PID控制（输出电流指令修正速度） ==========
void pidSpeedControl() {
  speedPID.Compute();

  // 将PID输出（电流指令）发送给C620（实际为电流环输入）
  int currentOutput = (int)constrain(speedError, -20, 20);
  byte currentData[2] = {(byte)(currentOutput >> 8), (byte)currentOutput};
  CAN.send(DRIVER_ID, currentData, 2);
}

// ========== 监控速度误差，超差报警 ==========
void checkSpeedError() {
  // 判断速度是否稳定（误差<2%视为稳定）
  float errorPercent = (currentSpeedError / max(abs(targetSpeed), 1.0)) * 100;
  if (errorPercent <= 2.0 && !isSpeedStable) {
    isSpeedStable = true;
    Serial.print("速度稳定！目标速度：");
    Serial.print(targetSpeed);
    Serial.print("rpm，实际速度：");
    Serial.print(actualSpeed);
    Serial.print("rpm，误差：");
    Serial.println(errorPercent, 2);
  } else if (errorPercent > 5.0) {
    Serial.println("警告：速度误差超限（>5%）！当前误差：");
    Serial.println(errorPercent, 2);
    digitalWrite(13, HIGH); // 点亮板载LED报警
  } else {
    digitalWrite(13, LOW); // 误差正常，关闭报警灯
  }
}

6、柔性协作机器人关节力矩/位置复合伺服（安全人机协作）
场景定位
柔性协作机器人关节，需实现力矩控制+位置控制的复合伺服，例如人机协作装配场景，既要精准到达目标角度，又要在遇到碰撞时通过编码器反馈的力矩信号快速回退，避免伤人，需实现“位置伺服+力矩限制+碰撞回退”的复合功能。

核心控制逻辑
正常运行时，执行位置伺服，控制关节精准到达目标角度；
当力矩传感器检测到碰撞力矩超过阈值，立即切换为力矩回退模式，基于编码器反馈的位置信号，执行反向位置伺服，让关节回退避开碰撞；
碰撞解除后，自动切换回位置伺服模式，继续执行任务，确保人机安全协作。

// 柔性协作机器人关节复合伺服代码（Arduino Nano + J-MA100驱动板）
#include <CAN.h>
#include <PID_v1.h>

// ========== 硬件与通信配置 ==========
#define CAN_BAUD_RATE 500000
#define DRIVER_ID 0x03
#define TORQUE_THRESHOLD 5.0    // 碰撞力矩阈值（N·m，需校准）
#define POSITION_ERROR_LIMIT 0.5 // 位置到位误差限（度）

// ========== 控制模式参数 ==========
enum ServoMode {POSITION_MODE, TORQUE_RETREAT_MODE};
ServoMode currentMode = POSITION_MODE;

// 位置PID参数
double Kp_pos = 1.8, Ki_pos = 0.5, Kd_pos = 0.2;
double targetPos = 0.0, actualPos = 0.0, posError = 0.0;
PID posPID(&targetPos, &actualPos, &posError, Kp_pos, Ki_pos, Kd_pos, DIRECT);

// 回退PID参数
double Kp_ret = 2.0, Ki_ret = 0.6, Kd_ret = 0.3;
double retreatPos = 0.0, currentPos = 0.0, retError = 0.0;
PID retPID(&retreatPos, &currentPos, &retError, Kp_ret, Ki_ret, Kd_ret, DIRECT);

// ========== 全局变量 ==========
unsigned long lastLoop = 0;
const unsigned long CYCLE_PERIOD = 15; // 15ms控制周期
float torqueSensorValue = 0.0;
bool isCollision = false;

// ========== 函数声明 ==========
void setupCAN();
void readTargetPosition();
void readActualPosition();
void readTorqueSensor();
void positionServoControl();
void torqueRetreatControl();
void switchControlMode();

void setup() {
  Serial.begin(115200);
  Serial.println("柔性协作关节复合伺服初始化...");

  pinMode(A0, INPUT); // 力矩传感器模拟输入
  setupCAN();

  posPID.SetMode(AUTOMATIC);
  posPID.SetOutputLimits(-30, 30);
  retPID.SetMode(AUTOMATIC);
  retPID.SetOutputLimits(-35, 35);

  // 初始化目标位置（示例：初始0°，目标90°）
  targetPos = 0.0;
  retreatPos = 0.0;

  Serial.println("初始化完成，当前模式：位置伺服");
}

void loop() {
  if (millis() - lastLoop >= CYCLE_PERIOD) {
    lastLoop = millis();

    1. 读取传感器数据（位置、力矩）
    readActualPosition();
    readTorqueSensor();

    2. 模式切换判断（碰撞检测）
    switchControlMode();

    3. 执行对应模式的控制
    if (currentMode == POSITION_MODE) {
      positionServoControl();
    } else if (currentMode == TORQUE_RETREAT_MODE) {
      torqueRetreatControl();
    }
  }
}

// ========== 初始化CAN（适配J-MA100驱动板） ==========
void setupCAN() {
  CAN.begin(CAN_BAUD_RATE);
  while (!CAN.setup(CAN_BAUD_RATE)) {
    Serial.println("J-MA100 CAN初始化失败，重试...");
    delay(500);
  }
  Serial.println("J-MA100驱动板CAN通信初始化成功");
}

// ========== 读取实际位置（J-MA100反馈，单位度） ==========
void readActualPosition() {
  // 发送位置反馈请求指令（按J-MA100协议）
  byte reqPos[2] = {0x02, 0x01};
  CAN.send(DRIVER_ID, reqPos, 2);
  delay(2);

  if (CAN.available()) {
    byte id = CAN.getReceivedId();
    byte len = CAN.getReceivedLength();
    byte* data = CAN.getReceivedData();

    if (id == DRIVER_ID && len >= 4) {
      // 转换为角度（假设4字节为浮点数，简化为整数示例）
      actualPos = (data[0] << 8) | data[1]; // 高字节在前，单位度
      currentPos = actualPos; // 回退模式时的当前位置
    }
  }
}

// ========== 读取力矩传感器（模拟输入，转换为力矩值N·m） ==========
void readTorqueSensor() {
  int adcValue = analogRead(A0);
  torqueSensorValue = (adcValue / 1023.0) * 10.0; // 转换为0~10N·m（需校准）
  isCollision = (torqueSensorValue >= TORQUE_THRESHOLD);
}

// ========== 切换控制模式（碰撞检测触发回退） ==========
void switchControlMode() {
  // 碰撞检测：力矩超阈值，且当前处于位置模式
  if (isCollision && currentMode == POSITION_MODE) {
    Serial.println("碰撞检测！切换为回退模式");
    currentMode = TORQUE_RETREAT_MODE;
    // 设置回退目标位置：当前位置回退20°
    retreatPos = currentPos - 20.0;
    if (retreatPos < 0) retreatPos += 360; // 处理角度溢出
    posPID.SetMode(MANUAL); // 关闭位置PID
  }

  // 回退完成判断：回退误差达标，且力矩恢复
  if (currentMode == TORQUE_RETREAT_MODE) {
    float retErrorAbs = fabs(retreatPos - currentPos);
    if (retErrorAbs <= POSITION_ERROR_LIMIT && torqueSensorValue < TORQUE_THRESHOLD) {
      Serial.println("碰撞解除，切换回位置模式");
      currentMode = POSITION_MODE;
      retPID.SetMode(MANUAL); // 关闭回退PID
      Serial.println("继续执行位置任务...");
    }
  }
}

// ========== 位置伺服控制（正常任务模式） ==========
void positionServoControl() {
  posPID.Compute();

  // 发送位置指令给J-MA100（4字节浮点数，单位度）
  byte posData[4] = {
    (byte)((int)posError >> 24),
    (byte)((int)posError >> 16),
    (byte)((int)posError >> 8),
    (byte)(int)posError
  };
  CAN.send(DRIVER_ID, posData, 4);

  // 判断位置到位（误差<0.5°）
  if (fabs(posError) <= POSITION_ERROR_LIMIT) {
    Serial.println("位置到位，任务完成！");
    posPID.SetMode(MANUAL);
  }
}

// ========== 力矩回退控制（碰撞规避模式） ==========
void torqueRetreatControl() {
  retPID.Compute();

  // 发送回退速度指令（负方向，回退）
  byte retData[4] = {
    (byte)((int)retError >> 24),
    (byte)((int)retError >> 16),
    (byte)((int)retError >> 8),
    (byte)(int)retError
  };
  CAN.send(DRIVER_ID, retData, 4);

  // 点亮报警LED（板载LED）
  digitalWrite(13, HIGH);
}

要点解读

1. 编码器选型与反馈信号的“适配性设计”：精度、接口与数据处理的关键
   编码器是闭环伺服的“眼睛”，选型与数据处理直接影响控制精度，需匹配驱动板接口与控制需求：
   编码器类型适配：增量式编码器成本低、响应快，需驱动板具备倍频功能（如VESC将2000线编码器等效为8000CPR），适合通用场景；绝对式编码器无零点丢失，适合断电重启无需回零的场景（如协作机器人），需驱动板支持绝对式接口；
   分辨率与控制精度匹配：编码器分辨率需满足控制精度要求，若要求关节定位精度±0.1°，电机减速比10:1，则编码器等效分辨率需≥3600CPR（360°×10/0.1°），否则会出现分辨率不足导致的定位震荡；
   数据转换规范：需统一编码器脉冲数与角度的换算关系，避免数据错位，同时需处理角度溢出问题，避免出现角度跳变。
2. 闭环控制算法的“参数整定与模式协同”：平衡响应速度与稳定性
   闭环算法是伺服控制的核心，需结合场景需求选择控制模式，并通过参数整定平衡响应速度与稳定性：
   控制模式按需选择：
   位置伺服优先选择“位置环+速度环+电流环”三环控制，驱动板底层执行速度/电流环，Arduino上层执行位置环，实现高精度定位；
   速度伺服采用“速度环+电流环”，重点保证速度稳定性，适合恒速跟踪场景；
   复合场景（如协作机器人）需实现模式切换（位置→力矩→位置），切换时需平滑过渡，避免控制突变；
   PID参数分环整定：先整定底层速度环PID，再整定上层位置环PID。整定步骤为：先设P=0，I=0，D=0，从小到大调P，直到出现轻微震荡，再调D抑制震荡，最后调I消除稳态误差，参数调试需结合示波器观察误差曲线，避免盲目套用经验值；
   积分限幅与抗饱和：为防止积分项累积过大导致输出超限，需对积分项进行限幅，避免出现积分饱和，导致系统响应迟缓或超调过大。
3. 硬件驱动与通信的“可靠性设计”：抗干扰、防丢包与故障应对
   BLDC伺服系统的稳定运行依赖可靠的硬件驱动与通信，需针对干扰、丢包等问题设计防护机制：
   通信接口抗干扰：CAN总线是BLDC伺服的主流通信方式，需在总线两端并联120Ω终端电阻，减少信号反射，同时通信线采用双绞屏蔽线，屏蔽层接地，降低电磁干扰。若环境干扰强，可在Arduino与驱动板之间增加CAN隔离模块，防止地线环路干扰；
   指令与反馈的防丢包：采用定时循环收发机制，确保指令与反馈不丢失，同时对关键指令（如紧急停止、碰撞回退）增加重发机制；
   驱动板参数匹配：需根据电机额定电压、电流、转速，配置驱动板的过流保护阈值、过压保护值、PWM频率，避免因参数不匹配导致电机过热、堵转烧毁，同时启用驱动板的过热保护，实时监控驱动板温度，超温时切断输出。
4. 安全机制与故障保护的“冗余设计”：极端场景下的可靠防护
   特种应用场景（如工业、协作机器人）需强化安全保护，避免因传感器异常、通信中断、电机故障导致事故：
   多重故障检测：实时检测编码器信号，若编码器断线或无反馈，立即停止输出并触发声光报警；监测驱动板温度，超温时切断电机输出并切换至安全模式；
   紧急停止机制：支持硬件紧急停止，一旦触发，立即切断所有电机输出，同时软件层面检测到碰撞、超限等故障时，也立即进入紧急停止状态，确保设备与人员安全；
   掉电与制动保护：对带电磁制动的BLDC，掉电时触发制动，防止关节自由坠落；上电初始化时，先解除制动，再启动电机，避免制动状态下电机堵转；
   角度限位与软件限幅：通过硬件限位开关限制关节最大旋转范围，软件层面对目标角度进行限幅，防止超限碰撞，同时对位置误差、速度误差进行限幅，避免PID输出过大导致电机飞车。
5. 工程化落地的“调试与优化策略”：从实验室到现场的适配技巧
   伺服系统从实验室验证到现场落地，需经过细致的调试与优化，适配现场工况与负载变化：
   先空载后加载调试：先在空载状态下整定PID参数，确保位置/速度响应无震荡、无稳态误差，再逐步增加负载，根据负载变化微调PID参数，避免负载突变导致控制失效；
   减速比与机械特性匹配：若电机带减速器，需将机械减速比纳入角度换算，同时需补偿减速器的传动间隙，在位置环PID中加入死区补偿或间隙补偿算法，避免机械间隙导致的定位误差或震荡；
   采样周期与系统稳定性：控制周期需根据系统动态特性选择，BLDC伺服系统一般选择10~20ms采样周期，周期过短会增加计算负担，导致响应滞后，过长则会导致控制精度下降、震荡，需通过实验确定最优周期；
   现场工况适配：针对现场振动、温度变化等环境因素，对编码器、电机安装进行加固，避免振动导致编码器信号失真，对驱动板、Arduino做防尘、散热处理，确保在极端环境下稳定运行。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。