diff --git a/BasicComponents/BasicComponents.plcproj b/BasicComponents/BasicComponents.plcproj
index 124a628..14875d4 100644
--- a/BasicComponents/BasicComponents.plcproj
+++ b/BasicComponents/BasicComponents.plcproj
@@ -45,6 +45,9 @@
     </SelectedLibraryCategories>
   </PropertyGroup>
   <ItemGroup>
+    <Compile Include="DUTs\E_CmdResult.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="PlcTask.TcTTO">
       <SubType>Code</SubType>
     </Compile>
@@ -72,6 +75,9 @@
     <Compile Include="POUs\Components\AxisPTP\FB_AxisPTP.TcPOU">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="POUs\Components\AxisPTP\ST_AxisPTP_IO.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="POUs\Components\Controller\FB_Limit.TcPOU">
       <SubType>Code</SubType>
     </Compile>
diff --git a/BasicComponents/DUTs/E_CmdResult.TcDUT b/BasicComponents/DUTs/E_CmdResult.TcDUT
new file mode 100644
index 0000000..7e49151
--- /dev/null
+++ b/BasicComponents/DUTs/E_CmdResult.TcDUT
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="E_CmdResult" Id="{280b22d6-e4fc-482b-8eba-52154be334ab}">
+    <Declaration><![CDATA[{attribute 'qualified_only'}
+{attribute 'strict'}
+{attribute 'to_string'}
+TYPE E_CmdResult :
+(
+	// Command accepted and in execution
+	ACCEPTED := 0,
+	
+	// Component currently busy with another command
+	BUSY := 1,
+	
+	// Command currently not allowed
+	REJECTED := 2,
+	
+	// Component has error
+	ERROR := 3
+) INT;
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file
diff --git a/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT b/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT
index 40c18a8..7814430 100644
--- a/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT
+++ b/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT
@@ -6,16 +6,24 @@
 {attribute 'to_string'}
 TYPE E_AXIS_PTP_STATE :
 (
-	OFF := 0,
-	WAIT_FOR_ENABLE,
-	ENABLED,
-	HOMING,
-	MOVING_ABSOLUTE,
-	MOVING_RELATIVE,
-	MOVING_VELOCITY,
-	MOVING_MODULO,
-	HALTING,
-	WAIT_FOR_DISABLE,
+	IDLE := 0,
+	START_ENABLE,
+	WAIT_ENABLE,
+	START_DISABLE,
+	WAIT_DISABLE,
+	START_HOMING,
+	WAIT_HOMING,
+	START_MOVING_ABSOLUTE,
+	WAIT_MOVING_ABSOLUTE,
+	START_MOVING_RELATIVE,
+	WAIT_MOVING_RELATIVE,
+	START_MOVING_VELOCITY,
+	WAIT_MOVING_VELOCITY,
+	START_MOVING_MODULO,
+	WAIT_MOVING_MODULO,
+	START_HALTING,
+	WAIT_HALTING,
+	ENTER_ERROR,
 	ERROR,
 	RESET
 );
diff --git a/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU b/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU
index 61f2d5d..6232180 100644
--- a/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU
+++ b/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU
@@ -3,8 +3,6 @@
   <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;
@@ -22,16 +20,20 @@ VAR_OUTPUT
 	xHomed : BOOL;
 	lrActPosition : LREAL;
 	xIsStopped : BOOL;
+	
+	// Outputs for execute pattern
 	xBusy : BOOL;
-	xDone : BOOL;
+	xDone : BOOL := TRUE;
 	xError : BOOL;
 END_VAR
+VAR_IN_OUT
+	stIO : ST_AxisPTP_IO;
+END_VAR
 VAR
 	// ========
 	// Axis fbs
 	// ========
 	
-	_fbAxis : AXIS_REF;
 	_fbPower : MC_Power;
 	_fbHome : MC_Home;
 	_fbMoveAbsolute : MC_MoveAbsolute;
@@ -42,20 +44,6 @@ VAR
 	_fbReset : MC_Reset;
 	
 	
-	// ======================
-	// Axis fbs control flags
-	// ======================
-	
-	_xEnable : BOOL;
-	_xStartHomeing : BOOL;
-	_xStartMoveAbsolute : BOOL;
-	_xStartMoveRelative : BOOL;
-	_xStartMoveVelocity : BOOL;
-	_xStartMoveModulo : BOOL;
-	_xHalt : BOOL;
-	_xReset : BOOL;
-	
-	
 	// =====================
 	// Sequence control data
 	// =====================
@@ -82,30 +70,17 @@ VAR
 	
 	_xCanExecNewCmd : BOOL;
 	
-	_eState : E_AXIS_PTP_STATE;
-	
-	
-	// =============
-	// Ouput buffers
-	// =============
-	
-	_xDone : BOOL;
-	_xBusy : BOOL;
-	_xError : BOOL;
+	_eState : E_AXIS_PTP_STATE := E_AXIS_PTP_STATE.IDLE;
 END_VAR
 ]]></Declaration>
     <Implementation>
       <ST><![CDATA[// Call axis interface
-_fbAxis.ReadStatus();
-
-IF _fbAxis.Status.Error THEN
-	_xError := TRUE;
-END_IF
+stIO.io_fbAxisRef.ReadStatus();
 
 IF xInvertCalibrationCam THEN
-	_xCalibrationCam := NOT xCalibrationCam;
+	_xCalibrationCam := NOT stIO.i_xCalibrationCam;
 ELSE
-	_xCalibrationCam := xCalibrationCam;
+	_xCalibrationCam := stIO.i_xCalibrationCam;
 END_IF
 
 
@@ -114,261 +89,167 @@ END_IF
 // =================
 
 _fbPower(
-	Axis:= _fbAxis, 
-	Enable:= _xEnable, 
+	Axis:= stIO.io_fbAxisRef, 
 	Enable_Positive:= xEnablePositive, 
 	Enable_Negative:= xEnableNegative, 
 	Override:= rOverride);
-	
-IF _fbPower.Error THEN
-	_xError := TRUE;
-END_IF
-
 
 _fbHome(
-	Axis:= _fbAxis, 
-	Execute:= _xStartHomeing, 
+	Axis:= stIO.io_fbAxisRef, 
 	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
 
+_fbReset(Axis:= stIO.io_fbAxisRef);
 
 _fbMoveAbsolute(
-	Axis:= _fbAxis, 
-	Execute:= _xStartMoveAbsolute, 
+	Axis:= stIO.io_fbAxisRef, 
 	Position:= _lrTargetPosition, 
 	Velocity:= lrVelocity, 
 	Acceleration:= 0, 
 	Deceleration:= 0,
 	Jerk:= 0);
-	
-IF _fbMoveAbsolute.Error THEN
-	_xError := TRUE;
-END_IF
-
 
 _fbMoveRelative(
-	Axis:= _fbAxis, 
-	Execute:= _xStartMoveRelative, 
+	Axis:= stIO.io_fbAxisRef, 
 	Distance:= _lrRelativeDistance, 
 	Velocity:= lrVelocity, 
 	Acceleration:= 0, 
 	Deceleration:= 0,
 	Jerk:= 0);
-	
-IF _fbMoveRelative.Error THEN
-	_xError := TRUE;
-END_IF
-
 
 _fbMoveVelocity(
-	Axis:= _fbAxis, 
-	Execute:= _xStartMoveVelocity, 
+	Axis:= stIO.io_fbAxisRef, 
 	Velocity:= lrVelocity, 
 	Acceleration:= 0, 
 	Deceleration:= 0, 
 	Jerk:= 0, 
 	Direction:= _eMoveDirection);
 	
-IF _fbMoveVelocity.Error THEN
-	_xError := TRUE;
-END_IF
-
 _fbMoveModulo(
-	Axis:= _fbAxis, 
-	Execute:= _xStartMoveModulo, 
+	Axis:= stIO.io_fbAxisRef, 
 	Position:= _lrTargetPosition, 
 	Velocity:= lrVelocity, 
 	Acceleration:= 0, 
 	Deceleration:= 0, 
 	Jerk:= 0, 
 	Direction:= _eMoveDirection);
-	
-IF _fbMoveModulo.Error THEN
-	_xError := TRUE;
-END_IF
-
 
 _fbHalt(
-	Axis:= _fbAxis, 
-	Execute:= _xHalt, 
+	Axis:= stIO.io_fbAxisRef, 
 	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;
-			_xDone := FALSE;
-			_eState := E_AXIS_PTP_STATE.WAIT_FOR_ENABLE;
-		END_IF
-		
-		IF _xError THEN
-			_xBusy := FALSE;
+	E_AXIS_PTP_STATE.IDLE:
+		// Check for error
+		IF stIO.io_fbAxisRef.Status.Error THEN
 			_eState := E_AXIS_PTP_STATE.ERROR;
 		END_IF
 		
+	// =========
+	// Enableing
+	// =========
+	E_AXIS_PTP_STATE.START_ENABLE:
+		_fbPower.Enable := TRUE;
+		_eState := E_AXIS_PTP_STATE.WAIT_ENABLE;
 		
-	E_AXIS_PTP_STATE.WAIT_FOR_ENABLE:
-		IF _fbPower.Status AND (NOT _xError) THEN
-			_xBusy := FALSE;
-			_xDone := TRUE;
-			_eState := E_AXIS_PTP_STATE.ENABLED;
+	E_AXIS_PTP_STATE.WAIT_ENABLE:
+		IF _fbPower.Status AND (NOT _fbPower.Error) THEN
+			xBusy := FALSE;
+			xDone := TRUE;
+			_eState := E_AXIS_PTP_STATE.IDLE;
 		END_IF
 		
-		IF _xError THEN
-			_xEnable := FALSE;
-			_xBusy := FALSE;
-			_eState := E_AXIS_PTP_STATE.ERROR;
+		IF _fbPower.Error THEN
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 		
+	// =======
+	// Homeing
+	// =======
+	E_AXIS_PTP_STATE.START_HOMING:
+		_fbHome.Execute := TRUE;
+		_eState := E_AXIS_PTP_STATE.WAIT_HOMING;
 		
-	E_AXIS_PTP_STATE.ENABLED:
-		
-		IF _xExecuteMoveAbs THEN
-			_xExecuteMoveAbs := FALSE;
-			_xStartMoveAbsolute := TRUE;
-			_eState := E_AXIS_PTP_STATE.MOVING_ABSOLUTE;
-		END_IF
-				
-		IF _xExecuteMoveRel THEN
-			_xExecuteMoveRel := FALSE;
-			_xStartMoveRelative := TRUE;
-			_eState := E_AXIS_PTP_STATE.MOVING_RELATIVE;
+	E_AXIS_PTP_STATE.WAIT_HOMING:
+		IF _fbHome.Done THEN
+			_fbHome.Execute := FALSE;
+			xBusy := FALSE;
+			xDone := TRUE;
+			_eState := E_AXIS_PTP_STATE.IDLE;
 		END_IF
 		
-		IF _xExecuteHoming THEN
-			_xExecuteHoming := FALSE;
-			_xStartHomeing := TRUE;
-			_eState := E_AXIS_PTP_STATE.HOMING;
-		END_IF
-		
-		IF _xExecuteMoveVelocity THEN
-			_xExecuteMoveVelocity := FALSE;
-			_xStartMoveVelocity := TRUE;
-			_eState := E_AXIS_PTP_STATE.MOVING_VELOCITY;
-		END_IF
-		
-		IF _xExecuteMoveModulo THEN
-			_xExecuteMoveModulo := FALSE;
-			_xStartMoveModulo := TRUE;
-			_eState := E_AXIS_PTP_STATE.MOVING_MODULO;
+		IF _fbHome.Error THEN
+			_fbHome.Execute := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 	
-		IF (NOT xEnable) AND (NOT _xError) THEN
-			_xEnable := FALSE;
-			_xBusy := TRUE;
-			_xDone := FALSE;
-			_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;
-			_xDone := TRUE;
-			_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
+	// =============
+	// Move absolute
+	// =============
+	E_AXIS_PTP_STATE.START_MOVING_ABSOLUTE:
+		_fbMoveAbsolute.Execute := TRUE;
+		_eState := E_AXIS_PTP_STATE.WAIT_MOVING_ABSOLUTE;
 	
-	
-	E_AXIS_PTP_STATE.MOVING_ABSOLUTE:
-		IF (NOT _fbMoveAbsolute.Busy) AND (NOT _xError) THEN
-			_xStartMoveAbsolute := FALSE;
-			_xBusy := FALSE;
-			_xDone := TRUE;
-			_eState := E_AXIS_PTP_STATE.ENABLED;
+	E_AXIS_PTP_STATE.WAIT_MOVING_ABSOLUTE:
+		IF _fbMoveAbsolute.Done THEN
+			_fbMoveAbsolute.Execute := FALSE;
+			xBusy := FALSE;
+			xDone := TRUE;
+			_eState := E_AXIS_PTP_STATE.IDLE;
 		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;
+		IF _fbMoveAbsolute.Error THEN
+			_fbMoveAbsolute.Execute := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 
 
-	E_AXIS_PTP_STATE.MOVING_RELATIVE:
-		IF (NOT _fbMoveRelative.Busy) AND (NOT _xError) THEN
-			_xStartMoveRelative := FALSE;
-			_xBusy := FALSE;
-			_xDone := TRUE;
-			_eState := E_AXIS_PTP_STATE.ENABLED;
+	// =============
+	// Move relative
+	// =============
+	E_AXIS_PTP_STATE.START_MOVING_RELATIVE:
+		_fbMoveRelative.Execute := TRUE;
+		_eState := E_AXIS_PTP_STATE.WAIT_MOVING_RELATIVE;
+	
+	E_AXIS_PTP_STATE.WAIT_MOVING_RELATIVE:
+		IF _fbMoveRelative.Done THEN
+			_fbMoveRelative.Execute := FALSE;
+			xBusy := FALSE;
+			xDone := TRUE;
+			_eState := E_AXIS_PTP_STATE.IDLE;
 		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;
+		IF _fbMoveRelative.Error THEN
+			_fbMoveRelative.Execute := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 	
 	
-	E_AXIS_PTP_STATE.MOVING_VELOCITY:
+	// =============
+	// Move velocity
+	// =============
+	E_AXIS_PTP_STATE.START_MOVING_VELOCITY:
+		_fbMoveVelocity.Execute := TRUE;
+		_eState := E_AXIS_PTP_STATE.WAIT_MOVING_VELOCITY;
+	
+	E_AXIS_PTP_STATE.WAIT_MOVING_VELOCITY:
+		IF _fbMoveVelocity.Busy THEN
+			_fbMoveVelocity.Execute := FALSE;
+			xBusy := FALSE;
+			xDone := TRUE;
+			_eState := E_AXIS_PTP_STATE.IDLE;
+		END_IF
+		
 		IF _fbMoveVelocity.Error THEN
-			_xBusy := FALSE;
-			_xStartMoveVelocity := FALSE;
-			_eState := E_AXIS_PTP_STATE.ERROR;
-		END_IF
-		
-		IF _xExecuteHalt THEN
-			_xExecuteHalt := FALSE;
-			_xStartMoveVelocity := FALSE;
-			_xHalt := TRUE;
-			_eState := E_AXIS_PTP_STATE.HALTING;
+			_fbMoveVelocity.Execute := FALSE;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 		
 
@@ -389,8 +270,7 @@ CASE _eState OF
 		
 		IF _xError THEN
 			_xStartMoveModulo := FALSE;
-			_xBusy := FALSE;
-			_eState := E_AXIS_PTP_STATE.ERROR;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 	
 	
@@ -403,9 +283,7 @@ CASE _eState OF
 		END_IF
 		
 		IF _xError THEN
-			_xHalt := FALSE;
-			_xBusy := FALSE;
-			_eState := E_AXIS_PTP_STATE.ERROR;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 	
 		
@@ -417,17 +295,19 @@ CASE _eState OF
 		END_IF
 		
 		IF _xError THEN
-			_xEnable := FALSE;
-			_xBusy := FALSE;
-			_eState := E_AXIS_PTP_STATE.ERROR;
+			_eState := E_AXIS_PTP_STATE.ENTER_ERROR;
 		END_IF
 	
+	E_AXIS_PTP_STATE.ENTER_ERROR:
+		xError := TRUE;
+		xBusy := FALSE;
+		xDone := FALSE;
+		
+		_eState := E_AXIS_PTP_STATE.ERROR;
 	
 	E_AXIS_PTP_STATE.ERROR:
 		IF xConfirmAlarms THEN
-			_xBusy := FALSE;
 			_xReset := TRUE;
-			_xError := FALSE;
 			_eState := E_AXIS_PTP_STATE.RESET;
 		END_IF
 	
@@ -435,164 +315,224 @@ CASE _eState OF
 	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
 
-// Can the axis perform a new move command
-_xCanExecNewCmd := (NOT _xBusy) AND _fbPower.Status AND (NOT _xError);
 
 // Copy internal buffers to outputs
 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>
+xHomed := stIO.io_fbAxisRef.Status.Homed;
+lrActPosition := stIO.io_fbAxisRef.NcToPlc.ActPos;
+xIsStopped := stIO.io_fbAxisRef.Status.StandStill OR stIO.io_fbAxisRef.Status.Disabled;]]></ST>
     </Implementation>
+    <Method Name="M_Disable" Id="{9ad5c9f5-bbf1-4888-86a0-ffdb2251d9c0}">
+      <Declaration><![CDATA[METHOD M_Disable : E_CmdResult
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_eState := E_AXIS_PTP_STATE.START_DISABLE;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_Disable := E_CmdResult.ACCEPTED;
+	
+	E_AXIS_PTP_STATE.ERROR:
+		M_Disable := E_CmdResult.ERROR;
+		
+	ELSE
+		M_Disable := E_CmdResult.BUSY;
+END_CASE]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="M_Enable" Id="{d68c8a7b-c709-4efe-872c-84892228bb19}">
+      <Declaration><![CDATA[METHOD M_Enable : E_CmdResult
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_eState := E_AXIS_PTP_STATE.START_ENABLE;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_Enable := E_CmdResult.ACCEPTED;
+	
+	E_AXIS_PTP_STATE.ERROR:
+		M_Enable := E_CmdResult.ERROR;
+		
+	ELSE
+		M_Enable := E_CmdResult.BUSY;
+END_CASE]]></ST>
+      </Implementation>
+    </Method>
     <Method Name="M_Halt" Id="{f6e3b049-4121-4299-b0f5-55178ecb35c3}">
-      <Declaration><![CDATA[METHOD M_Halt
+      <Declaration><![CDATA[METHOD M_Halt : E_CmdResult
 VAR_OUTPUT
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF _xBusy THEN
-	_xExecuteHalt := TRUE;
-	_xDone := FALSE;
-	xDone := FALSE;
-	_xBusy := TRUE;
-	xBusy := TRUE;
-END_IF]]></ST>
+        <ST><![CDATA[_eState := E_AXIS_PTP_STATE.START_HALTING;
+
+xBusy := TRUE;
+xDone := FALSE;
+M_Halt := E_CmdResult.ACCEPTED;]]></ST>
       </Implementation>
     </Method>
     <Method Name="M_Homing" Id="{816a1a43-38da-49f0-b902-c1027a6ba127}">
-      <Declaration><![CDATA[METHOD M_Homing : BOOL
+      <Declaration><![CDATA[METHOD M_Homing : E_CmdResult
 VAR_INPUT
 	lrHomingPosition : LREAL := Tc2_MC2.DEFAULT_HOME_POSITION;
 	eHomingMode : MC_HomingMode := MC_HomingMode.MC_DefaultHoming;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF _xCanExecNewCmd THEN
-	_xCanExecNewCmd := FALSE;
+        <ST><![CDATA[// Reject command if axis is not enabled
+IF (NOT _fbPower.Status) THEN
+	M_Homing := E_CmdResult.REJECTED;
+	RETURN;
+END_IF
+
+CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_eState := E_AXIS_PTP_STATE.START_HOMING;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_Homing := E_CmdResult.ACCEPTED;
 	
-	_lrHomingPos := lrHomingPosition;
-	_eHomingMode := eHomingMode;
-	_xExecuteHoming := TRUE;
-	
-	_xDone := FALSE;
-	xDone := FALSE;
-	_xBusy := TRUE;
-	xBusy := TRUE;
-	
-	M_Homing := TRUE;
-ELSE
-	M_Homing := FALSE;
-END_IF]]></ST>
+	E_AXIS_PTP_STATE.ERROR:
+		M_Homing := E_CmdResult.ERROR;
+		
+	ELSE
+		M_Homing := E_CmdResult.BUSY;
+END_CASE]]></ST>
       </Implementation>
     </Method>
     <Method Name="M_MoveAbs" Id="{f67a4861-cdb0-4dd2-93c8-1ae9dbc9a9a6}">
-      <Declaration><![CDATA[METHOD M_MoveAbs : BOOL;
+      <Declaration><![CDATA[METHOD M_MoveAbs : E_CmdResult
 VAR_INPUT
 	lrTargetPos : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF _xCanExecNewCmd THEN
-	_xCanExecNewCmd := FALSE;
-
-	_lrTargetPosition := lrTargetPos;
-	_xExecuteMoveAbs := TRUE;
-	
-	_xDone := FALSE;
-	xDone := FALSE;
-	_xBusy := TRUE;
-	xBusy := TRUE;
-	
-	M_MoveAbs := TRUE;
-ELSE
-	M_MoveAbs := FALSE;
+        <ST><![CDATA[// Reject command if axis is not enabled
+IF (NOT _fbPower.Status) THEN
+	M_MoveAbs := E_CmdResult.REJECTED;
+	RETURN;
 END_IF
+
+CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_lrTargetPosition := lrTargetPos;
+		_eState := E_AXIS_PTP_STATE.START_MOVING_ABSOLUTE;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_MoveAbs := E_CmdResult.ACCEPTED;
+	
+	E_AXIS_PTP_STATE.ERROR:
+		M_MoveAbs := E_CmdResult.ERROR;
+		
+	ELSE
+		M_MoveAbs := E_CmdResult.BUSY;
+END_CASE
 ]]></ST>
       </Implementation>
     </Method>
     <Method Name="M_MoveModulo" Id="{1a5eaefa-5bb1-4438-b3e0-80c1a34fc0ee}">
-      <Declaration><![CDATA[METHOD M_MoveModulo : BOOL
+      <Declaration><![CDATA[METHOD M_MoveModulo : E_CmdResult
 VAR_INPUT
 	lrTargetPos : LREAL;
 	eMoveDirection : MC_Direction;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF _xCanExecNewCmd THEN
-	_xCanExecNewCmd := FALSE;
+        <ST><![CDATA[// Reject command if axis is not enabled
+IF (NOT _fbPower.Status) THEN
+	M_MoveModulo := E_CmdResult.REJECTED;
+	RETURN;
+END_IF
+
+CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_lrTargetPosition := lrTargetPos;
+		_eMoveDirection := eMoveDirection;
+		_eState := E_AXIS_PTP_STATE.START_MOVING_MODULO;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_MoveModulo := E_CmdResult.ACCEPTED;
 	
-	_lrTargetPosition := lrTargetPos;
-	_eMoveDirection := eMoveDirection;
-	_xExecuteMoveModulo := TRUE;
-	
-	_xDone := FALSE;
-	xDone := FALSE;
-	_xBusy := TRUE;
-	xBusy := TRUE;
-	
-	M_MoveModulo := TRUE;
-ELSE
-	M_MoveModulo := FALSE;
-END_IF]]></ST>
+	E_AXIS_PTP_STATE.ERROR:
+		M_MoveModulo := E_CmdResult.ERROR;
+		
+	ELSE
+		M_MoveModulo := E_CmdResult.BUSY;
+END_CASE]]></ST>
       </Implementation>
     </Method>
     <Method Name="M_MoveRel" Id="{51e5fe82-5e25-4de0-84c7-b4d6560c312f}">
-      <Declaration><![CDATA[METHOD M_MoveRel : BOOL;
+      <Declaration><![CDATA[METHOD M_MoveRel : E_CmdResult
 VAR_INPUT
 	lrRelDist : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF _xCanExecNewCmd THEN
-	_xCanExecNewCmd := FALSE;
+        <ST><![CDATA[// Reject command if axis is not enabled
+IF (NOT _fbPower.Status) THEN
+	M_MoveRel := E_CmdResult.REJECTED;
+	RETURN;
+END_IF
+
+CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_lrRelativeDistance := lrRelDist;
+		_eState := E_AXIS_PTP_STATE.START_MOVING_RELATIVE;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_MoveRel := E_CmdResult.ACCEPTED;
 	
-	_xBusy := TRUE;
-	_lrRelativeDistance := lrRelDist;
-	_xExecuteMoveRel:= TRUE;
-	
-	_xDone := FALSE;
-	xDone := FALSE;
-	_xBusy := TRUE;
-	xBusy := TRUE;
-	
-	M_MoveRel := TRUE;
-ELSE
-	M_MoveRel := FALSE;
-END_IF]]></ST>
+	E_AXIS_PTP_STATE.ERROR:
+		M_MoveRel := E_CmdResult.ERROR;
+		
+	ELSE
+		M_MoveRel := E_CmdResult.BUSY;
+END_CASE]]></ST>
       </Implementation>
     </Method>
     <Method Name="M_MoveVelocity" Id="{fd4ad471-f522-45f2-a138-ebe509907cf6}">
-      <Declaration><![CDATA[METHOD M_MoveVelocity : BOOL;
+      <Declaration><![CDATA[METHOD M_MoveVelocity : E_CmdResult
 VAR_INPUT
 	eDirection : MC_Direction := MC_Direction.MC_Positive_Direction;
 END_VAR
 ]]></Declaration>
       <Implementation>
-        <ST><![CDATA[IF _xCanExecNewCmd THEN
-	_xCanExecNewCmd := FALSE;
-	
-	_xBusy := TRUE;	
-	_xExecuteMoveVelocity := TRUE;
-	_eMoveDirection := eDirection;
-	
-	_xDone := FALSE;
-	xDone := FALSE;
-	_xBusy := TRUE;
-	xBusy := TRUE;
-	
-	M_MoveVelocity := TRUE;
-ELSE
-	M_MoveVelocity := FALSE;
+        <ST><![CDATA[// Reject command if axis is not enabled
+IF (NOT _fbPower.Status) THEN
+	M_MoveRel := E_CmdResult.REJECTED;
+	RETURN;
 END_IF
-	]]></ST>
+
+CASE _eState OF
+	E_AXIS_PTP_STATE.IDLE:
+		_eMoveDirection := eDirection;
+		_eState := E_AXIS_PTP_STATE.START_MOVING_VELOCITY;
+		
+		xBusy := TRUE;
+		xDone := FALSE;
+		M_MoveRel := E_CmdResult.ACCEPTED;
+	
+	E_AXIS_PTP_STATE.ERROR:
+		M_MoveRel := E_CmdResult.ERROR;
+		
+	ELSE
+		M_MoveRel := E_CmdResult.BUSY;
+END_CASE]]></ST>
       </Implementation>
     </Method>
   </POU>
diff --git a/BasicComponents/POUs/Components/AxisPTP/ST_AxisPTP_IO.TcDUT b/BasicComponents/POUs/Components/AxisPTP/ST_AxisPTP_IO.TcDUT
new file mode 100644
index 0000000..6d68578
--- /dev/null
+++ b/BasicComponents/POUs/Components/AxisPTP/ST_AxisPTP_IO.TcDUT
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="ST_AxisPTP_IO" Id="{e9334c76-7085-45c0-b389-151c48bf0632}">
+    <Declaration><![CDATA[TYPE ST_AxisPTP_IO :
+STRUCT
+	i_xCalibrationCam AT %I* : BOOL;
+	io_fbAxisRef : AXIS_REF;
+END_STRUCT
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file