MBP-224: Enable external setpoint generator

DUTs/Axis_Structures/ST_AxisControl.TcDUT

@@ -25,6 +25,8 @@ STRUCT
     bAirpadManualControl: BOOL := FALSE; //Allow manual control airpad
     bAirpadManualPressureOn: BOOL := FALSE; //Pressurize airpad manually
     bReleaseBrake AT %Q*: BOOL; //Output from method controlling brake
+    stExternalSetpoint: ST_ExternalSetpoint; //Input structure mapped to external setpoint feed
+    bEnableSetPointGen: BOOL := FALSE; //Enable external setpoint generator
 END_STRUCT
 END_TYPE
 ]]></Declaration>

DUTs/Axis_Structures/ST_AxisStatus.TcDUT

@@ -47,6 +47,7 @@ STRUCT
     bCommandHandlerBufferingCommand: BOOL; //True if CommandHandler is waiting on a permit before releasing the command
     bCommandHandlerTimeoutError: BOOL; //True if CommandHandler did not receive a permit in time
     eInternalMotionCommand: E_MotionFunctions;
+    bExternalSetpointGeneratorEnabled: BOOL; //True if external setpoint generator enabled.
 END_STRUCT
 END_TYPE
 ]]></Declaration>

DUTs/Axis_Structures/ST_ExternalSetpoint.TcDUT

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="ST_ExternalSetpoint" Id="{e2ab0bf5-3269-0dc3-1c73-a72e4685c342}">
+    <Declaration><![CDATA[TYPE ST_ExternalSetpoint :
+STRUCT
+    fPosition: LREAL := 0;
+    fVelocity: LREAL := 0;
+    fAcceleration: LREAL := 0;
+END_STRUCT
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>

POUs/Motion/FB_Axis.TcPOU

@@ -55,6 +55,10 @@ VAR
     fbGearOut: MC_GearOut;
     stMcStatusGearOut: ST_McStatus;
 
+    fbExtSetPointGenEnable: MC_ExtSetPointGenEnable;
+    fbExtSetPointGenDisable: MC_ExtSetPointGenDisable;
+    eExtSetPointGen: (STANDBY, ENABLE_GEN, ENABLED, DISABLE_GEN);
+
     stMasterAxis: ST_GearAxis := (nIndex := 1, fRatio := 0);
     nSlaveAxesIndex: UINT;
     nAxisIndex: UINT;
@@ -611,6 +615,76 @@ bReady :=
         AND _stAxis.stInputs.bBrakeReleased)
     OR NOT _stAxis.stConfig.stAirpadBrake.bExternalBrakeFeedbackConnected);
 mControl_ExternalBrake := bEnableAmp;
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="mControl_ExtSetPointGen" Id="{dfce07ce-0d00-06e4-1ded-daf10cdf7b25}" FolderPath="Control Methods\">
+      <Declaration><![CDATA[// External Setpoint Generator Control
+METHOD mControl_ExtSetPointGen
+VAR
+    nDirection: SINT := 0;
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[CASE eExtSetPointGen OF
+
+    STANDBY:
+        fbExtSetPointGenEnable.Execute := FALSE;
+        fbExtSetPointGenDisable.Execute := FALSE;
+        IF _stAxis.stControl.bEnableSetPointGen
+            AND_THEN _stAxis.stStatus.bEnabled THEN
+            eExtSetPointGen := ENABLE_GEN;
+        END_IF
+
+    ENABLE_GEN:
+        fbExtSetPointGenEnable.Execute := TRUE;
+
+        IF NOT fbExtSetPointGenEnable.Busy
+            AND_THEN fbExtSetPointGenEnable.Done THEN
+            fbExtSetPointGenEnable.Execute := FALSE;
+            eExtSetPointGen := ENABLED;
+        ELSIF fbExtSetPointGenEnable.Error THEN
+            fbExtSetPointGenEnable.Execute := FALSE;
+            _stAxis.stControl.bEnableSetPointGen := FALSE;
+            eExtSetPointGen := STANDBY;
+        END_IF
+
+    ENABLED:
+        IF NOT _stAxis.stControl.bEnableSetPointGen THEN
+            eExtSetPointGen := DISABLE_GEN;
+        END_IF
+
+    DISABLE_GEN:
+        fbExtSetPointGenDisable.Execute := TRUE;
+
+        IF NOT fbExtSetPointGenDisable.Busy
+            AND_THEN fbExtSetPointGenDisable.Done THEN
+            fbExtSetPointGenDisable.Execute := FALSE;
+            eExtSetPointGen := STANDBY;
+        END_IF
+
+END_CASE
+
+IF _stAxis.stControl.stExternalSetpoint.fVelocity = 0
+    AND_THEN _stAxis.stControl.stExternalSetpoint.fAcceleration = 0 THEN
+    nDirection := 0;
+ELSIF _stAxis.stControl.stExternalSetpoint.fVelocity >= 0 THEN
+    nDirection := 1;
+ELSE
+    nDirection := -1;
+END_IF
+
+fbExtSetPointGenEnable(Axis := _stAxis.Axis);
+fbExtSetPointGenDisable(Axis := _stAxis.Axis);
+
+MC_ExtSetPointGenFeed(
+    Axis := _stAxis.Axis,
+    Position := _stAxis.stControl.stExternalSetpoint.fPosition,
+    Velocity := _stAxis.stControl.stExternalSetpoint.fVelocity,
+    Acceleration := _stAxis.stControl.stExternalSetpoint.fAcceleration,
+    Direction := nDirection);
+
+stAxis.stStatus.bExternalSetpointGeneratorEnabled := eExtSetPointGen = ENABLED;
 ]]></ST>
       </Implementation>
     </Method>
@@ -1808,6 +1882,7 @@ stMcStatusHalt := mMove_Halt();
 stMcStatusStop := mMove_Stop();
 stMcStatusReset := mControl_Reset();
 
+mControl_ExtSetPointGen();
 stMcStatusMoveAbsolute := mMove_MoveAbsolute();
 stMcStatusMoveRelative := mMove_MoveRelative();
 stMcStatusMoveModulo := mMove_MoveModulo();