berti_r/Motion_test
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
741d03da8d35fb1fe2efd7faee2eb693af2c3292
Motion_test/solution/tc_project_app/POUs/MAIN.TcPOU

399 lines
16 KiB
XML
Raw Blame History

<?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;
(******Error Handling********)
fbErrorSystem: FB_ErrorSystem;
aFbAxesError: ARRAY [1..gvl_app.axisNum] OF FB_NC_Error;
//fbEL1018 : EL1018;
//fbEL2808 : EL2808;
//fbEL5101 : EL5101;
//fbEL9505 : EL9505;
//fbEL1252 : EL1252;
//fbEL9410 : EL9410;
//fbEL7037 : EL7037;
//fbEK1110 : EK1110;
(******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();
ERROR();]]></ST>
</Implementation>
<Folder Name="POSITION_RECOVERY" Id="{3561f6ef-e145-4ed3-9839-f17334bd2d97}" />
<Action Name="AXES" Id="{7eb32732-9b53-4934-8cd9-20ba971dd8ff}">
<Implementation>
<ST><![CDATA[FOR GVL.iAxis:=1 TO gvl_app.axisNum DO
aFbAxes[GVL.iAxis](stAxisStruct:=gvl.axes[GVL.iAxis]);
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="ERROR" Id="{35f2cf38-f81e-4aa3-9534-be5fb417817d}">
<Implementation>
<ST><![CDATA[//
(****FB containting the log of the errors****)
//
fbErrorSystem(
En:= TRUE,
bReset:= ,
nErrorNum:= ,
bACK:= ,
bValidSelection:= ,
nTableRowIndex:= ,
EnO=> ,
nSelectedError=> ,
pErrorSystem=> );
FOR GVL.iAxis:=1 TO gvl_app.axisNum DO
aFbAxesError[gvl.iAxis](EN:=TRUE,
nNC_ErrorID:= gvl.axes[gvl.iAxis].status.nErrorID,
nNC_AxisID:=gvl.axes[gvl.iAxis].Axis.NcToPlc.AxisId,
ErrorSystem:= fbErrorSystem.pErrorSystem);
END_FOR
(*call all the necessary instance (input assistance F2 or right click) according to the terminals that you have in your hardware and
add "TRUE" in the input En, the corresponding number of termianl to the iTerminal_ID and
the variable "fbErrorSystem.pErrorSystem" to the input ErrorSystem in each FB E. g. :
fbEL1808(
En:= TRUE,
iTerminal_ID:= 01,
ErrorSystem:= fbErorSystem.pErrorSystem,
EnO=> ,
bError=> );
*)
]]></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="1" />
</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.ERROR">
<LineId Id="31" Count="0" />
<LineId Id="10" Count="1" />
<LineId Id="34" Count="8" />
<LineId Id="1" Count="0" />
<LineId Id="90" Count="1" />
<LineId Id="88" Count="1" />
<LineId Id="92" Count="2" />
<LineId Id="87" Count="0" />
<LineId Id="79" Count="1" />
<LineId Id="12" Count="0" />
<LineId Id="44" Count="8" />
<LineId Id="62" Count="0" />
<LineId Id="14" Count="0" />
<LineId Id="33" 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>
Reference in New Issue View Git Blame Copy Permalink