在基于 Arduino 平台的 SCARA（选择性顺应装配机器人）BLDC 控制系统中，“恒力装配 + 位置补偿”是一种结合了动力学模型前馈与柔顺控制的高级架构。以下从专业视角为您详细解析其主要特点、应用场景及注意事项：
一、 主要特点
基于运动学模型的位置补偿（前馈控制）
纯反馈的 PID 控制器在应对重力等非线性效应时往往“慢半拍”。位置补偿的核心在于通过正/逆运动学模型，实时计算当前机械臂构型下各关节的重力矩，并将其作为前馈量叠加到 PID 的输出上。这种基于模型的前馈控制能极大地减少稳态误差，提升动态响应性能。
高精度的臂长与参数标定补偿
SCARA 机器人的末端平面坐标高度依赖两个机械臂的长度及旋转角度。通过控制机械臂运动至特定位置，结合外部高精度位移测定系统（如视觉系统）获取真实坐标，可以反推计算出机械臂的真实臂长和零点补偿值。将这些真实参数导入控制系统，能够显著消除制造和装配误差，提升绝对定位精度。
柔顺的恒力控制与扰动抑制
在装配场景中，系统需具备力矩控制能力。通过低侧电流采样或霍尔传感器获取相电流，结合 FOC（磁场定向控制）算法，可将目标力矩转化为 q 轴电流指令。同时，系统可引入扰动观测器（Disturbance Observer），通过电流与指令的残差实时估算外部接触力，从而实现柔顺的恒力装配，有效避免硬性碰撞。
平滑的轨迹规划与抗冲击设计
为防止电机全速启动产生巨大超调或机械冲击，系统绝对禁止阶跃指令。必须使用插补算法和轨迹规划器（如梯形速度曲线或 S 型曲线），在当前位置和目标位置之间生成带加速度约束的平滑曲线。这从根本上保证了装配过程的平稳与精确。
高频闭环与算力分层架构
恒力装配与位置补偿对实时性要求极高，控制环频率通常需 ≥1 kHz。鉴于标准 AVR Arduino 的算力瓶颈，系统通常采用分层架构：Arduino（或 ESP32/STM32）作为下位机负责高频的 FOC 电流环与速度环执行；上位机负责复杂的运动学解算、轨迹规划与力矩补偿指令的下发。
二、 应用场景
该架构主要适用于对装配精度、力控柔顺性及成本有平衡要求的桌面级或轻型自动化场景：
精密电子装配：如 PCB 板元器件的插件、微小螺丝的锁付、连接器插拔等，需严格限制插入力以防损坏针脚。
轻型工业拾放与涂胶：执行小零件的拾放、简单装配、涂胶等任务，这些场景负载轻、速度要求不高，关节空间 PID 配合重力补偿已能胜任。
医疗与康复辅助：在手术机器人或康复外骨骼原型中，恒力控制能使设备在执行复杂操作时保持平稳，实现人机安全交互与示教（Lead-through Teaching）。
教育与科研验证：作为学习机器人动力学、阻抗控制、柔顺装配等先进控制理论的理想低成本实验平台。
三、 需要注意的事项
传感器精度与安装位置
编码器必须尽量安装在关节输出端（减速器后），若仅装在电机轴，将无法感知减速器背隙和弹性变形，导致位置/力矩反馈失真。此外，电流采样的精度直接决定恒力控制的性能，需采用低温度系数采样电阻和差分放大器抑制噪声。
机械传动系统的背隙与刚性
齿轮箱或传动部件的背隙是定位精度的“杀手”，会导致换向时的死区和非线性误差。在腕部等高精度关节建议使用低背隙的谐波减速器，基座关节可酌情使用行星减速器，同时需避免连杆悬臂过长引发机械共振。
控制参数的整定顺序
多闭环系统的整定必须严格遵循“从内环到外环”的顺序：先整定好驱动器的电流环（力矩环）和速度环，再整定位置环。PID 参数需具备一定的鲁棒性，因为同一个关节在不同构型下的动态特性（如重力矩）是变化的。
算力瓶颈与实时性保障
实时进行逆运动学计算、重力补偿甚至动力学前馈，对计算能力是巨大考验。若使用标准 Arduino Uno/Mega 可能会力不从心，强烈推荐使用 Arduino Due 或基于 ARM Cortex-M 的 STM32、ESP32 等 32 位平台，并利用定时器中断严格调度控制循环。
安全机制与边界保护
恒力装配涉及物理接触，必须设置软硬件双重安全边界。软件上需实现力矩/速度软限幅、碰撞检测阈值及看门狗定时器；硬件上需配备独立的使能继电器和急停按钮，当检测到持续超差或过流时立即触发紧急制动。

1、基于末端力传感器的阻抗控制（力/位混合）
适用场景：精密零件插入、表面打磨等需要同时维持恒定接触力并跟随工件轮廓的任务。

