From fcb69b590d7c94982a45303bb15c662408191bca Mon Sep 17 00:00:00 2001
From: Matthias Heisig <m.heisig@heisig-gmbh.de>
Date: Mon, 2 Feb 2026 09:56:21 +0100
Subject: [PATCH] Added Axis PTP FB

---
 BasicComponents/BasicComponents.plcproj       |   9 +-
 .../Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT |  24 +
 .../POUs/Components/AxisPTP/FB_AxisPTP.TcPOU  | 504 ++++++++++++++++++
 3 files changed, 536 insertions(+), 1 deletion(-)
 create mode 100644 BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT
 create mode 100644 BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU

diff --git a/BasicComponents/BasicComponents.plcproj b/BasicComponents/BasicComponents.plcproj
index 492bd9f..f9370b7 100644
--- a/BasicComponents/BasicComponents.plcproj
+++ b/BasicComponents/BasicComponents.plcproj
@@ -9,7 +9,7 @@
     <WriteProductVersion>false</WriteProductVersion>
     <GenerateTpy>false</GenerateTpy>
     <Name>BasicComponents</Name>
-    <ProgramVersion>3.1.4026.19</ProgramVersion>
+    <ProgramVersion>3.1.4026.20</ProgramVersion>
     <Application>{7cc9c530-510c-48e6-a294-2e6b5e6fa820}</Application>
     <TypeSystem>{951d7a0c-817c-4013-8204-ecd29ee0162e}</TypeSystem>
     <Implicit_Task_Info>{e6d3915d-876b-4120-9e42-92be330ed750}</Implicit_Task_Info>
@@ -67,6 +67,12 @@
     <Compile Include="POUs\Components\Analog\Types\ST_ANALOG_IO_CONFIG.TcDUT">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="POUs\Components\AxisPTP\E_AXIS_PTP_STATE.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Components\AxisPTP\FB_AxisPTP.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="POUs\Components\EventListener\FB_EventListener.TcPOU">
       <SubType>Code</SubType>
     </Compile>
@@ -211,6 +217,7 @@
     <Folder Include="GVLs" />
     <Folder Include="POUs\Components\Analog" />
     <Folder Include="POUs\Components\Analog\Types" />
+    <Folder Include="POUs\Components\AxisPTP" />
     <Folder Include="POUs\Components\EventListener" />
     <Folder Include="POUs\Components\EventListener\Types" />
     <Folder Include="POUs\Components\Motor">
