From 1b57b2e04ca19c9f70dd7305002916072f578985 Mon Sep 17 00:00:00 2001 From: Federico Rojas Date: Tue, 27 Sep 2022 16:50:37 +0200 Subject: [PATCH] Add code to read. store and restore the encoder position BIAS in case of power failure --- solution/tc_project_app/POUs/MAIN.TcPOU | 59 ++++++++++++++++++++----- 1 file changed, 47 insertions(+), 12 deletions(-) diff --git a/solution/tc_project_app/POUs/MAIN.TcPOU b/solution/tc_project_app/POUs/MAIN.TcPOU index a97fe32..dfb07a0 100644 --- a/solution/tc_project_app/POUs/MAIN.TcPOU +++ b/solution/tc_project_app/POUs/MAIN.TcPOU @@ -19,7 +19,9 @@ VAR 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_GetDeviceIdentificationEx; + afbReadPositionBias: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_ReadParameter; + afbWritePositionBias: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_WriteParameter; + fbGetDeviceIdentification: FB_GetDeviceIdentificationEx; END_VAR VAR PERSISTENT @@ -50,6 +52,16 @@ END_FOR]]> '0') THEN +IF (fbGetDeviceIdentification.stDevIdent.strHardwareSerialNo <> '') THEN CHECK_UPS(); RESTORE_POSITIONS(); END_IF]]> @@ -89,6 +101,9 @@ END_IF]]> //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." +//Encoder Reference system types: INCREMENTAL=0; INCREMENTAL (singleturn absolute)=4; +// ABSOLUTE=1; ABSOLUTE MULTITURN RANGE (with single overflow)=3; ABSOLUTE SINGLETURN RANGE (with single overflow)=5; +// ABSOLUTE (modulo)=2; IF bRestoreOnStartup AND eGlobalSUpsState = eSUPS_PowerOK THEN bRestoreOnStartup := FALSE; @@ -113,6 +128,11 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN afbRestorePosition[iAxes]( Axis := GVL.astAxes[iAxes].Axis, Position := astAxesPersistent[iAxes].fPositionAtShutdown); + + afbWritePositionBias[iAxes]( + Axis:= GVL.astAxes[iAxes].Axis, + ParameterNumber:= E_AxisParameters.AxisEncoderOffset, + Value:= astAxesPersistent[iAxes].fBiasAtShutdown); END_FOR CASE eStartUp OF @@ -153,23 +173,32 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN 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 + FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO + //Restore position value for incremental encoders + IF afbReadEncRefSys[iAxes].Valid = TRUE AND NOT astAxesPersistent[iAxes].bMovingAtShutdown AND + (afbReadEncRefSys[iAxes].Value = 0 OR afbReadEncRefSys[iAxes].Value = 4 OR afbReadEncRefSys[iAxes].Value = 2) THEN IF GVL.astAxes[iAxes].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN afbRestorePosition[iAxes].Execute := TRUE; END_IF - END_IF - END_FOR + //Restore encoder position BIAS for absolute encoders + ELSIF afbReadEncRefSys[iAxes].Valid = TRUE THEN + IF GVL.astAxes[iAxes].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN + afbWritePositionBias[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 + FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO + IF afbReadEncRefSys[iAxes].Valid = TRUE AND NOT astAxesPersistent[iAxes].bMovingAtShutdown AND + (afbReadEncRefSys[iAxes].Value = 0 OR afbReadEncRefSys[iAxes].Value = 4 OR afbReadEncRefSys[iAxes].Value = 2) THEN IF GVL.astAxes[iAxes].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN - IF NOT afbRestorePosition[iAxes].Done THEN + IF NOT afbRestorePosition[iAxes].Done OR NOT afbWritePositionBias[iAxes].Done THEN RETURN; END_IF END_IF @@ -181,6 +210,7 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN //Remove execute = TRUE for afbRestorePosition FOR iAxes := 1 TO GVL_APP.nAXIS_NUM DO afbRestorePosition[iAxes].Execute := FALSE; + afbWritePositionBias[iAxes].Execute := FALSE; END_FOR bPositionRestoreDone := TRUE; bRestoreExecute := FALSE; @@ -191,13 +221,18 @@ END_IF]]> 0 THEN + //Store Position beofre shutdown + astAxesPersistent[iAxes].fPositionAtShutdown := GVL.astAxes[iAxes].Axis.NcToPlc.ActPos; + + //Store encoder position BIAS at shutdown + astAxesPersistent[iAxes].fBiasAtShutdown := afbReadPositionBias[iAxes].Value; + + //Store value of moving at shutdown + IF NOT GVL.astAxes[iAxes].Axis.Status.StandStill 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]]>