#include <SimpleFOC.h>
#include <Wire.h>
#include <PID_v1.h>

// 假设通过I2C读取六维力传感器的Z轴力
#define FORCE_SENSOR_ADDR 0x40

// SCARA两个关节的BLDC电机
BLDCMotor motor1(7), motor2(7);
// 需补充Encoder和Driver初始化...

// 运动学参数
const float L1 = 10.0, L2 = 10.0;  // 连杆长度(cm)

// 力控PID参数
double targetForce = 0.5;      // 目标接触力(N)
double currentForce = 0;
double forceError = 0;
double positionCompensation = 0;
double Kp = 1.2, Ki = 0.05, Kd = 0.01;
PID forcePID(&currentForce, &positionCompensation, &targetForce, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  // 电机初始化（SimpleFOC角度控制模式）
  motor1.init(); motor2.init();
  motor1.initFOC(); motor2.initFOC();
  
  forcePID.SetMode(AUTOMATIC);
  forcePID.SetOutputLimits(-0.5, 0.5);  // 位置补偿幅度限制
}

void loop() {
  motor1.loopFOC(); motor2.loopFOC();
  
  // 1. 读取末端力（假设已经过滤波）
  currentForce = readForceSensor();
  
  // 2. 【核心】力PID计算位置补偿量
  forcePID.Compute();  // 输出positionCompensation
  
  // 3. 基础轨迹位置（例如：沿Y轴下降插入工件）
  float baseX = 5.0;
  float baseY = 8.0 - millis() * 0.001 * 0.5;  // 缓慢下降
  
  // 4. 【核心】力补偿叠加到位置指令
  float targetX = baseX;  // X方向保持不动
  float targetY = baseY + positionCompensation;  // Y方向根据力调整
  
  // 5. 逆运动学解算关节角度
  float theta1, theta2;
  inverseKinematics(targetX, targetY, theta1, theta2);
  
  // 6. BLDC执行
  motor1.move(theta1);
  motor2.move(theta2);
  
  // 调试输出
  Serial.print("Force: "); Serial.print(currentForce);
  Serial.print(" | Comp: "); Serial.println(positionCompensation);
  
  delay(10);  // 100Hz控制频率
}

// 逆运动学
void inverseKinematics(float x, float y, float &t1, float &t2) {
  float r = sqrt(x*x + y*y);
  t2 = acos((L1*L1 + L2*L2 - r*r) / (2*L1*L2));
  t1 = atan2(y, x) - atan2(L2*sin(t2), L1 + L2*cos(t2));
}

代码要点：本案例实现“阻抗控制”的基本框架。末端力偏差通过PID映射为位置修正量，再叠加到基础轨迹上。其本质是将力控制问题转化为位置控制问题，使机器人表现出“弹簧”般的柔顺特性。

2、基于电流环的力矩补偿（无传感器力控）
适用场景：成本敏感的装配场景，通过检测BLDC电机的电流变化间接感知末端负载，实现无需力传感器的恒力控制。

#include <SimpleFOC.h>
#include <PID_v1.h>

BLDCMotor motor1(7), motor2(7);
// 需补充Encoder和Driver初始化...

// 运动学参数
const float L1 = 10.0, L2 = 10.0;

// 【核心】将关节力矩映射到末端力的雅可比矩阵系数
const float JACOBIAN_SCALE = 0.1;

// 力矩PID
double targetTorque = 0.3;       // 目标关节力矩(N·m)
double currentTorque = 0;
double torqueCompensation = 0;   // 关节角度补偿量
double Kp_t = 1.0, Ki_t = 0.02, Kd_t = 0.01;
PID torquePID(&currentTorque, &torqueCompensation, &targetTorque, Kp_t, Ki_t, Kd_t, DIRECT);

void setup() {
  Serial.begin(115200);
  
  motor1.init(); motor2.init();
  motor1.initFOC(); motor2.initFOC();
  
  // 将电机设为力矩控制模式
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
  
  torquePID.SetMode(AUTOMATIC);
  torquePID.SetOutputLimits(-0.3, 0.3);
}

void loop() {
  motor1.loopFOC(); motor2.loopFOC();
  
  // 1. 【核心】从FOC获取力矩电流分量
  // motor.current_q 是q轴电流，正比于电磁力矩
  currentTorque = motor1.current_q * motor1.phase_resistance * JACOBIAN_SCALE;
  
  // 2. 力矩PID计算补偿量
  torquePID.Compute();
  
  // 3. 基础轨迹（沿Y方向缓慢下压）
  float baseY = 10.0 - millis() * 0.001 * 0.3;
  
  // 4. 力矩补偿→位置补偿（通过逆运动学）
  float targetX = 5.0;
  float targetY = baseY + torqueCompensation * 0.5;
  
  float theta1, theta2;
  inverseKinematics(targetX, targetY, theta1, theta2);
  
  // 5. 位置模式下执行
  motor1.controller = MotionControlType::angle;
  motor2.controller = MotionControlType::angle;
  motor1.move(theta1);
  motor2.move(theta2);
  
  // 6. 返回力矩模式准备下一轮
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
  
  Serial.print("Torque: "); Serial.print(currentTorque);
  Serial.print(" | Comp: "); Serial.println(torqueCompensation);
  
  delay(10);
}

