代码如下：
FUNCTION_BLOCK Positionc_PID_Cycle_Period_First_shock (* 位置 PID带第一次冲击 液压滞后处理闭环系统 应用液压伺服控制 *)
(* U(t) = KP(err(t) + Σ(t)*dt/TI+TD*derr(t)/dt ) ;
err(t) = rin(t)-rout(t) ;
Σ(t) = err(t)+ err(t+1)+┈┈ ;
derr(t) = err(t)- err(t-1) ;
dt = 采样 循环周期 S
Kp为比例带，TI为积分时间，TD为微分时间 ;
*)
VAR_INPUT
Set:REAL; (* 设定值 *)
Actual:REAL; (* 实际值 *)
Cycle_time:REAL; (* 采样循环周期 单位：S *)
Blind_spot:REAL ; (* 死区 默认值转向角度1.0% *)
KP :REAL; (* 比例带 单位：% *)
TI :REAL; (* 积分时间 单位：S *)
Integral_limit :REAL; (* 积分 累计值限幅 Σ(t) 默认绝对值为300 *)
TD :REAL; (* 微分时间 单位：S *)
OUT_Manual:REAL; (* 手动 输出值 *)
OUT_OFFSET:REAL; (* 输出修正值 *)
OUT_MIN:REAL; (* 输出范围最小值 *)
OUT_MAX:REAL; (* 输出范围最大值 *)
Manual:BOOL; (* 手动 *)
Reset:BOOL; (* 复位 *)
END_VAR
VAR_OUTPUT
OUT:REAL;(* 输出值 *)
Direction:BOOL;(* 输出方向 *)
END_VAR
VAR
Accuracy_Revise: REAL:=0.0 ; (* 死区 0 预设值 1 预估值 2 *)
dRho:ARRAY[0..1] OF REAL; (* 偏差 0 当前偏差 1 上一次偏差 *)
SumdRho :REAL; (* 积分累计值 *)
PID_Y: REAL; (*PID输出值*)
Angle_Value_OUT,First_shock_OUT: REAL; (*限幅后的输出值*)
Value: ARRAY [0..1] OF REAL; (*限下幅后的输出值*)
ID: INT;
ID0: INT := 0;
ID1: INT := 1;
TON_Accuracy_Revise_0,TON_Accuracy_Revise_1 : TON;
TOF_First_shock :TOF;
UP_R_First_shock,UP_R0,UP_R1:R_TRIG; (* 每次运行启动前 初始化 一次 *)
Min_Accuracy_Revise: REAL:=0.15 ; (* 最小死区 编码器1 = 0.17% *)
Offset_Accuracy_Revise :REAL:=0.4; (* 最小死区偏移值 默认0.4% 延时0.6S *)
TOF_First_shock_time:TIME:=t#40ms;
END_VAR
(* ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ 偏差值 ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ *)
dRho[1] := dRho[0] ; (* 上一次偏差 *)
dRho[0] := Set - Actual ;
(* 当前 偏差 ： 目标 - 实际 *)
(* ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ 第一次冲击 ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ *)
UP_R_First_shock(CLK:= NOT Reset AND ABS( Accuracy_Revise ) < ABS( dRho[0] ) AND NOT Manual , Q=> );
TOF_First_shock(IN:=UP_R_First_shock.Q , PT:= TOF_First_shock_time, Q=> , ET=> );
IF TOF_First_shock.Q THEN
First_shock_OUT := (OUT_MAX - OUT_MIN )* 0.85 + OUT_MIN ;
ELSE
First_shock_OUT:=0.0;
END_IF
(* ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ 定位 ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ *)
IF NOT Reset AND ABS( Accuracy_Revise ) < ABS( dRho[0] ) AND NOT Manual AND NOT TOF_First_shock.Q THEN
(* &&&&&&&&&&&&&&& 积分限幅 300 &&&&&&&&&&&&&&& *)
IF SumdRho >Integral_limit THEN SumdRho :=Integral_limit ; END_IF;
IF SumdRho < -1.0*Integral_limit THEN SumdRho := -1.0*Integral_limit ; END_IF;
(* 积分累计运算 *)
SumdRho := SumdRho + dRho[0]; (* 累计积分 ：积分 当前偏差 + *)
(* U(t) = KP(err(t) + Σ(t) * dt/TI+TD*derr(t)/dt ) ;
err(t) = rin(t)-rout(t) ;
Σ(t) = err(t)+ err(t+1)+┈┈ ;
derr(t) = err(t)- err(t-1) ;
dt = 采样 循环周期 S
Kp为比例带，TI为积分时间，TD为微分时间 ;
*)
IF TI =0 THEN TI := 10000.0; END_IF;
IF Cycle_time =0 THEN Cycle_time := 0.02; END_IF;
PID_Y := KP *( dRho[0] + SumdRho * Cycle_time / TI (* PI 控制 *)
+ TD * (dRho[0] - dRho[1] ) / Cycle_time ) ;
(* D 控制 *)
(* 输出限幅 *)
IF PID_Y > ( OUT_MAX - OUT_MIN ) THEN
Angle_Value_OUT := OUT_MAX - OUT_MIN ; END_IF; (* 限制输出 上限限幅 *)
IF PID_Y < (( OUT_MAX - OUT_MIN ) *-1.0 ) THEN
Angle_Value_OUT := ( OUT_MAX - OUT_MIN ) *-1.0; END_IF; (* 限制输出下限限幅 *)
IF PID_Y >= (( OUT_MAX - OUT_MIN ) *-1.0 ) AND PID_Y <= ( OUT_MAX - OUT_MIN ) THEN
Angle_Value_OUT := PID_Y ;
END_IF
ELSE
SumdRho := 0.0;
Angle_Value_OUT := 0.0;
END_IF;
(* ◆◆◆◆◆◆ ◆◆◆◆◆◆◆◆◆◆◆ 自动回中 方向判断 电流 输出 ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ *)
ID:=ID0;
IF Angle_Value_OUT > 0 THEN
Value[ID] := ABS(Angle_Value_OUT) + OUT_MIN ;
ELSE Value[ID]:=0; END_IF;
ID:=ID1;
IF Angle_Value_OUT < 0 THEN
Value[ID] := ABS(Angle_Value_OUT ) + OUT_MIN ;
ELSE Value[ID]:=0; END_IF;
(* ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ 输出 ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ *)
IF NOT Manual THEN
OUT := Value[0] + Value[1] + First_shock_OUT + OUT_OFFSET;
ELSE
OUT := OUT_Manual + OUT_OFFSET ;
END_IF;
IF Value[0] >0 THEN
Direction :=TRUE;
ELSE
Direction :=FALSE;
END_IF
(* ◆◆◆◆◆◆ ◆◆◆◆◆◆◆◆◆◆◆ 死区判断 ： 输出电流 接近 电流 最小值 and 连续运行时间大于 0.5S and 转向 角度小于1 % = 暂停 转向 1 S 转向 ◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆◆ *)
TON_Accuracy_Revise_0(IN:=
ABS(OUT) < OUT_MIN +5+OUT_OFFSET AND
ABS( dRho[0]) < Blind_spot + Offset_Accuracy_Revise
, PT:=t#0.5S , Q=> , ET=> );
IF TON_Accuracy_Revise_0.Q THEN
Accuracy_Revise :=ABS(Blind_spot);
END_IF;
(* 死区初始化 自动修正 死区值 *)
TON_Accuracy_Revise_1(IN:=
ABS(dRho[0] ) > Min_Accuracy_Revise AND Angle_Value_OUT=0 AND
NOT Reset AND NOT Manual
, PT:=t#0.8S , Q=> , ET=> );
(* 死区初始化 *)
UP_R0(CLK:= ABS(dRho[0] ) > Min_Accuracy_Revise , Q=> );
UP_R1(CLK:= TON_Accuracy_Revise_1.Q , Q=> );
IF UP_R1.Q THEN
Offset_Accuracy_Revise := 0.4 - 0.25 ;
Accuracy_Revise:=0.0;
END_IF;
IF ABS( dRho[0] ) =0 OR Reset OR UP_R0.Q OR Manual THEN
Accuracy_Revise:=0.0;
Offset_Accuracy_Revise := 0.4 ;
END_IF;
