82 lines
1.7 KiB
XML
82 lines
1.7 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1">
|
|
<POU Name="FB_PI" Id="{d504557e-7fd7-4784-a00a-5d7d1bed0c95}" SpecialFunc="None">
|
|
<Declaration><![CDATA[FUNCTION_BLOCK FB_PI
|
|
VAR_INPUT
|
|
rSP : REAL;
|
|
rPV : REAL;
|
|
rKp : REAL;
|
|
rTn : REAL;
|
|
|
|
xEnable : BOOL;
|
|
xSaturatedUpper : BOOL := FALSE;
|
|
xSaturatedLower : BOOL := FALSE;
|
|
END_VAR
|
|
VAR_OUTPUT
|
|
rMV : REAL;
|
|
END_VAR
|
|
VAR
|
|
|
|
_rError : REAL := 0.0;
|
|
_rIntegral : REAL := 0.0;
|
|
_rProportinal : REAL := 0.0;
|
|
|
|
_rDeltaIntegral : REAL := 0.0;
|
|
|
|
_fbGetCurTaskIdx : GETCURTASKINDEX;
|
|
_rT : REAL;
|
|
|
|
_xFirstCylce : BOOL := TRUE;
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[IF _xFirstCylce THEN
|
|
_xFirstCylce := FALSE;
|
|
|
|
// Get current task time
|
|
_fbGetCurTaskIdx();
|
|
_rT := LREAL_TO_REAL(UDINT_TO_LREAL(TwinCAT_SystemInfoVarList._TASKInfo[_fbGetCurTaskIdx.index].CycleTime) * 10E-5);
|
|
END_IF
|
|
|
|
|
|
IF xEnable THEN
|
|
_rError := rSP - rPV;
|
|
ELSE
|
|
_rError := 0.0;
|
|
_rIntegral := 0.0;
|
|
rMV := 0.0;
|
|
RETURN;
|
|
END_IF
|
|
|
|
// Calculate proportinal part
|
|
_rProportinal := rKp * _rError;
|
|
|
|
// Calculate controller output
|
|
rMV := _rProportinal + _rIntegral;
|
|
|
|
// Calculate integral for this step
|
|
IF rTn <> 0 THEN
|
|
_rDeltaIntegral := (rKp * _rT / rTn) * _rError;
|
|
ELSE
|
|
_rDeltaIntegral := 0;
|
|
END_IF
|
|
|
|
// Only add new integral part if we are going away from the upper or lower bound
|
|
IF (xSaturatedUpper AND (_rDeltaIntegral > 0.0)) THEN
|
|
_rDeltaIntegral := 0.0;
|
|
END_IF
|
|
|
|
IF (xSaturatedLower AND (_rDeltaIntegral < 0.0)) THEN
|
|
_rDeltaIntegral := 0.0;
|
|
END_IF
|
|
|
|
// Calculate integral part
|
|
_rIntegral := _rIntegral + _rDeltaIntegral;
|
|
|
|
// Reset integral with deactivated integral time
|
|
IF (rTn = 0.0) AND (_rIntegral <> 0) THEN
|
|
_rIntegral := 0.0;
|
|
END_IF]]></ST>
|
|
</Implementation>
|
|
</POU>
|
|
</TcPlcObject> |