代码要点：本案例利用BLDC电机的电流环作为“电子测力计”。在FOC算法中，motor.current_q代表力矩电流分量，通过监测其变化可间接判断末端负载，实现“无传感器的力控”。

3、多阶段阻抗切换（孔轴装配）
适用场景：典型的PCB插入连接器、轴承压装等需要根据装配进度切换力控策略的多阶段任务。

#include <SimpleFOC.h>
#include <PID_v1.h>

BLDCMotor motor1(7), motor2(7);
// 需补充Encoder和Driver初始化...

const float L1 = 10.0, L2 = 10.0;

// 装配阶段定义
enum AssemblyPhase {
  APPROACH,      // 接近阶段：快速移动
  CONTACT,       // 接触阶段：恒力寻位
  INSERT,        // 插入阶段：恒力下压
  DONE           // 完成
};

AssemblyPhase phase = APPROACH;
float targetForce = 0.0;

// 力控PID（位置补偿型）
double currentForce = 0, posComp = 0;
double Kp_f = 1.5, Ki_f = 0.05, Kd_f = 0.02;
PID forcePID(&currentForce, &posComp, &targetForce, Kp_f, Ki_f, Kd_f, DIRECT);

void setup() {
  Serial.begin(115200);
  motor1.init(); motor2.init();
  motor1.initFOC(); motor2.initFOC();
  forcePID.SetMode(AUTOMATIC);
  forcePID.SetOutputLimits(-0.8, 0.8);
}

void loop() {
  motor1.loopFOC(); motor2.loopFOC();
  
  currentForce = readForceSensor();
  
  // ==================== 1. 阶段检测 ====================
  if (phase == APPROACH) {
    // 快速移动到工件附近
    float targetX = 8.0;
    float targetY = 12.0 - millis() * 0.001 * 1.0;
    
    if (targetY < 4.0) {
      phase = CONTACT;
      targetForce = 0.3;  // 接触阶段目标力
      Serial.println("Phase: CONTACT");
    }
    moveTo(targetX, targetY);
    
  } else if (phase == CONTACT) {
    // 【核心】恒力寻位：在X方向微动搜索插入位置
    static float searchOffset = 0;
    float searchSpeed = 0.5;  // cm/s
    
    // 力PID计算位置补偿
    forcePID.Compute();
    
    // 基础Y位置（缓慢下压）
    float baseY = 4.0 - millis() * 0.001 * 0.2;
    // X方向来回搜索
    float targetX = 8.0 + sin(millis() * 0.001 * 2.0) * 0.5;
    float targetY = baseY + posComp;
    
    moveTo(targetX, targetY);
    
    // 检测是否进入插入阶段：力稳定且位置变化缓慢
    if (fabs(currentForce - targetForce) < 0.05 && millis() > 5000) {
      phase = INSERT;
      targetForce = 1.0;  // 插入阶段目标力增大
      Serial.println("Phase: INSERT");
    }
    
  } else if (phase == INSERT) {
    // 【核心】恒力插入：主要沿Y方向下压，X保持锁定
    static float insertStartY = 0;
    if (insertStartY == 0) insertStartY = getCurrentY();
    
    forcePID.Compute();
    float targetY = insertStartY - millis() * 0.001 * 0.3 + posComp * 0.5;
    float targetX = 8.0;  // X方向锁定
    
    moveTo(targetX, targetY);
    
    // 完成检测：Y下压超过阈值
    if (targetY < -2.0) {
      phase = DONE;
      Serial.println("Phase: DONE - Assembly Complete!");
    }
  }
  
  delay(10);
}

void moveTo(float x, float y) {
  float t1, t2;
  inverseKinematics(x, y, t1, t2);
  motor1.move(t1);
  motor2.move(t2);
}

代码要点：本案例展示了“多阶段阻抗控制”策略。根据装配进度动态切换目标力和运动模式：接近阶段快速定位→接触阶段恒力寻位→插入阶段稳定下压。这种策略可有效应对PCB插入连接器等典型的力控装配任务。

要点解读
恒力装配的底层逻辑是“力误差→位置修正”：三个案例均遵循“测力→比较→补偿位置”的闭环。不同于单纯的位置控制，力控系统允许末端根据接触力“软化”轨迹，使其成为一台“懂力”的柔顺机器人。