diff --git a/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT b/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT
new file mode 100644
index 0000000..8c6fe63
--- /dev/null
+++ b/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="E_AXIS_PTP_STATE" Id="{e713a96e-1301-4139-8827-994d0c38662a}">
+    <Declaration><![CDATA[{attribute 'qualified_only'}
+{attribute 'strict'}
+{attribute 'to_string'}
+TYPE E_AXIS_PTP_STATE :
+(
+	OFF := 0,
+	WAIT_FOR_ENABLE,
+	ENABLED,
+	HOMING,
+	MOVING_ABSOLUTE,
+	MOVING_RELATIVE,
+	MOVING_VELOCITY,
+	HALTING,
+	WAIT_FOR_DISABLE,
+	ERROR,
+	RESET
+);
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file
diff --git a/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU b/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU
new file mode 100644
index 0000000..7f08312
--- /dev/null
+++ b/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU
@@ -0,0 +1,504 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_AxisPTP" Id="{5a7035b7-f11b-403b-9828-9d2d1f662a54}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_AxisPTP
+VAR_INPUT
+	xEnable : BOOL;
+	xCalibrationCam AT %I* : BOOL;
+	xInvertCalibrationCam : BOOL := FALSE;
+	xEnablePositive : BOOL := TRUE;
+	xEnableNegative : BOOL := TRUE;;
+	rOverride : REAL := 100.0;
+	
+	lrVelocity : LREAL;
+	lrAcceleration : LREAL := 0.0;
+	lrDecelleration : LREAL := 0.0;
+	lrJerk : LREAL := 0.0;
+	
+	xConfirmAlarms : BOOL;
+END_VAR
+VAR_OUTPUT
+	xEnabled : BOOL;
+	xHomed : BOOL;
+	lrActPosition : LREAL;
+	xIsStopped : BOOL;
+	xBusy : BOOL;
+	xError : BOOL;
+END_VAR
+VAR
+	// ========
+	// Axis fbs
+	// ========
+	
+	_fbAxis : AXIS_REF;
+	_fbPower : MC_Power;
+	_fbHome : MC_Home;
+	_fbMoveAbsolute : MC_MoveAbsolute;
+	_fbMoveRelative : MC_MoveRelative;
+	_fbMoveVelocity : MC_MoveVelocity;
+	_fbHalt : MC_Halt;
+	_fbReset : MC_Reset;
+	
+	
+	// ======================
+	// Axis fbs control flags
+	// ======================
+	
+	_xEnable : BOOL;
+	_xStartHomeing : BOOL;
+	_xStartMoveAbsolute : BOOL;
+	_xStartMoveRelative : BOOL;
+	_xStartMoveVelocity : BOOL;
+	_xHalt : BOOL;
+	_xReset : BOOL;
+	
+	
+	// =====================
+	// Sequence control data
+	// =====================
+	
+	_xExecuteHoming : BOOL;
+	_xExecuteMoveAbs : BOOL;
+	_xExecuteMoveRel : BOOL;
+	_xExecuteMoveVelocity : BOOL;
+	_xExecuteHalt : BOOL;
+	
+	
+	// =========================
+	// Internal helper variables
+	// =========================
+	
+	_lrTargetPosition : LREAL;
+	_lrRelativeDistance : LREAL;
+	_lrHomingPos : LREAL;
+	_eHomingMode : MC_HomingMode := MC_HomingMode.MC_DefaultHoming;
+	_xCalibrationCam : BOOL;
+	
+	_eMoveVelDirection : MC_Direction;
+	
+	_eState : E_AXIS_PTP_STATE;
+	
+	
+	// =============
+	// Ouput buffers
+	// =============
+	
+	_xBusy : BOOL;
+	_xError : BOOL;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[// Call axis interface
+_fbAxis.ReadStatus();
+
+IF xInvertCalibrationCam THEN
+	_xCalibrationCam := NOT xCalibrationCam;
+ELSE
+	_xCalibrationCam := xCalibrationCam;
+END_IF
+
+
+// =================
+// Call axis classes
+// =================
+
+_fbPower(
+	Axis:= _fbAxis, 
+	Enable:= _xEnable, 
+	Enable_Positive:= xEnablePositive, 
+	Enable_Negative:= xEnableNegative, 
+	Override:= rOverride);
+	
+IF _fbPower.Error THEN
+	_xError := TRUE;
+END_IF
+
+
+_fbHome(
+	Axis:= _fbAxis, 
+	Execute:= _xStartHomeing, 
+	Position:= _lrHomingPos, 
+	HomingMode:= _eHomingMode, 
+	bCalibrationCam:= _xCalibrationCam);
+	
+IF _fbHome.Error THEN
+	_xError := TRUE;
+END_IF
+
+_fbReset(
+	Axis:= _fbAxis, 
+	Execute:= _xReset);
+	
+IF _fbReset.Error THEN
+	_xError := TRUE;
+END_IF
+
+
+_fbMoveAbsolute(
+	Axis:= _fbAxis, 
+	Execute:= _xStartMoveAbsolute, 
+	Position:= _lrTargetPosition, 
+	Velocity:= lrVelocity, 
+	Acceleration:= 0, 
+	Deceleration:= 0,
+	Jerk:= 0);
+	
+IF _fbMoveAbsolute.Error THEN
+	_xError := TRUE;
+END_IF
+
+
+_fbMoveRelative(
+	Axis:= _fbAxis, 
+	Execute:= _xStartMoveRelative, 
+	Distance:= _lrRelativeDistance, 
+	Velocity:= lrVelocity, 
+	Acceleration:= 0, 
+	Deceleration:= 0,
+	Jerk:= 0);
+	
+IF _fbMoveRelative.Error THEN
+	_xError := TRUE;
+END_IF
+
+
+_fbMoveVelocity(
+	Axis:= _fbAxis, 
+	Execute:= _xStartMoveVelocity, 
+	Velocity:= lrVelocity, 
+	Acceleration:= 0, 
+	Deceleration:= 0, 
+	Jerk:= 0, 
+	Direction:= _eMoveVelDirection);
+	
+IF _fbMoveVelocity.Error THEN
+	_xError := TRUE;
+END_IF
+
+
+_fbHalt(
+	Axis:= _fbAxis, 
+	Execute:= _xHalt, 
+	Deceleration:= 0,
+	BufferMode := MC_BufferMode.MC_Aborting,
+	Jerk:= 0);
+	
+IF _fbHalt.Error THEN
+	_xError := TRUE;
+END_IF
+
+
+// ====================
+// Handle state machine
+// ====================
+
+CASE _eState OF
+	E_AXIS_PTP_STATE.OFF:
+		IF xEnable THEN
+			_xEnable := TRUE;
+			_xBusy := TRUE;
+			_eState := E_AXIS_PTP_STATE.WAIT_FOR_ENABLE;
+		END_IF
+		
+		IF _xError THEN
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+		
+		
+	E_AXIS_PTP_STATE.WAIT_FOR_ENABLE:
+		IF _fbPower.Status AND (NOT _xError) THEN
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENABLED;
+		END_IF
+		
+		IF _xError THEN
+			_xEnable := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+		
+		
+	E_AXIS_PTP_STATE.ENABLED:
+		IF _xExecuteMoveAbs THEN
+			_xExecuteMoveAbs := FALSE;
+			_xBusy := TRUE;
+			_xStartMoveAbsolute := TRUE;
+			_eState := E_AXIS_PTP_STATE.MOVING_ABSOLUTE;
+		END_IF
+				
+		IF _xExecuteMoveRel THEN
+			_xExecuteMoveRel := FALSE;
+			_xBusy := TRUE;
+			_xStartMoveRelative := TRUE;
+			_eState := E_AXIS_PTP_STATE.MOVING_RELATIVE;
+		END_IF
+		
+		IF _xExecuteHoming THEN
+			_xExecuteHoming := FALSE;
+			_xStartHomeing := TRUE;
+			_xBusy := TRUE;
+			_eState := E_AXIS_PTP_STATE.HOMING;
+		END_IF
+		
+		IF _xExecuteMoveVelocity THEN
+			_xExecuteMoveVelocity := FALSE;
+			_xStartMoveVelocity := TRUE;
+			_xBusy := TRUE;
+			_eState := E_AXIS_PTP_STATE.MOVING_VELOCITY;
+		END_IF
+	
+		IF (NOT xEnable) AND (NOT _xError) THEN
+			_xEnable := FALSE;
+			_xBusy := TRUE;
+			_eState := E_AXIS_PTP_STATE.WAIT_FOR_DISABLE;
+		END_IF
+		
+		IF _xError THEN
+			_xEnable := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+		
+		
+	E_AXIS_PTP_STATE.HOMING:
+		IF (NOT _fbHome.Busy) AND (NOT _xError) THEN
+			_xStartHomeing := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENABLED;
+		END_IF
+		
+		IF _xExecuteHalt THEN
+			_xExecuteHalt := FALSE;
+			_xStartHomeing := FALSE;
+			_xHalt := TRUE;
+			_eState := E_AXIS_PTP_STATE.HALTING;
+		END_IF
+		
+		IF _xError THEN
+			_xStartHomeing := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+	
+	
+	E_AXIS_PTP_STATE.MOVING_ABSOLUTE:
+		IF (NOT _fbMoveAbsolute.Busy) AND (NOT _xError) THEN
+			_xStartMoveAbsolute := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENABLED;
+		END_IF
+		
+		IF _xExecuteHalt THEN
+			_xExecuteHalt := FALSE;
+			_xStartMoveAbsolute := FALSE;
+			_xHalt := TRUE;
+			_eState := E_AXIS_PTP_STATE.HALTING;
+		END_IF
+		
+		IF _xError THEN
+			_xStartMoveAbsolute := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+
+
+	E_AXIS_PTP_STATE.MOVING_RELATIVE:
+		IF (NOT _fbMoveRelative.Busy) AND (NOT _xError) THEN
+			_xStartMoveRelative := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENABLED;
+		END_IF
+		
+		IF _xExecuteHalt THEN
+			_xExecuteHalt := FALSE;
+			_xStartMoveAbsolute := FALSE;
+			_xHalt := TRUE;
+			_eState := E_AXIS_PTP_STATE.HALTING;
+		END_IF
+		
+		IF _xError THEN
+			_xStartMoveRelative := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+	
+	
+	E_AXIS_PTP_STATE.MOVING_VELOCITY:
+		IF _fbMoveVelocity.Error THEN
+			_xBusy := FALSE;
+			_xStartMoveVelocity := 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;
+			_eState := E_AXIS_PTP_STATE.ENABLED;
+		END_IF
+		
+		IF _xError THEN
+			_xHalt := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+	
+		
+	E_AXIS_PTP_STATE.WAIT_FOR_DISABLE:
+		IF (NOT _fbPower.Status) AND (NOT _xError) THEN
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.OFF;
+		END_IF
+		
+		IF _xError THEN
+			_xEnable := FALSE;
+			_xBusy := FALSE;
+			_eState := E_AXIS_PTP_STATE.ERROR;
+		END_IF
+	
+	
+	E_AXIS_PTP_STATE.ERROR:
+		IF xConfirmAlarms THEN
+			_xBusy := FALSE;
+			_xReset := TRUE;
+			_xError := FALSE;
+			_eState := E_AXIS_PTP_STATE.RESET;
+		END_IF
+	
+	
+	E_AXIS_PTP_STATE.RESET:
+		IF _fbReset.Done THEN
+			_xReset := FALSE;
+			_xError := FALSE;
+			_eState := E_AXIS_PTP_STATE.OFF;
+		END_IF
+
+	
+	ELSE
+		_eState := E_AXIS_PTP_STATE.ERROR;
+		_xError := TRUE;
+END_CASE
+
+
+// Copy internal buffers to outputs
+xEnabled := _fbPower.Status;
+xHomed := _fbAxis.Status.Homed;
+lrActPosition := _fbAxis.NcToPlc.ActPos;
+xIsStopped := _fbAxis.Status.StandStill OR _fbAxis.Status.Disabled;
+xBusy := _xBusy;
+xError := _xError;]]></ST>
+    </Implementation>
+    <Method Name="M_Halt" Id="{f6e3b049-4121-4299-b0f5-55178ecb35c3}">
+      <Declaration><![CDATA[METHOD M_Halt
+VAR_OUTPUT
+	eResultCode : E_SequenceResultCode;
+	eFailureCode : E_SequenceFailureCode;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF _xBusy THEN
+	_xExecuteHalt := TRUE;
+	eResultCode := E_SequenceResultCode.EXECUTING;
+	eFailureCode := E_SequenceFailureCode.NONE;
+ELSE
+	eResultCode := E_SequenceResultCode.DONE;
+	eFailureCode := E_SequenceFailureCode.NONE;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="M_Homing" Id="{816a1a43-38da-49f0-b902-c1027a6ba127}">
+      <Declaration><![CDATA[METHOD M_Homing
+VAR_INPUT
+	lrHomingPosition : LREAL;
+	eHomingMode : MC_HomingMode := MC_HomingMode.MC_DefaultHoming;
+END_VAR
+VAR_OUTPUT
+	eResultCode : E_SequenceResultCode;
+	eFailureCode : E_SequenceFailureCode;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+	_xBusy := TRUE;
+	_lrHomingPos := lrHomingPosition;
+	_eHomingMode := eHomingMode;
+	_xExecuteHoming := TRUE;
+	eResultCode := E_SequenceResultCode.EXECUTING;
+	eFailureCode := E_SequenceFailureCode.NONE;
+ELSE
+	eResultCode := E_SequenceResultCode.FAILED;
+	eFailureCode := E_SequenceFailureCode.OTHER;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="M_MoveAbs" Id="{f67a4861-cdb0-4dd2-93c8-1ae9dbc9a9a6}">
+      <Declaration><![CDATA[METHOD M_MoveAbs
+VAR_INPUT
+	lrTargetPos : LREAL;
+END_VAR
+VAR_OUTPUT
+	eResultCode : E_SequenceResultCode;
+	eFailureCode : E_SequenceFailureCode;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+	_xBusy := TRUE;
+	_lrTargetPosition := lrTargetPos;
+	_xExecuteMoveAbs := TRUE;
+	eResultCode := E_SequenceResultCode.EXECUTING;
+	eFailureCode := E_SequenceFailureCode.NONE;
+ELSE
+	eResultCode := E_SequenceResultCode.FAILED;
+	eFailureCode := E_SequenceFailureCode.OTHER;
+END_IF
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="M_MoveRel" Id="{51e5fe82-5e25-4de0-84c7-b4d6560c312f}">
+      <Declaration><![CDATA[METHOD M_MoveRel
+VAR_INPUT
+	lrRelDist : LREAL;
+END_VAR
+VAR_OUTPUT
+	eResultCode : E_SequenceResultCode;
+	eFailureCode : E_SequenceFailureCode;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+	_xBusy := TRUE;
+	_lrRelativeDistance := lrRelDist;
+	_xExecuteMoveRel:= TRUE;
+	eResultCode := E_SequenceResultCode.EXECUTING;
+	eFailureCode := E_SequenceFailureCode.NONE;
+ELSE
+	eResultCode := E_SequenceResultCode.FAILED;
+	eFailureCode := E_SequenceFailureCode.OTHER;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="M_MoveVelocity" Id="{fd4ad471-f522-45f2-a138-ebe509907cf6}">
+      <Declaration><![CDATA[METHOD M_MoveVelocity
+VAR_INPUT
+	eDirection : MC_Direction := MC_Direction.MC_Positive_Direction;
+END_VAR
+VAR_OUTPUT
+	eResultCode : E_SequenceResultCode;
+	eFailureCode : E_SequenceFailureCode;
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF (NOT _xBusy) AND _fbPower.Status THEN
+	_xBusy := TRUE;	
+	_xExecuteMoveVelocity := TRUE;
+	_eMoveVelDirection := eDirection;
+	eResultCode := E_SequenceResultCode.EXECUTING;
+	eFailureCode := E_SequenceFailureCode.NONE;
+ELSE
+	eResultCode := E_SequenceResultCode.FAILED;
+	eFailureCode := E_SequenceFailureCode.OTHER;
+END_IF
+	]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file