m.heisig
cf9501ea01
- All components now use HAL structs as inputs and outputs - Restructured library folders - FB_Axis_PTP now implements the execute pattern with E_CmdResult - Bumped version number to 2.0.0
108 lines
2.2 KiB
XML
108 lines
2.2 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1">
|
|
<POU Name="FB_PID" Id="{7822f674-b0e7-48ea-8d27-164e15dbed0c}" SpecialFunc="None">
|
|
<Declaration><![CDATA[FUNCTION_BLOCK FB_PID
|
|
VAR_INPUT
|
|
rSP : REAL;
|
|
rPV : REAL;
|
|
rKp : REAL;
|
|
rTn : REAL; // In seconds
|
|
rTv : REAL; // In seconds
|
|
rTdFilter : REAL; // Filter time constant
|
|
|
|
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;
|
|
_rProportional : REAL := 0.0;
|
|
_rDerivative : REAL := 0.0;
|
|
_rDeltaPV : REAL := 0.0;
|
|
_rPVLast : REAL := 0.0;
|
|
_rDRaw : REAL := 0.0;
|
|
|
|
_rDeltaIntegral : REAL := 0.0;
|
|
|
|
_rT : REAL;
|
|
|
|
_xFirstCycle : BOOL := TRUE;
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[IF _xFirstCycle THEN
|
|
_xFirstCycle := FALSE;
|
|
|
|
// Get current task time
|
|
_rT := F_GetTaskCycleTime();
|
|
|
|
_rPVLast := rPV;
|
|
END_IF
|
|
|
|
|
|
IF xEnable THEN
|
|
_rError := rSP - rPV;
|
|
ELSE
|
|
_rError := 0.0;
|
|
_rIntegral := 0.0;
|
|
_rDerivative := 0.0;
|
|
_rPVLast := rPv;
|
|
rMV := 0.0;
|
|
RETURN;
|
|
END_IF
|
|
|
|
// Calculate proportinal part
|
|
_rProportional := rKp * _rError;
|
|
|
|
// 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) THEN
|
|
_rIntegral := 0.0;
|
|
END_IF
|
|
|
|
IF rTv > 0.0 THEN
|
|
_rDeltaPV := (rPV - _rPVLast) / _rT;
|
|
|
|
// Unfiltered derivative
|
|
_rDRaw := -rKp * rTv * _rDeltaPV;
|
|
|
|
// Filter with PT1
|
|
IF rTdFilter > 0.0 THEN
|
|
_rDerivative := _rDerivative + (_rT / (rTdFilter + _rT)) * (_rDRaw - _rDerivative);
|
|
ELSE
|
|
_rDerivative := _rDRaw;
|
|
END_IF
|
|
ELSE
|
|
_rDerivative := 0.0;
|
|
END_IF
|
|
|
|
_rPVLast := rPV;
|
|
|
|
// Calculate controller output
|
|
rMV := _rProportional + _rIntegral + _rDerivative;]]></ST>
|
|
</Implementation>
|
|
</POU>
|
|
</TcPlcObject> |