Improved AxisPTP, FB_PI and added FB_PID and FB_RampGen

BaseComponents.tsproj

@@ -136,7 +136,7 @@
 		</System>
 		<Plc>
 			<Project GUID="{4E62D9E7-436C-457D-8DC4-82D2FEF91C96}" Name="BasicComponents" PrjFilePath="BasicComponents\BasicComponents.plcproj" TmcFilePath="BasicComponents\BasicComponents.tmc" ReloadTmc="true" AmsPort="851" FileArchiveSettings="#x000e" CopyTmcToTarget="true" CopyTpyToTarget="false" SymbolicMapping="true">
-				<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcHash="{FB895001-23AB-2B17-4B12-BEB2F42468E3}" TmcPath="BasicComponents\BasicComponents.tmc">
+				<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcHash="{26169BD6-1E5A-D1C1-C99A-F9225BAE6710}" TmcPath="BasicComponents\BasicComponents.tmc">
 					<Name>BasicComponents Instance</Name>
 					<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
 					<Vars VarGrpType="2" AreaNo="1">

BasicComponents/BasicComponents.plcproj

@@ -79,6 +79,12 @@
     <Compile Include="POUs\Components\Controller\FB_PI.TcPOU">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="POUs\Components\Controller\FB_PID.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Components\Controller\FB_RampGen.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="POUs\Components\EventListener\FB_EventListener.TcPOU">
       <SubType>Code</SubType>
     </Compile>
@@ -2666,6 +2672,12 @@
             <v>410,5410</v>
           </d>
         </o>
+        <v>{eeeeeeee-3909-4298-8022-501ac3238667}</v>
+        <o>
+          <v n="Name">"{eeeeeeee-3909-4298-8022-501ac3238667}"</v>
+          <d n="SubKeys" t="Hashtable" />
+          <d n="Values" t="Hashtable" />
+        </o>
         <v>{F66C7017-BDD8-4114-926C-81D6D687E35F}</v>
         <o>
           <v n="Name">"{F66C7017-BDD8-4114-926C-81D6D687E35F}"</v>

BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT

@@ -13,6 +13,7 @@ TYPE E_AXIS_PTP_STATE :
 	MOVING_ABSOLUTE,
 	MOVING_RELATIVE,
 	MOVING_VELOCITY,
+	MOVING_MODULO,
 	HALTING,
 	WAIT_FOR_DISABLE,
 	ERROR,

BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU

@@ -7,7 +7,7 @@ VAR_INPUT
 	xCalibrationCam AT %I* : BOOL;
 	xInvertCalibrationCam : BOOL := FALSE;
 	xEnablePositive : BOOL := TRUE;
-	xEnableNegative : BOOL := TRUE;;
+	xEnableNegative : BOOL := TRUE;
 	rOverride : REAL := 100.0;
 	
 	lrVelocity : LREAL;
@@ -23,6 +23,7 @@ VAR_OUTPUT
 	lrActPosition : LREAL;
 	xIsStopped : BOOL;
 	xBusy : BOOL;
+	xDone : BOOL;
 	xError : BOOL;
 END_VAR
 VAR
@@ -36,6 +37,7 @@ VAR
 	_fbMoveAbsolute : MC_MoveAbsolute;
 	_fbMoveRelative : MC_MoveRelative;
 	_fbMoveVelocity : MC_MoveVelocity;
+	_fbMoveModulo : MC_MoveModulo;
 	_fbHalt : MC_Halt;
 	_fbReset : MC_Reset;
 	
@@ -49,6 +51,7 @@ VAR
 	_xStartMoveAbsolute : BOOL;
 	_xStartMoveRelative : BOOL;
 	_xStartMoveVelocity : BOOL;
+	_xStartMoveModulo : BOOL;
 	_xHalt : BOOL;
 	_xReset : BOOL;
 	