力传感器与电流环是两种“力感知”路径：案例一依赖末端力传感器（精度高但增加成本），案例二利用BLDC的current_q分量间接测力（成本低但精度受限于电机参数和摩擦力）。选择取决于应用场景对力控精度的要求。

阻抗控制的“目标阻抗”决定了力-位转换关系：实际力控系统中，力到位置的转换并非简单的PID，而是一个二阶系统的“目标阻抗”——包含质量、阻尼、刚度三个参数，分别决定力控的响应速度、振荡抑制和最终精度。

逆运动学是“力-位映射”的数学桥梁：末端位置修正量需要通过逆运动学转换为各关节的角度增量。SCARA机器人的逆运动学是闭式解析解，适合在Arduino上实时计算。

“多阶段策略”是力控任务成功的关键：案例三展示了一种更精细的方法——将复杂任务拆解为“接近-接触-插入”等阶段，不同阶段采用不同目标阻抗和运动策略。这有效避免了单一力控参数在不同工况下的“顾此失彼”。

4、电子元件精密装配系统（恒力按压+位置误差主动补偿）
适用场景：SCARA机器人装配0402微型贴片元件时，需以恒定压力按压元件至PCB焊盘，避免压力过大损坏元件或过小导致虚焊；同时实时补偿PCB定位偏差，确保元件精准贴合。

// 核心：力控恒压+位置补偿，适配Arduino Mega（多PWM+ADC通道）
#include <SimpleFOC.h>
#include <HX711.h> // 力传感器（应变式，接入ADC）
#include <Stepper.h> // Z轴精密升降（可选，辅助力控）

// SCARA关节电机定义（4个关节：J1旋转/J2摆臂/Z轴升降）
BLDCMotor jointJ1 = BLDCMotor(5);
BLDCMotor jointJ2 = BLDCMotor(6);
BLDCMotor axisZ = BLDCMotor(7); // Z轴装配执行轴
HX711 forceSensor; // 力传感器

// 恒力装配参数
const float TARGET_FORCE = 2.5;    // 目标装配力（N，适配0402元件）
const float FORCE_KP = 1.2;        // 力控比例系数
const float POS_COMP_KP = 0.8;     // 位置补偿比例系数
const float SAFE_FORCE = 5.0;      // 最大安全力（超限急停）
const int MAX_COMP_STEP = 10;      // 单次补偿最大角度（°）
const int FORCE_SAMPLE_NUM = 5;    // 力采样滤波次数

// 装配任务目标（初始标定值）
float targetX = 50.0;  // X轴目标位置（mm）
float targetY = 30.0;  // Y轴目标位置（mm）
float targetZ = 0.0;   // Z轴下压位置（mm，相对零点）
bool isPressing = false; // 是否处于按压状态

void setup() {
  Serial.begin(115200);
  // 初始化力传感器（接A2-A4，示例）
  forceSensor.begin(A2, A3, A4);
  forceSensor.set_scale(100.0); // 力传感器标定系数（需按实际校准）
  
  // 初始化关节电机
  jointJ1.init(); jointJ2.init(); axisZ.init();
  // 电机零点初始化（回到机械零点）
  goToHome();
  // 目标位置初始化（示例）
  moveToTarget(targetX, targetY, targetZ);
}

