240 lines
11 KiB
XML
240 lines
11 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.0">
|
|
<POU Name="MAIN" Id="{33eb6f49-7781-4211-a70b-87ada6d80cb7}" SpecialFunc="None">
|
|
<Declaration><![CDATA[
|
|
PROGRAM MAIN
|
|
VAR
|
|
sVersion: STRING := '1.0.0';
|
|
afbAxes: ARRAY [1..GVL_APP.nAXIS_NUM] OF FB_Axis;
|
|
hmiAxisSelection: INT := 1; //Not possible to use local hmi variables for array indexes
|
|
|
|
//Outputs: Power for Limit switches and Home Sensors (every 4th output)
|
|
bOutput1 AT %Q*: BOOL:= TRUE;
|
|
//bOutput5 AT %Q*: BOOL:= TRUE;
|
|
//bOutput13 AT %Q*: BOOL:= TRUE;
|
|
//bOutput17 AT %Q*: BOOL:= TRUE;
|
|
//bOutput21 AT %Q*: BOOL:= TRUE;
|
|
//bOutput24 AT %Q*: BOOL:= TRUE;
|
|
//bOutput28 AT %Q*: BOOL:= TRUE;
|
|
|
|
//Startup, Shutdown and UPS
|
|
fbUPS: FB_S_UPS_CX51x0;
|
|
eUpsMode: E_S_UPS_Mode := eSUPS_WrPersistData_Shutdown;
|
|
eStartUp: (eColdStart, eReadAxisFeedbackType, eCheckReadDone, eExecuteRestore, eCheckRestore, eFinishRestore);
|
|
bPositionRestoreDone: BOOL := FALSE;
|
|
bRestoreExecute: BOOL := FALSE;
|
|
bExecuteReadEncRefSys: BOOL := FALSE;
|
|
nRetry: INT;
|
|
iAxes : UINT; //index for for loops in Position recovery actions
|
|
afbReadEncRefSys: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_ReadParameter;
|
|
afbRestorePosition: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_SetPosition;
|
|
fbGetDeviceIdentification: FB_GetDeviceIdentification;
|
|
END_VAR
|
|
|
|
VAR PERSISTENT
|
|
bRestoreOnStartup: BOOL;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[POSITION_RECOVERY();
|
|
PROG();
|
|
AXES();]]></ST>
|
|
</Implementation>
|
|
<Folder Name="POSITION_RECOVERY" Id="{3561f6ef-e145-4ed3-9839-f17334bd2d97}" />
|
|
<Action Name="AXES" Id="{7eb32732-9b53-4934-8cd9-20ba971dd8ff}">
|
|
<Implementation>
|
|
<ST><;
|
|
END_FOR]]></ST>
|
|
</Implementation>
|
|
</Action>
|
|
<Action Name="CHECK_UPS" Id="{f0f28f50-53b8-4f73-b0f5-6d7ce4c1636f}" FolderPath="POSITION_RECOVERY\">
|
|
<Implementation>
|
|
<ST><![CDATA[fbUPS(eUpsMode := eUpsMode);
|
|
|
|
IF eGlobalSUpsState = eSUPS_PowerFailure THEN
|
|
//first cycle of powerfailure
|
|
//execute code that should only be done once with each powerfailure, i.e. increase powerfailure counter
|
|
bRestoreOnStartup := TRUE;
|
|
STORE_PERSISTENT();
|
|
RETURN;
|
|
ELSIF eGlobalSUpsState <> eSUPS_PowerOK THEN
|
|
//next cycles of powerfailure
|
|
//skip regular code execution for the remaining cycles of the powerfailure/writing of persistent data/quick shutdown...
|
|
RETURN;
|
|
END_IF]]></ST>
|
|
</Implementation>
|
|
</Action>
|
|
<Action Name="POSITION_RECOVERY" Id="{28e203b7-aea5-42d0-980d-12a6841f9d22}" FolderPath="POSITION_RECOVERY\">
|
|
<Implementation>
|
|
<ST><![CDATA[fbGetDeviceIdentification(bExecute := TRUE);
|
|
IF (fbGetDeviceIdentification.stDevIdent.strHardwareSerialNo <> '0') THEN
|
|
CHECK_UPS();
|
|
RESTORE_POSITIONS();
|
|
END_IF]]></ST>
|
|
</Implementation>
|
|
</Action>
|
|
<Action Name="PROG" Id="{5d03ebbb-2a47-4890-ad6d-e82daf72dc51}">
|
|
<Implementation>
|
|
<ST><![CDATA[//Program any sequence, safety or feature (if necessary) application specific in thsi section]]></ST>
|
|
</Implementation>
|
|
</Action>
|
|
<Action Name="RESTORE_POSITIONS" Id="{0c7ee537-7bd9-4833-b428-c17cbb57e893}" FolderPath="POSITION_RECOVERY\">
|
|
<Implementation>
|
|
<ST><![CDATA[//This ACT will restore the position of an incremental axis on startup with the act position it read before losing power.
|
|
//It checks the type of axis, 0=incremental, and that the axis was stationary at shut down.
|
|
//Because 0 equates to incremental we also need to check that the data is valid, otherwise by default it would restore all axes.
|
|
//By default an axis will not restore the position unless it is set to opt-in, i.e. GVL.aAxes[iAxes].stConfig.eRestorePosition is non-zero.
|
|
//This needs to be initialised somewhere in TwinCAT code otherwise it will not be available at start up.
|
|
//A restore will only be performed on a loss of power, this code shouldn't make any changes on a reset cold, a rest origin or a download.
|
|
//There is a enum to allow for different types of restore modes, currently only one is implemented.
|
|
//0 'DontRestore'
|
|
//1 'RestoreWithoutHome' -restores the position using a set position fb and does not set the home bit in the axis struct.
|
|
//Note from Beckhoff: "A maximum of 1 MB persistent data can be reliably saved over the entire service life."
|
|
|
|
IF bRestoreOnStartup AND eGlobalSUpsState = eSUPS_PowerOK THEN
|
|
bRestoreOnStartup := FALSE;
|
|
bRestoreExecute := TRUE;
|
|
END_IF
|
|
|
|
//Upon startup bPositionRestoreDone will be set to FALSE, after successfully completing the following code it will be set TRUE
|
|
//and should stay TRUE for the rest of the time the PLC is operational, thus this routine should only be completed once.
|
|
IF bRestoreExecute AND NOT bPositionRestoreDone THEN
|
|
//Cycle through function blocks that read the encoder reference system i.e. whether axis is incremental or absolute
|
|
//Result stored in Value, 0=Inc 1=Abs, execute set during the case statement
|
|
FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
afbReadEncRefSys[iAxes](
|
|
Axis := GVL.astAxes[iAxes].Axis,
|
|
Enable := bExecuteReadEncRefSys,
|
|
ParameterNumber := MC_AxisParameter.AxisEncoderReferenceSystem,
|
|
ReadMode := E_READMODE.READMODE_ONCE);
|
|
END_FOR
|
|
|
|
//Cycle through set position function blocks for each axis
|
|
FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
afbRestorePosition[iAxes](
|
|
Axis := GVL.astAxes[iAxes].Axis,
|
|
Position := astAxesPersistent[iAxes].fPositionAtShutdown);
|
|
END_FOR
|
|
|
|
CASE eStartUp OF
|
|
eColdStart:
|
|
//First cycle of the PLC, do nothing just give one cycle for variables to initialise
|
|
IF NOT bPositionRestoreDone THEN
|
|
eStartUp := eReadAxisFeedbackType;
|
|
nRetry := 0;
|
|
END_IF
|
|
|
|
eReadAxisFeedbackType:
|
|
//Exectute the function blocks to read the encoder reference system (0=inc OR 1=ABS)
|
|
bExecuteReadEncRefSys := TRUE;
|
|
eStartUp := eCheckReadDone;
|
|
|
|
eCheckReadDone:
|
|
//Check the encoder reference system has been read for all axis -> if busy then continue with PLC cycle and check again next time.
|
|
//If afbReadEncRefSys not started then go back a step.
|
|
//If any axes result in an error the code will get stuck here, this happens if GVL_APP.nAXIS_NUM is not set correctly
|
|
FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
IF afbReadEncRefSys[iAxes].Valid = FALSE THEN
|
|
IF afbReadEncRefSys[iAxes].Busy = TRUE THEN
|
|
//Exit MAIN.STARTUP Action and wait till next cycle, needs to cycle through whole program in order for data to update
|
|
RETURN;
|
|
ELSE
|
|
//Sometimes the code gets here and the afbReadEncRefSys[iAxes] misses the rising edge. If the code gets here it means
|
|
//valid=FALSE and .busy=FALSE which indicateds the FB probably hasn't started and thus needs to see a rising edge.
|
|
//Set execute to low, Exit MAIN.STARTUP and go back a step in the CASE statement.
|
|
bExecuteReadEncRefSys := FALSE;
|
|
eStartUp := eReadAxisFeedbackType;
|
|
nRetry := nRetry + 1; //counter used for troubleshooting to see how many cycles it takes before afbReadEncRefSys function blocks are read correctly
|
|
RETURN;
|
|
END_IF
|
|
END_IF
|
|
END_FOR
|
|
//If the code gets here all axes either have .valid=TRUE for all axes
|
|
eStartUp := eExecuteRestore;
|
|
|
|
eExecuteRestore:
|
|
//Execute position restore by setting afbRestorePosition.execute = TRUE
|
|
FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
IF afbReadEncRefSys[iAxes].Valid = TRUE AND afbReadEncRefSys[iAxes].Value = 0 AND NOT(astAxesPersistent[iAxes].bMovingAtShutdown) THEN
|
|
IF GVL.astAxes[iAxes].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
|
|
afbRestorePosition[iAxes].Execute := TRUE;
|
|
END_IF
|
|
END_IF
|
|
END_FOR
|
|
eStartUp := eCheckRestore;
|
|
|
|
eCheckRestore:
|
|
//Check the set position fbs are finished
|
|
//Nothing actually happens if the restore is not done, the code just returns from here each cycle and the
|
|
//bPositionRestoreDone will never get set to TRUE and will take up cycle time
|
|
FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
IF afbReadEncRefSys[iAxes].Valid = TRUE AND afbReadEncRefSys[iAxes].Value = 0 AND NOT(astAxesPersistent[iAxes].bMovingAtShutdown) THEN
|
|
IF GVL.astAxes[iAxes].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
|
|
IF NOT afbRestorePosition[iAxes].Done THEN
|
|
RETURN;
|
|
END_IF
|
|
END_IF
|
|
END_IF
|
|
END_FOR
|
|
eStartUp := eFinishRestore;
|
|
|
|
eFinishRestore:
|
|
//Remove execute = TRUE for afbRestorePosition
|
|
FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
afbRestorePosition[iAxes].Execute := FALSE;
|
|
END_FOR
|
|
bPositionRestoreDone := TRUE;
|
|
bRestoreExecute := FALSE;
|
|
END_CASE
|
|
END_IF]]></ST>
|
|
</Implementation>
|
|
</Action>
|
|
<Action Name="STORE_PERSISTENT" Id="{cb5c9254-2e5f-47b1-9baa-10e728a961b0}" FolderPath="POSITION_RECOVERY\">
|
|
<Implementation>
|
|
<ST><![CDATA[FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO
|
|
astAxesPersistent[iAxes].fPositionAtShutdown := GVL.astAxes[iAxes].Axis.NcToPlc.ActPos;
|
|
IF GVL.astAxes[iAxes].Axis.NcToPlc.ActVelo <> 0 THEN
|
|
astAxesPersistent[iAxes].bMovingAtShutdown := TRUE;
|
|
ELSE
|
|
astAxesPersistent[iAxes].bMovingAtShutdown := FALSE;
|
|
END_IF
|
|
astAxesPersistent[iAxes].bMovingAtShutdown := astAxesPersistent[iAxes].bMovingAtShutdown OR GVL.astAxes[iAxes].Axis.Status.Moving;
|
|
END_FOR]]></ST>
|
|
</Implementation>
|
|
</Action>
|
|
<LineIds Name="MAIN">
|
|
<LineId Id="505" Count="0" />
|
|
<LineId Id="134" Count="0" />
|
|
<LineId Id="81" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.AXES">
|
|
<LineId Id="1" Count="0" />
|
|
<LineId Id="4" Count="0" />
|
|
<LineId Id="10" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.CHECK_UPS">
|
|
<LineId Id="2" Count="11" />
|
|
<LineId Id="1" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.POSITION_RECOVERY">
|
|
<LineId Id="2" Count="3" />
|
|
<LineId Id="1" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.PROG">
|
|
<LineId Id="2" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.RESTORE_POSITIONS">
|
|
<LineId Id="567" Count="105" />
|
|
<LineId Id="1" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.STORE_PERSISTENT">
|
|
<LineId Id="10" Count="0" />
|
|
<LineId Id="3" Count="0" />
|
|
<LineId Id="5" Count="1" />
|
|
<LineId Id="8" Count="1" />
|
|
<LineId Id="7" Count="0" />
|
|
<LineId Id="4" Count="0" />
|
|
<LineId Id="1" Count="0" />
|
|
</LineIds>
|
|
</POU>
|
|
</TcPlcObject> |