FUNCTION "RegPos" : VOID
TITLE ="Position regulator"
AUTHOR : BenBen
VERSION : 1.1


VAR_INPUT
Val : BOOL ; //Block validation
Smax : REAL ; //Maximal speed (m/min)
Acc : REAL ; //Acceleration (m/s?
DeadB : REAL ; //Dead band (mm)
P_Ref : REAL ; //Position reference (mm)
P_Meas : REAL ; //Position measure (mm)
END_VAR
VAR_OUTPUT
Reg_OK : BOOL ; //Regulation block OK
Pos_OK : BOOL ; //Position OK
Spd : REAL ; //Speed reference (m/min)
END_VAR
VAR_TEMP
Gap : REAL ; //Gap between reference and measure
END_VAR
BEGIN
NETWORK
TITLE =
// Variables initialization
L 0.000000e+000;
T #Spd; // Output speed 0
A #Pos_OK;
R #Pos_OK; // Position OK = 0

// Checking of input parameter
A #Val; // Block validation
A( ;
L #DeadB; // Dead band > 0
L 0.000000e+000;
>R ;
) ;
A( ;
L #Acc; //Acceleration > 0
L 0.000000e+000;
>R ;
) ;
JC jp1;
R #Reg_OK; // If parameters not good then
JC FIN; // Output REG OK = 0 and end

// Test position measure = position reference
jp1: L #P_Ref;
L #P_Meas;
<>R ;
JC jp2;
L 0.000000e+000; // If reference = measure then
T #Spd; // speed = 0
S #Pos_OK; // Position OK = 1
JU FIN; // and end

// Gap calculation
jp2: L #P_Ref;
L #P_Meas;
-R ;
ABS ;
T #Gap;

// Speed calcualtion : Spd = SQRT ( 2 * 3.6 * Acc * Gap )
L 2.000000e+000;
L 3.600000e+000;
*R ;
L #Acc;
*R ;
L #Gap;
*R ;
SQRT ;
T #Spd;
L #Smax;
JC jp3;
L #Smax; // If Spd > Smax then Spd = Smax
T #Spd;
jp3: NOP 0;

// +/- speed
L #P_Ref;
L #P_Meas;
>R ;
JC jp4; //If Reference > Measure
L #Spd;
NEGR ;
T #Spd; //then speed = speed * (-1)
jp4: NOP 0;

// Dead band
FIN: L #P_Ref;
L #P_Meas;
-R ;
ABS ;
T #Gap;

L #Gap;
L #DeadB;
>R ; // If Gap > DeadB then jump
JC jp5;
L 0.000000e+000;
T #Spd; // Speed = 0
S #Pos_OK; // Position OK = 1
R #Reg_OK;
JU jp6;
jp5: NOP 0;
R #Pos_OK; // Position OK = 0
NOP 0;
jp6: NOP 0;
END_FUNCTION