329 lines
14 KiB
XML
329 lines
14 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';
|
|
i : UINT; //index variable for AXES()
|
|
aFbAxes: ARRAY [1..gvl_app.axisNum] 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;
|
|
//bOutput2 AT %Q*: BOOL:= TRUE;
|
|
//bOutput3 AT %Q*: BOOL:= TRUE;
|
|
//bOutput4 AT %Q*: BOOL:= TRUE;
|
|
//bOutput5 AT %Q*: BOOL:= TRUE;
|
|
//bOutput6 AT %Q*: BOOL:= TRUE;
|
|
//bOutput7 AT %Q*: BOOL:= TRUE;
|
|
//bOutput8 AT %Q*: BOOL:= TRUE;
|
|
//bOutput9 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: (ColdStart, ReadAxisFeedbackType, CheckReadDone, ExecuteRestore, CheckRestore, FinishRestore);
|
|
bPositionRestoreDone : BOOL := FALSE;
|
|
bRestoreExecute : BOOL := FALSE;
|
|
bExecuteReadEncRefSys : BOOL := FALSE;
|
|
iRetry : INT;
|
|
fbReadEncRefSys : ARRAY [1..gvl_app.axisNum] OF MC_ReadParameter;
|
|
fbRestorePosition : ARRAY [1..GVL_app.axisNum] 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); (* call UPS-FB instance in first lines of the fastest PLC Task *)
|
|
|
|
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.axes[i].config.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 i:=1 TO gvl_app.axisNum DO
|
|
fbReadEncRefSys[i](
|
|
Axis:= gvl.axes[i].Axis,
|
|
Enable:= bExecuteReadEncRefSys,
|
|
ParameterNumber:= MC_AxisParameter.AxisEncoderReferenceSystem,
|
|
Value=>,
|
|
ReadMode:= E_READMODE.READMODE_ONCE);
|
|
END_FOR
|
|
|
|
// Cycle through set position function blocks for each axis
|
|
FOR i:=1 TO gvl_app.axisNum DO
|
|
fbRestorePosition[i](
|
|
Axis:= gvl.axes[i].Axis,
|
|
Execute:= ,
|
|
Position:= axesPersistent[i].iPositionAtShutdown);
|
|
END_FOR
|
|
|
|
CASE eStartUp OF
|
|
ColdStart:
|
|
// First cycle of the PLC, do nothing just give one cycle for variables to initialise
|
|
IF NOT bPositionRestoreDone THEN
|
|
eStartUp:= ReadAxisFeedbackType;
|
|
iRetry:=0;
|
|
END_IF
|
|
|
|
ReadAxisFeedbackType:
|
|
// Exectute the function blocks to read the encoder reference system (0=inc OR 1=ABS)
|
|
bExecuteReadEncRefSys:=TRUE;
|
|
eStartUp:=CheckReadDone;
|
|
|
|
CheckReadDone:
|
|
// Check the encoder reference system has been read for all axis -> if busy then continue with PLC cycle and check again next time.
|
|
// If fbReadEncRefSys 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.axisNum is not set correctly
|
|
FOR i:=1 TO gvl_app.axisNum DO
|
|
IF fbReadEncRefSys[i].Valid = FALSE THEN
|
|
IF fbReadEncRefSys[i].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 fbReadEncRefSys[i] 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:=ReadAxisFeedbackType;
|
|
iRetry:=iRetry+1; // counter used for troubleshooting to see how many cycles it takes before fbReadEncRefSys 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:= ExecuteRestore;
|
|
|
|
ExecuteRestore:
|
|
// Execute position restore by setting fbRestorePosition.execute = TRUE
|
|
FOR i:=1 TO gvl_app.axisNum DO
|
|
IF fbReadEncRefSys[i].Valid = TRUE AND fbReadEncRefSys[i].Value = 0 AND NOT(axesPersistent[i].bMovingAtShutdown) THEN
|
|
IF GVL.axes[i].config.eRestorePosition = RestorePosition.RestoreWithoutHome THEN
|
|
fbRestorePosition[i].Execute:=TRUE;
|
|
END_IF
|
|
END_IF
|
|
END_FOR
|
|
eStartUp:= CheckRestore;
|
|
|
|
CheckRestore:
|
|
// 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 i:=1 TO gvl_app.axisNum DO
|
|
IF fbReadEncRefSys[i].Valid = TRUE AND fbReadEncRefSys[i].Value = 0 AND NOT(axesPersistent[i].bMovingAtShutdown) THEN
|
|
IF GVL.axes[i].config.eRestorePosition = RestorePosition.RestoreWithoutHome THEN
|
|
IF NOT fbRestorePosition[i].Done THEN
|
|
RETURN;
|
|
END_IF
|
|
END_IF
|
|
END_IF
|
|
END_FOR
|
|
eStartUp:= FinishRestore;
|
|
|
|
FinishRestore:
|
|
// Remove execute = TRUE for fbRestorePosition
|
|
FOR i:=1 TO gvl_app.axisNum DO
|
|
fbRestorePosition[i].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 i:=1 TO gvl_app.axisNum DO
|
|
axesPersistent[i].iPositionAtShutdown:=gvl.axes[i].Axis.NcToPlc.ActPos;
|
|
IF gvl.axes[i].Axis.NcToPlc.ActVelo <> 0 THEN
|
|
axesPersistent[i].bMovingAtShutdown:=TRUE;
|
|
ELSE
|
|
axesPersistent[i].bMovingAtShutdown:=FALSE;
|
|
END_IF
|
|
axesPersistent[i].bMovingAtShutdown:=axesPersistent[i].bMovingAtShutdown OR gvl.axes[i].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" />
|
|
<LineId Id="1" Count="0" />
|
|
<LineId Id="3" Count="0" />
|
|
</LineIds>
|
|
<LineIds Name="MAIN.RESTORE_POSITIONS">
|
|
<LineId Id="99" Count="0" />
|
|
<LineId Id="94" Count="0" />
|
|
<LineId Id="98" Count="0" />
|
|
<LineId Id="213" Count="0" />
|
|
<LineId Id="101" Count="0" />
|
|
<LineId Id="233" Count="1" />
|
|
<LineId Id="102" Count="0" />
|
|
<LineId Id="215" Count="0" />
|
|
<LineId Id="214" Count="0" />
|
|
<LineId Id="109" Count="0" />
|
|
<LineId Id="95" Count="0" />
|
|
<LineId Id="236" Count="2" />
|
|
<LineId Id="240" Count="0" />
|
|
<LineId Id="239" Count="0" />
|
|
<LineId Id="206" Count="0" />
|
|
<LineId Id="208" Count="1" />
|
|
<LineId Id="207" Count="0" />
|
|
<LineId Id="100" Count="0" />
|
|
<LineId Id="91" Count="1" />
|
|
<LineId Id="2" Count="7" />
|
|
<LineId Id="96" Count="0" />
|
|
<LineId Id="10" Count="6" />
|
|
<LineId Id="18" Count="2" />
|
|
<LineId Id="177" Count="0" />
|
|
<LineId Id="21" Count="3" />
|
|
<LineId Id="171" Count="0" />
|
|
<LineId Id="25" Count="0" />
|
|
<LineId Id="178" Count="0" />
|
|
<LineId Id="26" Count="1" />
|
|
<LineId Id="170" Count="0" />
|
|
<LineId Id="29" Count="0" />
|
|
<LineId Id="179" Count="0" />
|
|
<LineId Id="163" Count="0" />
|
|
<LineId Id="212" Count="0" />
|
|
<LineId Id="152" Count="0" />
|
|
<LineId Id="139" Count="0" />
|
|
<LineId Id="131" Count="0" />
|
|
<LineId Id="155" Count="0" />
|
|
<LineId Id="190" Count="0" />
|
|
<LineId Id="140" Count="0" />
|
|
<LineId Id="157" Count="0" />
|
|
<LineId Id="210" Count="1" />
|
|
<LineId Id="187" Count="0" />
|
|
<LineId Id="150" Count="1" />
|
|
<LineId Id="200" Count="0" />
|
|
<LineId Id="145" Count="0" />
|
|
<LineId Id="141" Count="0" />
|
|
<LineId Id="166" Count="0" />
|
|
<LineId Id="203" Count="0" />
|
|
<LineId Id="201" Count="0" />
|
|
<LineId Id="173" Count="0" />
|
|
<LineId Id="56" Count="0" />
|
|
<LineId Id="181" Count="0" />
|
|
<LineId Id="57" Count="1" />
|
|
<LineId Id="216" Count="0" />
|
|
<LineId Id="219" Count="1" />
|
|
<LineId Id="63" Count="2" />
|
|
<LineId Id="174" Count="0" />
|
|
<LineId Id="66" Count="0" />
|
|
<LineId Id="182" Count="0" />
|
|
<LineId Id="204" Count="1" />
|
|
<LineId Id="67" Count="1" />
|
|
<LineId Id="221" Count="0" />
|
|
<LineId Id="71" Count="2" />
|
|
<LineId Id="222" Count="0" />
|
|
<LineId Id="79" Count="2" />
|
|
<LineId Id="175" Count="0" />
|
|
<LineId Id="82" Count="0" />
|
|
<LineId Id="223" Count="0" />
|
|
<LineId Id="83" Count="3" />
|
|
<LineId Id="241" Count="0" />
|
|
<LineId Id="87" Count="0" />
|
|
<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> |