void loop() {
  // 1. 力采样与滤波：采集装配力，避免传感器噪声干扰
  float currentForce = 0;
  for(int i=0; i<FORCE_SAMPLE_NUM; i++) {
    currentForce += forceSensor.read();
    delay(2);
  }
  currentForce /= FORCE_SAMPLE_NUM;
  
  // 2. 力控恒压闭环：动态调整Z轴速度，维持目标力
  if(isPressing) {
    // 安全检测：超安全力立即停止
    if(currentForce > SAFE_FORCE) {
      axisZ.move(0);
      Serial.println("Force Overrun! Emergency Stop");
      return;
    }
    
    // 力控计算：力偏差→Z轴速度补偿
    float forceError = TARGET_FORCE - currentForce;
    float forceAdjust = FORCE_KP * forceError;
    // Z轴下压速度：力不足时加速，力过大时减速
    float zSpeed = map(forceAdjust, -3, 3, -50, 50); // 速度范围-50~50（相对值）
    zSpeed = constrain(zSpeed, -100, 100);
    axisZ.move(zSpeed);
    
    // 3. 位置补偿：实时采集力反馈，补偿PCB定位偏差（X/Y轴调整）
    if(abs(forceError) > 0.3) { // 力偏差过大说明位置偏移
      // 补偿逻辑：力偏差转化为位置偏差，驱动J1/J2修正
      float posXComp = forceError * POS_COMP_KP;
      float posYComp = forceError * POS_COMP_KP * 0.8; // 可根据实际调整权重
      
      // 映射为关节角度补偿（需结合SCARA正运动学，此处简化为线性映射）
      int j1Speed = map(posXComp, -2, 2, -30, 30);
      int j2Speed = map(posYComp, -2, 2, -30, 30);
      
      // 限幅补偿，避免超量程
      j1Speed = constrain(j1Speed, -MAX_COMP_STEP, MAX_COMP_STEP);
      j2Speed = constrain(j2Speed, -MAX_COMP_STEP, MAX_COMP_STEP);
      
      jointJ1.move(j1Speed);
      jointJ2.move(j2Speed);
      
      Serial.print("Pos Comp: J1="); Serial.print(j1Speed);
      Serial.print(" J2="); Serial.print(j2Speed);
      Serial.print(" Force="); Serial.println(currentForce);
    } else {
      // 力稳定，保持当前位置
      jointJ1.move(0);
      jointJ2.move(0);
    }
  }
  
  // 4. 任务触发：按下按钮或接收指令开始装配
  if(Serial.available() && Serial.read() == 's') {
    isPressing = true;
    Serial.println("Start Precision Assembly");
  }
  
  // 力达标判定：维持恒力100ms后完成装配
  if(isPressing && abs(currentForce - TARGET_FORCE) < 0.2) {
    static unsigned long stableTime = 0;
    if(stableTime == 0) stableTime = millis();
    if(millis() - stableTime > 100) {
      // 装配完成：抬起Z轴
      axisZ.move(-80); // 快速抬起
      isPressing = false;
      stableTime = 0;
      Serial.println("Assembly Complete");
    }
  } else {
    stableTime = 0;
  }
  
  delay(10); // 100Hz闭环周期，保障实时性
}

// 回机械零点（初始化）
void goToHome() {
  jointJ1.move(-30); // J1逆时针回零
  jointJ2.move(40);  // J2顺时针回零
  axisZ.move(100);   // Z轴抬起
  delay(1000);
  jointJ1.move(0);
  jointJ2.move(0);
  axisZ.move(0);
  Serial.println("Home Complete");
}

// 移动到目标位置（简化：仅示意，实际需正运动学计算）
void moveToTarget(float x, float y, float z) {
  // 计算J1/J2关节角度（正运动学，实际需反解）
  // 此处用目标位置映射速度，示意核心逻辑
  int j1Speed = map(x, 0, 100, 20, 80);
  int j2Speed = map(y, 0, 100, -40, 20);
  int zSpeed = map(z, 0, 50, 50, 0);
  
  jointJ1.move(j1Speed);
  jointJ2.move(j2Speed);
  axisZ.move(zSpeed);
  delay(800);
  jointJ1.move(0);
  jointJ2.move(0);
  axisZ.move(0);
  Serial.println("Move to Target");
}

核心逻辑：通过力传感器实时反馈装配力，PID调节Z轴下压速度维持恒力；力偏差超过阈值时，动态补偿X/Y轴位置，修正PCB定位偏差，确保元件精准贴合且压力恒定，适配精密电子元件装配的高精度、低损伤需求。

5、微型轴承压装系统（恒力压装+弹性工件位置补偿）
适用场景：SCARA机器人将微型轴承压入电机座孔时，轴承与座孔存在微小弹性变形，需以恒定压力推进轴承，同时补偿压装过程中的位置偏差（避免卡滞、倾斜），保障轴承顺利压装到位。

// 核心：恒力压装+弹性位置补偿，适配带负载力控的SCARA
#include <SimpleFOC.h>
#include <HX711.h>
#include <PID_v1.h>

// SCARA关节电机
BLDCMotor jointJ1 = BLDCMotor(5);
BLDCMotor jointJ2 = BLDCMotor(6);
BLDCMotor pressAxis = BLDCMotor(7); // 压装轴（Z轴）
HX711 loadCell; // 力传感器（推力检测）

// 压装参数
const float PRESS_FORCE = 30.0;    // 目标压装力（N，适配微型轴承）
const float FORCE_KP = 0.8;
const float FORCE_KI = 0.1;
const float FORCE_KD = 0.05;
const float ELASTIC_COMP_KP = 1.0; // 弹性工件位置补偿系数
const float MIN_POS_SPEED = 5;     // 最小位置补偿速度（避免抖动）
const int PRESS_DEPTH = 20;        // 压装深度（mm，需结合实际标定）
bool isPressing = false;
float currentDepth = 0; // 当前压装深度

// PID控制器（力闭环）
PID forcePID(&loadCellValue, &forceOutput, &forceSetpoint, FORCE_KP, FORCE_KI, FORCE_KD, DIRECT);
float loadCellValue = 0;
float forceOutput = 0;
float forceSetpoint = PRESS_FORCE;

