我有两个伺服，走的ethercat协议，用PLC的ST语言帮我写一个伺服电机直线插补算法程序，注意：没有指令库，需要自己写算法

在没有EtherCAT指令库的情况下，编写一个PLC ST语言的伺服电机直线插补算法程序会涉及到基本的数学计算和状态控制。以下是一个简化版的示例，假设我们有位置目标（TargetPosition），当前位置（CurrentPosition），速度限制（MaxSpeed），以及步进大小（StepSize）。请注意，这个例子并没有实际的 EtherCAT 指令，因为PLC ST语言的具体语法可能会因型号而异，你需要将以下伪代码转化为对应于你的PLC型号的实际ST语句。

-- 定义变量和常量 --
VAR PositionTarget, CurrentPosition : REAL;
VAR MaxSpeed, StepSize : REAL;
VAR MoveStatus : BOOL := FALSE;
VAR LastMoveTime : TIMESTAMP;

-- 直线插补函数 --
PROC LinearInterpolation() :
  IF MoveStatus THEN
    -- 计算移动时间
    DeltaTime := Now - LastMoveTime;
    DistanceToTravel := Abs(PositionTarget - CurrentPosition);
    
    -- 根据速度限制调整步长
    NewStepSize := Min(MaxSpeed * DeltaTime, DistanceToTravel / DeltaTime);
    
    -- 更新电机位移
    IF CurrentPosition < PositionTarget THEN
      CurrentPosition := CurrentPosition + NewStepSize;
    ELSE
      CurrentPosition := CurrentPosition - NewStepSize;
    ENDIF
    
    -- 如果到达终点或停止条件满足，则更新状态
    IF CurrentPosition = PositionTarget OR (NewStepSize <= StepSize AND DeltaTime > 0.1) THEN
      MoveStatus := FALSE;
      LastMoveTime := Now;
    ENDIF
  ENDIF;
ENDPROC;

-- 主循环 --
WHILE TRUE DO
  -- 这里是你读取或接收伺服状态的地方
  -- 例如 Read(EtherCatMotorState)
  
  -- 调用插补函数
  LinearInterpolation();
  
  -- 可能还需要其他控制操作...
ENDWHILE;
  
-- 相关问题--
1. 在实际应用中如何处理 EtherCAT 状态变化，如错误或中断？
2. PLC 的定时机制如何配合插补算法以保证精度？
3. 有没有办法优化这个算法来提高响应速度？

请确保根据你的PLC型号和提供的功能调整上述代码，并参考相关文档了解如何在ST语言中处理实时I/O和计时。