504 lines
11 KiB
XML
504 lines
11 KiB
XML
<?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> |