void setup() {
  Serial.begin(115200);
  loadCell.begin(A2, A3, A4);
  loadCell.set_scale(50.0); // 推力传感器标定系数
  
  // 初始化电机
  jointJ1.init(); jointJ2.init(); pressAxis.init();
  goToHome();
  // 初始化PID
  forcePID.SetMode(AUTOMATIC);
  forcePID.SetSampleTime(20);
}

void loop() {
  // 1. 力采样与PID控制
  float currentForce = loadCell.read();
  // 滤波处理
  static float forceBuf[5] = {0};
  static int bufIdx = 0;
  forceBuf[bufIdx++] = currentForce;
  if(bufIdx >= 5) bufIdx = 0;
  float avgForce = (forceBuf[0]+forceBuf[1]+forceBuf[2]+forceBuf[3]+forceBuf[4])/5;
  loadCellValue = avgForce;
  
  // 力PID闭环：自动输出压装轴速度补偿
  forcePID.Compute();
  
  // 2. 恒力压装控制
  if(isPressing) {
    // 深度计算：通过编码器或速度积分估算（简化，实际需位移传感器）
    currentDepth += forceOutput * 0.01; // 速度积分得深度
    
    // 压装到位判定：深度达标且力稳定
    if(currentDepth >= PRESS_DEPTH && abs(avgForce - PRESS_FORCE) < 1.0) {
      pressAxis.move(0);
      isPressing = false;
      // 回退1mm避免卡滞
      pressAxis.move(-10);
      delay(100);
      pressAxis.move(0);
      Serial.println("Press Complete: Depth="); Serial.print(currentDepth);
      Serial.print(" Force="); Serial.println(avgForce);
      currentDepth = 0;
      return;
    }
    
    // 3. 弹性位置补偿：补偿轴承弹性变形导致的位置偏差
    // 逻辑：力偏差与深度变化关联，动态调整J1/J2位置，避免倾斜
    float depthError = PRESS_DEPTH - currentDepth;
    float forceError = forceSetpoint - avgForce;
    // 位置补偿量：力偏差+深度偏差综合计算
    float compAmount = (forceError * ELASTIC_COMP_KP) + (depthError * 0.3);
    compAmount = constrain(compAmount, -2, 2);
    
    // 映射为关节速度补偿
    int j1Speed = map(compAmount, -2, 2, -20, 20);
    int j2Speed = map(compAmount, -2, 2, -15, 15);
    // 速度下限，避免微小偏差频繁抖动
    j1Speed = abs(j1Speed) < MIN_POS_SPEED ? 0 : j1Speed;
    j2Speed = abs(j2Speed) < MIN_POS_SPEED ? 0 : j2Speed;
    
    jointJ1.move(j1Speed);
    jointJ2.move(j2Speed);
    
    // 力输出映射为压装轴速度
    float pressSpeed = map(forceOutput, -5, 5, -60, 60);
    pressSpeed = constrain(pressSpeed, -80, 80);
    pressAxis.move(pressSpeed);
    
    Serial.print("Press: Depth="); Serial.print(currentDepth);
    Serial.print(" Force="); Serial.print(avgForce);
    Serial.print(" Comp: J1="); Serial.print(j1Speed);
    Serial.print(" J2="); Serial.println(j2Speed);
  }
  
  // 任务触发：接收上位机指令或按钮信号
  if(Serial.available() && Serial.read() == 'p') {
    isPressing = true;
    Serial.println("Start Bearing Press");
  }
  
  delay(20); // 50Hz控制周期，保障PID稳定
}

// 回零函数
void goToHome() {
  jointJ1.move(-40);
  jointJ2.move(50);
  pressAxis.move(80);
  delay(1200);
  jointJ1.move(0);
  jointJ2.move(0);
  pressAxis.move(0);
  Serial.println("Home for Press");
}

核心逻辑：引入PID实现力闭环控制，确保压装力恒定；结合压装深度与力偏差，动态补偿X/Y轴位置，修正弹性工件的压装倾斜，避免轴承卡滞或压装不到位，适配微型轴承等弹性工件的精密压装场景。

6、柔性插件插装系统（恒力插装+插入位置动态补偿）
适用场景：SCARA机器人插装柔性排线、微型插座等柔性插件时，需以恒定力缓慢插入，避免插针变形；同时补偿插件与插座的对准偏差，确保插装过程中力恒定、位置精准，防止插件损坏。

// 核心：恒力插装+柔性位置补偿，适配带缓冲的力控插装
#include <SimpleFOC.h>
#include <HX711.h>
#include <Filters.h> // 滤波库（去除力传感器抖动）

// SCARA关节电机
BLDCMotor jointJ1 = BLDCMotor(5);
BLDCMotor jointJ2 = BLDCMotor(6);
BLDCMotor insertAxis = BLDCMotor(7); // 插装轴
HX711 insertForceSensor; // 插装力传感器