@@ -61,6 +64,7 @@ VAR
 	_xExecuteMoveAbs : BOOL;
 	_xExecuteMoveRel : BOOL;
 	_xExecuteMoveVelocity : BOOL;
+	_xExecuteMoveModulo : BOOL;
 	_xExecuteHalt : BOOL;
 	
 	
@@ -74,7 +78,9 @@ VAR
 	_eHomingMode : MC_HomingMode := MC_HomingMode.MC_DefaultHoming;
 	_xCalibrationCam : BOOL;
 	
-	_eMoveVelDirection : MC_Direction;
+	_eMoveDirection : MC_Direction;
+	
+	_xCanExecNewCmd : BOOL;
 	
 	_eState : E_AXIS_PTP_STATE;
 	
@@ -83,6 +89,7 @@ VAR
 	// Ouput buffers
 	// =============
 	
+	_xDone : BOOL;
 	_xBusy : BOOL;
 	_xError : BOOL;
 END_VAR
@@ -91,6 +98,10 @@ END_VAR
       <ST><![CDATA[// Call axis interface
 _fbAxis.ReadStatus();
 
+IF _fbAxis.Status.Error THEN
+	_xError := TRUE;
+END_IF
+
 IF xInvertCalibrationCam THEN
 	_xCalibrationCam := NOT xCalibrationCam;
 ELSE
@@ -169,12 +180,26 @@ _fbMoveVelocity(
 	Acceleration:= 0, 
 	Deceleration:= 0, 
 	Jerk:= 0, 
-	Direction:= _eMoveVelDirection);
+	Direction:= _eMoveDirection);
 	
 IF _fbMoveVelocity.Error THEN
 	_xError := TRUE;
 END_IF
 
+_fbMoveModulo(
+	Axis:= _fbAxis, 
+	Execute:= _xStartMoveModulo, 
+	Position:= _lrTargetPosition, 
+	Velocity:= lrVelocity, 
+	Acceleration:= 0, 
+	Deceleration:= 0, 
+	Jerk:= 0, 
+	Direction:= _eMoveDirection);
+	
+IF _fbMoveModulo.Error THEN
+	_xError := TRUE;
+END_IF
+
 
 _fbHalt(
 	Axis:= _fbAxis, 
@@ -187,6 +212,8 @@ IF _fbHalt.Error THEN
 	_xError := TRUE;
 END_IF
 
+// Can the axis perform a new move command
+_xCanExecNewCmd := (NOT _xBusy) AND _fbPower.Status AND (NOT _xError);
 
 // ====================
 // Handle state machine
@@ -209,6 +236,7 @@ CASE _eState OF
 	E_AXIS_PTP_STATE.WAIT_FOR_ENABLE:
 		IF _fbPower.Status AND (NOT _xError) THEN
 			_xBusy := FALSE;
+			_xDone := TRUE;
 			_eState := E_AXIS_PTP_STATE.ENABLED;
 		END_IF
 		
@@ -220,6 +248,8 @@ CASE _eState OF
 		
 		
 	E_AXIS_PTP_STATE.ENABLED:
+		_xDone := FALSE;
+		
 		IF _xExecuteMoveAbs THEN
 			_xExecuteMoveAbs := FALSE;
 			_xBusy := TRUE;
@@ -248,6 +278,13 @@ CASE _eState OF
 			_eState := E_AXIS_PTP_STATE.MOVING_VELOCITY;
 		END_IF
 		
+		IF _xExecuteMoveModulo THEN
+			_xExecuteMoveModulo := FALSE;
+			_xStartMoveModulo := TRUE;
+			_xBusy := TRUE;
+			_eState := E_AXIS_PTP_STATE.MOVING_MODULO;
+		END_IF
+	
 		IF (NOT xEnable) AND (NOT _xError) THEN
 			_xEnable := FALSE;
 			_xBusy := TRUE;
@@ -265,6 +302,7 @@ CASE _eState OF
 		IF (NOT _fbHome.Busy) AND (NOT _xError) THEN
 			_xStartHomeing := FALSE;
 			_xBusy := FALSE;
+			_xDone := TRUE;
 			_eState := E_AXIS_PTP_STATE.ENABLED;
 		END_IF
 		
@@ -286,6 +324,7 @@ CASE _eState OF
 		IF (NOT _fbMoveAbsolute.Busy) AND (NOT _xError) THEN
 			_xStartMoveAbsolute := FALSE;
 			_xBusy := FALSE;
+			_xDone := TRUE;
 			_eState := E_AXIS_PTP_STATE.ENABLED;
 		END_IF
 		
@@ -307,6 +346,7 @@ CASE _eState OF
 		IF (NOT _fbMoveRelative.Busy) AND (NOT _xError) THEN
 			_xStartMoveRelative := FALSE;
 			_xBusy := FALSE;
+			_xDone := TRUE;
 			_eState := E_AXIS_PTP_STATE.ENABLED;
 		END_IF
 		
@@ -331,11 +371,41 @@ CASE _eState OF
 			_eState := E_AXIS_PTP_STATE.ERROR;
 		END_IF
 		
+		IF _xExecuteHalt THEN
+			_xExecuteHalt := FALSE;
+			_xStartMoveVelocity := FALSE;
+			_xHalt := TRUE;
+			_eState := E_AXIS_PTP_STATE.HALTING;
+		END_IF
+		
+
+	E_AXIS_PTP_STATE.MOVING_MODULO:
+		IF _fbMoveModulo.Done THEN
+			_xStartMoveModulo := FALSE;
+			_xBusy := FALSE;
+			_xDone := TRUE;
+			_eState := E_AXIS_PTP_STATE.ENABLED;
+		END_IF
+		
+		IF _xExecuteHalt THEN
+			_xExecuteHalt := FALSE;
+			_xStartMoveModulo := FALSE;
+			_xHalt := TRUE;
+			_eState := E_AXIS_PTP_STATE.HALTING;
+		END_IF
+		
+		IF _xError THEN
+			_xStartMoveModulo := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+	
 	
 	E_AXIS_PTP_STATE.HALTING:
 		IF (NOT _fbHalt.Busy) AND (NOT _xError) THEN
 			_xHalt := FALSE;
 			_xBusy := FALSE;
+			_xDone := TRUE;
 			_eState := E_AXIS_PTP_STATE.ENABLED;
 		END_IF
 		
@@ -349,6 +419,7 @@ CASE _eState OF
 	E_AXIS_PTP_STATE.WAIT_FOR_DISABLE:
 		IF (NOT _fbPower.Status) AND (NOT _xError) THEN
 			_xBusy := FALSE;
+			_xDone := TRUE;
 			_eState := E_AXIS_PTP_STATE.OFF;
 		END_IF
 		
@@ -387,6 +458,7 @@ xEnabled := _fbPower.Status;
 xHomed := _fbAxis.Status.Homed;
 lrActPosition := _fbAxis.NcToPlc.ActPos;
 xIsStopped := _fbAxis.Status.StandStill OR _fbAxis.Status.Disabled;
+xDone := _xDone;
 xBusy := _xBusy;
 xError := _xError;]]></ST>
     </Implementation>
