发一个算法大家参考一下，有什么问题可以继续讨论
FUNCTION S_PosV1 : VOID

TITLE = 'Positioning Function with limit '

VAR_INPUT
EnaPos : BOOL ; // Enable Positioning
JogFor : BOOL ; // Jog Forward
JogRev : BOOL ; // Jog Reverse
JogSlow : BOOL ; // Jog slow speed
LimFor : BOOL ; // Not Limit Forward
LimRev : BOOL ; // Not Limit Reverse
ActPos : REAL ; // [mm] actual position
SetPos : REAL ; // [mm] wanted position
MaxPos : REAL ; // [mm] Maximum Limit Position
MinPos : REAL ; // [mm] Minimum Limit Position
PosWin : REAL ; // [mm] positioning window
Kp : REAL ; // positioning P-gain 0.1 ... 5.0
vMax : REAL ; // [mm/s] v at vPos
tRamp : REAL ; // [s] ramp down time
vPos : REAL ; // v out 0..100% Positioning Speed
vJog : REAL ; // v out 0..100% Jogging
vJSlow : REAL ; // v out 0..100% slow speed Jogging
Dly : INT ; // [ms] positioning delay
TiOut : DINT ; // [ms] positioning Time out
actT0 : INT ; // [ms] act. cycle time
END_VAR

VAR_IN_OUT
TimerCnt : ARRAY [0..1] OF INT ; // [cycles] Timer for positioning delay
END_VAR

VAR_OUTPUT
PosOk : BOOL ; // Position ok
PosBsy : BOOL ; // Positioning is running
ErrPos : BOOL ; // Out of Position
ErrTout : BOOL ; // Positioning Time Out
n_out : REAL ; // [%] Speed output
END_VAR

VAR_TEMP
PoWin : BOOL ;
Enable : BOOL ;
tempi : INT ;
tPos : REAL ;
Speed : REAL ;
vTemp : REAL ;
tempf : REAL ;
abs_temp : REAL ;
KPdyn : REAL ;
titout : INT ;
tidly : INT ;
END_VAR

VAR
END_VAR

BEGIN
// if actual position is more then 10mm out of software limits = error
ErrPos := (ActPos > MaxPos + 10) OR (ActPos MinPos - 10) THEN
tPos := MinPos;
IF JogSlow THEN
vTemp := vJSlow;
ELSE
vTemp := vJog;
END_IF;
ELSIF SetPos > MaxPos THEN
tPos := MaxPos;
vTemp := vPos;
ELSIF SetPos vTemp THEN
Speed := vTemp;
END_IF;

PosBsy := TRUE;
ELSE
Speed := 0.0;
PosBsy := FALSE;
END_IF;

IF Speed 0 THEN
IF NOT LimFor THEN
Speed := 0;
END_IF;
END_IF;

PoWin := ABS(SetPos - ActPos) 0 THEN
TimerCnt[0] := TimerCnt[0] - 1;
END_IF;

IF TimerCnt[1] > 0 THEN
TimerCnt[1] := TimerCnt[1] - 1;
END_IF;

PosOk := PoWin AND NOT PosBsy;
n_out := Speed;

END_FUNCTION