// 插装参数
const float INSERT_FORCE = 5.0;    // 目标插装力（N，柔性插件轻力）
const float FORCE_K = 1.5;         // 力-速度转换系数
const float ALIGN_COMP_KP = 0.6;   // 对准偏差补偿系数
const float STABLE_FORCE_T = 80;   // 力稳定时间（ms）
const float INSERT_SPEED_LIMIT = 40; // 最大插装速度（避免过快）
bool isInserting = false;
unsigned long stableTime = 0;

// 低通滤波器（抑制力传感器高频噪声）
LowPassFilter forceFilter(5.0); // 截止频率5Hz

void setup() {
  Serial.begin(115200);
  insertForceSensor.begin(A2, A3, A4);
  insertForceSensor.set_scale(80.0);
  
  jointJ1.init(); jointJ2.init(); insertAxis.init();
  goToHome();
  // 初始化滤波器
  forceFilter.init(5.0);
}

void loop() {
  // 1. 力采样与滤波：去除高频抖动，获取稳定插装力
  float rawForce = insertForceSensor.read();
  float filteredForce = forceFilter.update(rawForce);
  
  // 2. 恒力插装控制
  if(isInserting) {
    // 力偏差计算
    float forceError = INSERT_FORCE - filteredForce;
    // 力控输出：力不足时加速，力过大时减速
    float speedAdjust = FORCE_K * forceError;
    float insertSpeed = constrain(speedAdjust, -INSERT_SPEED_LIMIT, INSERT_SPEED_LIMIT);
    
    // 动态限制速度，避免柔性插件受力突变
    if(abs(forceError) > 1.0) {
      insertSpeed = map(forceError, -2, 2, -INSERT_SPEED_LIMIT, INSERT_SPEED_LIMIT);
    } else {
      insertSpeed = map(forceError, -1, 1, -10, 10); // 力接近目标时降速，保障平稳
    }
    
    insertAxis.move(insertSpeed);
    
    // 3. 对准偏差补偿：实时补偿插件与插座的对准偏差（基于力反馈的偏差判断）
    // 逻辑：力突然增大说明插针偏移，调整X/Y轴对准
    if(abs(forceError) > 0.8) { // 力偏差大说明位置偏移
      // 计算补偿量：力偏差与补偿系数的乘积，结合方向调整
      float compX = forceError * ALIGN_COMP_KP;
      float compY = forceError * ALIGN_COMP_KP * 0.7; // X/Y轴权重适配SCARA结构
      
      // 映射为关节速度补偿
      int j1Speed = constrain(map(compX, -1.5, 1.5, -25, 25), -20, 20);
      int j2Speed = constrain(map(compY, -1.5, 1.5, -20, 20), -15, 15);
      
      // 限速，避免过度补偿导致抖动
      if(abs(j1Speed) > 10 || abs(j2Speed) > 8) {
        jointJ1.move(j1Speed);
        jointJ2.move(j2Speed);
      }
      
      Serial.print("Align Comp: J1="); Serial.print(j1Speed);
      Serial.print(" J2="); Serial.print(j2Speed);
      Serial.print(" Force="); Serial.println(filteredForce);
    } else {
      // 对准良好，保持X/Y轴静止
      jointJ1.move(0);
      jointJ2.move(0);
    }
    
    // 插装到位判定：力稳定在目标范围内持续STABLE_FORCE_T
    if(abs(filteredForce - INSERT_FORCE) < 0.5) {
      if(stableTime == 0) stableTime = millis();
      if(millis() - stableTime > STABLE_FORCE_T) {
        // 插装完成：停止插装轴，保持力1秒后回退
        insertAxis.move(0);
        stableTime = 0;
        delay(100);
        // 轻微回退，避免插件卡住
        insertAxis.move(-5);
        delay(50);
        insertAxis.move(0);
        isInserting = false;
        Serial.println("Insert Complete");
      }
    } else {
      stableTime = 0;
    }
  }
  
  // 任务触发：接收插装指令
  if(Serial.available() && Serial.read() == 'i') {
    isInserting = true;
    stableTime = 0;
    Serial.println("Start Flexible Insert");
  }
  
  delay(10); // 100Hz控制周期，保障插装实时性
}

// 回零（插装前的初始位置）
void goToHome() {
  jointJ1.move(-35);
  jointJ2.move(45);
  insertAxis.move(70); // 插装轴抬起
  delay(1000);
  jointJ1.move(0);
  jointJ2.move(0);
  insertAxis.move(0);
  Serial.println("Home for Insert");
}

核心逻辑：通过低通滤波抑制力传感器噪声，结合力反馈调节插装轴速度维持恒力；力偏差较大时动态补偿X/Y轴位置，修正柔性插件的对准偏差，确保插装过程中力恒定、位置精准，适配柔性插件的无损插装需求。

要点解读