@@ -403,11 +475,11 @@ END_IF]]></ST>
     <Method Name="M_Homing" Id="{816a1a43-38da-49f0-b902-c1027a6ba127}">
       <Declaration><![CDATA[METHOD M_Homing : BOOL
 VAR_INPUT
-	lrHomingPosition : LREAL;
+	lrHomingPosition : LREAL := Tc2_MC2.DEFAULT_HOME_POSITION;
 	eHomingMode : MC_HomingMode := MC_HomingMode.MC_DefaultHoming;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+        <ST><![CDATA[IF _xCanExecNewCmd THEN
 	_xBusy := TRUE;
 	_lrHomingPos := lrHomingPosition;
 	_eHomingMode := eHomingMode;
@@ -424,7 +496,7 @@ VAR_INPUT
 	lrTargetPos : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+        <ST><![CDATA[IF _xCanExecNewCmd THEN
 	_xBusy := TRUE;
 	_lrTargetPosition := lrTargetPos;
 	_xExecuteMoveAbs := TRUE;
@@ -435,13 +507,31 @@ END_IF
 ]]></ST>
       </Implementation>
     </Method>
+    <Method Name="M_MoveModulo" Id="{1a5eaefa-5bb1-4438-b3e0-80c1a34fc0ee}">
+      <Declaration><![CDATA[METHOD M_MoveModulo : BOOL
+VAR_INPUT
+	lrTargetPos : LREAL;
+	eMoveDirection : MC_Direction;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF _xCanExecNewCmd THEN
+	_xBusy := TRUE;
+	_lrTargetPosition := lrTargetPos;
+	_eMoveDirection := eMoveDirection;
+	_xExecuteMoveModulo := TRUE;
+	M_MoveModulo := TRUE;
+ELSE
+	M_MoveModulo := FALSE;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
     <Method Name="M_MoveRel" Id="{51e5fe82-5e25-4de0-84c7-b4d6560c312f}">
       <Declaration><![CDATA[METHOD M_MoveRel : BOOL;
 VAR_INPUT
 	lrRelDist : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+        <ST><![CDATA[IF _xCanExecNewCmd THEN
 	_xBusy := TRUE;
 	_lrRelativeDistance := lrRelDist;
 	_xExecuteMoveRel:= TRUE;
@@ -458,10 +548,10 @@ VAR_INPUT
 END_VAR
 ]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+        <ST><![CDATA[IF _xCanExecNewCmd THEN
 	_xBusy := TRUE;	
 	_xExecuteMoveVelocity := TRUE;
-	_eMoveVelDirection := eDirection;
+	_eMoveDirection := eDirection;
 	M_MoveVelocity := TRUE;
 ELSE
 	M_MoveVelocity := FALSE;

BasicComponents/POUs/Components/Controller/FB_PI.TcPOU

@@ -6,7 +6,7 @@ VAR_INPUT
 	rSP : REAL;
 	rPV : REAL;
 	rKp : REAL;
-	rTn : REAL;
+	rTn : REAL; // In seconds
 	
 	xEnable : BOOL;
 	xSaturatedUpper : BOOL := FALSE;
@@ -19,23 +19,21 @@ VAR
 	
 	_rError : REAL := 0.0;
 	_rIntegral : REAL := 0.0;
-	_rProportinal : REAL := 0.0;
+	_rProportional : REAL := 0.0;
 	
 	_rDeltaIntegral : REAL := 0.0;
 	
-	_fbGetCurTaskIdx : GETCURTASKINDEX;
 	_rT : REAL;
 	
-	_xFirstCylce : BOOL := TRUE;
+	_xFirstCycle : BOOL := TRUE;
 END_VAR
 ]]></Declaration>
     <Implementation>
-      <ST><![CDATA[IF _xFirstCylce THEN
+      <ST><![CDATA[IF _xFirstCycle THEN
-	_xFirstCylce := FALSE;
+	_xFirstCycle := FALSE;
 	
 	// Get current task time
-	_fbGetCurTaskIdx();
+	_rT := LREAL_TO_REAL(UDINT_TO_LREAL(_TaskInfo[GETCURTASKINDEXEX()].CycleTime) * 1E-7);
-	_rT := LREAL_TO_REAL(UDINT_TO_LREAL(TwinCAT_SystemInfoVarList._TASKInfo[_fbGetCurTaskIdx.index].CycleTime) * 10E-5);
 END_IF
 
 
@@ -49,13 +47,10 @@ ELSE
 END_IF
 
 // Calculate proportinal part
-_rProportinal := rKp * _rError;
+_rProportional := rKp * _rError;
-
-// Calculate controller output
-rMV := _rProportinal + _rIntegral;
 
 // Calculate integral for this step
-IF rTn <> 0 THEN
+IF rTn > 0 THEN
 	_rDeltaIntegral := (rKp * _rT / rTn) * _rError;
 ELSE
 	_rDeltaIntegral := 0;
@@ -74,9 +69,12 @@ END_IF
 _rIntegral := _rIntegral + _rDeltaIntegral;
 
 // Reset integral with deactivated integral time
-IF (rTn = 0.0) AND (_rIntegral <> 0) THEN
+IF (rTn <= 0.0) THEN
 	_rIntegral := 0.0;
-END_IF]]></ST>
+END_IF
+
+// Calculate controller output
+rMV := _rProportional + _rIntegral;]]></ST>
     </Implementation>
   </POU>
 </TcPlcObject>

BasicComponents/POUs/Components/Controller/FB_PID.TcPOU

@@ -0,0 +1,108 @@
+<?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 := LREAL_TO_REAL(UDINT_TO_LREAL(_TaskInfo[GETCURTASKINDEXEX()].CycleTime) * 1E-7);
+	
+	_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>

BasicComponents/POUs/Components/Controller/FB_RampGen.TcPOU

@@ -0,0 +1,86 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_RampGen" Id="{61136989-8ee2-49fa-af2b-1b579e85353f}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_RampGen
+VAR_INPUT
+    rTarget     : REAL;   // Zielwert
+    rRiseRate   : REAL;   // Steigung positiv [Einheit/s]
+    rFallRate   : REAL;   // Steigung negativ [Einheit/s]
+    xEnable     : BOOL;
+    xHold       : BOOL;   // Rampe einfrieren
+    xReset      : BOOL;   // Ausgang sofort auf Ziel setzen
+END_VAR
+
+VAR_OUTPUT
+    rOut        : REAL;   // Rampenausgang
+    xBusy       : BOOL;   // TRUE solange Rampe läuft
+END_VAR
+
+VAR
+    _xFirstCycle : BOOL := TRUE;
+    _rT          : REAL := 0.0;
+    _rDiff       : REAL := 0.0;
+    _rStep       : REAL := 0.0;
+END_VAR]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF _xFirstCycle THEN
+    _xFirstCycle := FALSE;
+
+    // Task-Zeit in Sekunden (100 ns Ticks)
+    _rT := LREAL_TO_REAL(
+        UDINT_TO_LREAL(_TaskInfo[GETCURTASKINDEXEX()].CycleTime) * 1E-7
+    );
+
+    rOut := rTarget;
+END_IF
+
+
+IF NOT xEnable THEN
+    rOut := rTarget;
+    xBusy := FALSE;
+    RETURN;
+END_IF
+
+
+IF xReset THEN
+    rOut := rTarget;
+    xBusy := FALSE;
+    RETURN;
+END_IF
+
+
+IF xHold THEN
+    xBusy := (ABS(rTarget - rOut) > 0.0);
+    RETURN;
+END_IF
+
+
+_rDiff := rTarget - rOut;
+
+
+IF _rDiff > 0 THEN
+    // Aufwärtsrampe
+    _rStep := rRiseRate * _rT;
+
+    IF _rStep > _rDiff THEN
+        rOut := rTarget;
+    ELSE
+        rOut := rOut + _rStep;
+    END_IF
+
+ELSIF _rDiff < 0 THEN
+    // Abwärtsrampe
+    _rStep := rFallRate * _rT;
+
+    IF _rStep > ABS(_rDiff) THEN
+        rOut := rTarget;
+    ELSE
+        rOut := rOut - _rStep;
+    END_IF
+END_IF
+
+
+xBusy := (ABS(rTarget - rOut) > 0.0);]]></ST>
+    </Implementation>
+  </POU>
+</TcPlcObject>