4. PLC补偿实现

好，咱们进入实操环节。前面几章讲了误差怎么来的、怎么测的，现在该动手写了。这一章我会带着你在西门子S7-1200/1500平台上，把补偿逻辑真正落地。

我个人习惯，做定位补偿从来不搞花架子。核心就三件事：功能块封装、补偿表存储、循环扫描执行。说白了，你得有一个能反复调用的FB，一个存数据的DB，还有一个在每个扫描周期里都能跑一遍的补偿逻辑。

4.1 补偿功能块（FB）的架构设计

先说说功能块。我建议用FB而不是FC，为什么？因为FB有背景数据块，可以保存每次调用的状态。你在现场调试时，总不希望每次上电补偿参数都丢了吧？

下面是我在项目中常用的接口定义：

接口类型	参数名	数据类型	说明
Input	TargetPos	Real	目标位置（用户期望到达的位置）
Input	Enable	Bool	使能信号，为True时执行补偿
InOut	CompTable	UDT_CompTable	补偿表数据（通过UDT传递）
Output	CompensatedPos	Real	补偿后的位置（发给驱动器）
Output	ErrorCode	Word	错误代码，0表示正常
Static	LastIndex	Int	上次查表索引，用于加速查找

4.2 DB块存储补偿表——数据结构设计

补偿表怎么存？我见过有人用一堆单独的变量，比如CompPos_1、CompPos_2……写到后面自己都晕了。千万别这么干。

正确的做法是用数组+UDT。下面是我常用的结构：

// UDT: UDT_CompPoint
DATA_BLOCK "DB_CompensationTable"
{ 
  // 补偿点数量
  NumPoints : Int := 10;
  
  // 补偿点数组
  Points : ARRAY[1..50] OF STRUCT
    RawPos      : Real;   // 原始位置（编码器读数）
    CompOffset  : Real;   // 补偿偏移量（带符号）
    IsValid     : Bool;   // 该点是否有效
  END_STRUCT;
  
  // 补偿参数
  EnableComp   : Bool := TRUE;   // 总使能
  InterpMethod : Int := 1;       // 插值方法：1-线性，2-三次样条
  MaxOffset    : Real := 5.0;    // 最大补偿量限幅（mm）
}

这里有个细节——为什么RawPos和CompOffset要分开存？ 我在早期项目里犯过错，直接存补偿后的位置。结果换了个编码器，所有数据全废了。分开存的好处是：你随时可以换编码器，只要重新标定RawPos就行，CompOffset是纯机械误差，跟编码器无关。

4.3 循环扫描补偿逻辑——核心算法

逻辑不复杂，但每一步都有坑。我把它拆成四个步骤：

1. 边界检查——目标位置是否在补偿表范围内？
2. 区间查找——目标位置落在哪两个补偿点之间？
3. 插值计算——线性插值还是样条插值？
4. 限幅输出——补偿量不能超过安全范围。

下面是线性插值的核心代码片段：

// 线性插值补偿
// 输入：TargetPos, Points数组
// 输出：CompensatedPos

// 1. 边界保护
IF TargetPos <= Points[1].RawPos THEN
    Offset := Points[1].CompOffset;
ELSIF TargetPos >= Points[NumPoints].RawPos THEN
    Offset := Points[NumPoints].CompOffset;
ELSE
    // 2. 查找区间（二分法）
    L := 1;
    R := NumPoints;
    WHILE (R - L) > 1 DO
        Mid := (L + R) / 2;
        IF TargetPos >= Points[Mid].RawPos THEN
            L := Mid;
        ELSE
            R := Mid;
        END_IF;
    END_WHILE;
    
    // 3. 线性插值
    Ratio := (TargetPos - Points[L].RawPos) / 
             (Points[R].RawPos - Points[L].RawPos);
    Offset := Points[L].CompOffset + 
              Ratio * (Points[R].CompOffset - Points[L].CompOffset);
END_IF;

// 4. 限幅
IF Offset > MaxOffset THEN Offset := MaxOffset; END_IF;
IF Offset < -MaxOffset THEN Offset := -MaxOffset; END_IF;

// 5. 输出
CompensatedPos := TargetPos + Offset;

4.4 补偿逻辑的扫描周期优化

你想想看，如果补偿表有50个点，每个扫描周期都从头到尾遍历一遍，CPU受不了。尤其是S7-1200，扫描周期本来就紧。

我的优化策略：

• 缓存上次索引：大多数情况下，目标位置是连续变化的。从上次索引开始查，大概率就在附近。
• 分段查找：如果目标位置变化很大（比如回零后突然跳到另一端），再用二分法。
• 预计算斜率：线性插值的斜率可以在下载补偿表时就算好存起来，不用每次算除法。

下面是带缓存索引的查找逻辑：

// 带缓存的区间查找
// 先检查上次索引附近
IF TargetPos >= Points[LastIndex].RawPos AND 
   TargetPos <= Points[LastIndex+1].RawPos THEN
    // 还在同一个区间，直接用
    L := LastIndex;
    R := LastIndex + 1;
ELSE
    // 区间变了，重新二分查找
    // ...（二分法代码同上）
    LastIndex := L;  // 更新缓存
END_IF;

4.5 整体知识结构

下面这张图把整个补偿实现的核心脉络画出来了，你看一眼就能明白各模块之间的关系：

4.6 完整调用示例

最后，给你看一个在OB1中调用的完整例子：

// OB1 主循环中调用
// 假设：
//   DB1 是补偿表数据块
//   "HMI_TargetPos" 是HMI设定的目标位置
//   "Drive_SetPos" 是发给驱动器的位置指令

// 使能补偿
"FB_Compensation_Instance"(
    Enable      := TRUE,
    TargetPos   := "HMI_TargetPos",
    CompTable   := "DB1".Points,  // 传递整个数组
    CompensatedPos => "Drive_SetPos",
    ErrorCode   => "Local_Error"
);

// 错误处理
IF "Local_Error" <> 0 THEN
    // 记录报警，切换到安全模式
    "Alarm_CompError" := TRUE;
    "Drive_SetPos" := "HMI_TargetPos";  // 旁路补偿
END_IF;