1. 力控闭环的精准实现：从传感器采样到动态补偿的全链路设计
   核心逻辑：恒力装配的核心是力信号采集→误差计算→速度补偿的闭环控制，需解决传感器噪声、采样实时性、补偿精度三大问题，确保力闭环稳定。
   关键要点：
   力传感器滤波与标定：采用滑动平均或低通滤波去除力传感器高频噪声，避免抖动导致控制失稳；通过标准砝码标定传感器系数，确保力信号与实际压力精准对应（标定误差需<1%）。
   控制周期与采样频率匹配：控制周期需小于力传感器的响应时间（通常10-20ms），避免采样延迟导致力闭环滞后；采样频率为控制周期的2-3倍，保障力信号的实时性与稳定性。
   力-速度映射规则：将力偏差转化为电机速度，采用分段映射策略——力偏差小时降低速度调整幅度（避免抖动），力偏差大时增大调整幅度（快速响应），确保恒力维持的平稳性。
2. 位置补偿的动态机制：基于力反馈的偏差识别与精准修正
   核心逻辑：位置补偿依赖力偏差→位置偏差的实时映射，通过动态调整关节位置补偿定位误差，解决机械定位偏差、工件形变、装配偏差等问题，是装配精度的保障。
   关键要点：
   偏差阈值设定：设置合理的力偏差阈值（如目标力的±10%-20%），仅当偏差超过阈值时启动补偿，避免微小噪声触发不必要的位置调整，降低系统抖动。
   补偿量的限幅与平滑：对位置补偿量进行限幅（如单次补偿角度≤10°），避免超量程补偿；采用渐变式补偿（如每次补偿量为限幅值的50%），防止位置突变导致机械冲击或系统振荡。
   补偿与运动的协同：位置补偿需与主运动（如Z轴压装、插装）解耦，即补偿过程中主运动速度根据力反馈动态调整，避免补偿动作干扰力闭环，实现“补偿-恒力”的协同。
3. SCARA运动学与力控的深度耦合：关节空间到任务空间的精准映射
   核心逻辑：SCARA机器人的恒力装配需将力反馈→关节位置补偿的映射，与机器人正/逆运动学结合，确保位置补偿动作在关节空间可实现，且末端执行器轨迹与力控需求匹配。
   关键要点：
   关节速度到末端轨迹的映射：SCARA的X/Y运动由J1、J2关节协同实现，需根据J1、J2的转动惯量与传动比，将末端位置偏差转化为关节速度，避免单一关节过载或运动失配。
   力控轴与运动轴的优先级划分：明确主任务轴（如Z轴压装、插装轴）与补偿轴（J1、J2）的优先级，主任务轴以力控为核心，补偿轴以位置修正为核心，二者独立调节但协同生效。
   运动学简化与精度平衡：Arduino资源有限，可采用简化的运动学模型（如忽略关节间隙、杆长误差的小偏差模型），结合人工标定补偿误差，在计算效率与控制精度之间取得平衡。
4. 安全机制的冗余设计：力超限与运动失控的双重防护
   核心逻辑：恒力装配中，力超限可能损坏工件或机械臂，运动失控可能导致碰撞，需构建力限防护+运动限位+急停机制的三重安全体系，保障系统与工件安全。
   关键要点：
   力超限急停：设置最大安全力阈值（如目标力的1.5-2倍），一旦检测到力超过阈值，立即停止所有运动轴，并发出声光报警，防止工件损坏或机械臂过载。
   运动限位保护：为各关节设置软限位（通过软件限制角度范围）与硬限位（机械限位开关），防止关节超程导致机械损坏；同时对主运动轴（如Z轴）设置位移限位，避免过度下压。
   故障自诊断与容错：实时检测力传感器、电机的异常信号（如传感器断线、电机失步），触发容错策略——传感器异常时暂停任务并回退到安全位置，电机失步时停止运动并复位。
5. 嵌入式平台的资源优化：有限算力下的实时控制保障
   核心逻辑：Arduino平台（如Mega）算力有限，需优化代码效率、合理分配资源，在保障力控与位置补偿实时性的同时，避免资源耗尽导致控制卡顿或失稳。
   关键要点：
   代码精简与高效运算：采用整数运算替代浮点运算（如力、位置数据量化为整数处理），减少CPU计算负担；避免复杂算法（如高级滤波、运动学解算），采用查表法、简化PID等轻量化方案。
   多任务调度优先级划分：将力采样、力闭环、位置补偿设为高优先级任务，采用固定周期调度（如10-20ms），确保核心控制逻辑实时执行；将串口打印、状态显示设为低优先级，避免占用CPU时间。
   硬件资源合理分配：优化引脚分配，将力传感器、电机控制引脚分开，避免信号干扰；采用外置滤波电路减少传感器噪声，降低软件滤波压力；必要时选用Arduino Mega而非Uno，拓展IO与内存资源。

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