berti_r/Motion_test
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Remove mode 2 from position restore

Browse Source 
It was decided in a code review that mode 2 wasn't doing exactly as we wanted
and that it probably wasn't required, at least for now.
Changes to logic so that the position restore is only done after a power cycle and
not a download, reset cold or reset origin.
This commit is contained in:
Paul Barron
2019-12-17 14:46:55 +01:00
parent e36d4bcab3
commit 1c9d8f3b6b
1 changed files with 59 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
solution/tc_project_app/POUs/MAIN.TcPOU
+59 -34
View File
@@ -39,15 +39,20 @@ VAR
//fbEK1110 : EK1110;
(******Startup, Shutdown and UPS********)
eStartUp: (ColdStart, ReadAxisFeedbackType, CheckReadDone, ExecuteRestore, CheckRestore, FinishRestore);
bPositionRestoreDone : BOOL := FALSE;
bExecuteReadEncRefSys : BOOL := TRUE;
iRetry : INT;
fbReadEncRefSys : ARRAY [1..gvl_app.axisNum] OF MC_ReadParameter;
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;
END_VAR
VAR PERSISTENT
bRestoreOnStartup : BOOL;
END_VAR]]></Declaration>
<Implementation>
<ST><![CDATA[fbUPS(eUpsMode := eUpsMode); (* call UPS-FB instance in first lines of the fastest PLC Task *)
@@ -55,7 +60,9 @@ END_VAR]]></Declaration>
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_PowerFailure THEN
(* next cycles of powerfailure *)
(* skip regular code execution for the remaining cycles of the powerfailure/writing of persistent data/quick shutdown ... *)
@@ -65,12 +72,12 @@ END_IF
RESTORE_POSITIONS();
PROG();
AXES();
ERROR();
STORE_PERSISTENT();]]></ST>
ERROR();]]></ST>
</Implementation>
<Action Name="AXES" Id="{7eb32732-9b53-4934-8cd9-20ba971dd8ff}">
<Implementation>
<ST><![CDATA[FOR GVL.iAxis:=1 TO gvl_app.axisNum DO
<ST><![CDATA[gvl.axes[1].config.eRestorePosition:=RestorePosition.RestoreWithoutHome;
FOR GVL.iAxis:=1 TO gvl_app.axisNum DO
aFbAxes[GVL.iAxis](stAxisStruct:=gvl.axes[GVL.iAxis]);
END_FOR]]></ST>
</Implementation>
@@ -130,15 +137,22 @@ fbEL1808(
// 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 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 bPositionRestoreDone = FALSE THEN
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
@@ -152,7 +166,6 @@ IF bPositionRestoreDone = FALSE THEN
END_FOR
// Cycle through set position function blocks for each axis
// Only axes with RestorePosition=RestoreWithoutHome (mode 1) will be utilised
FOR i:=1 TO gvl_app.axisNum DO
fbRestorePosition[i](
Axis:= gvl.axes[i].Axis,
@@ -200,7 +213,9 @@ IF bPositionRestoreDone = FALSE THEN
// 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
fbRestorePosition[i].Execute:=TRUE;
IF GVL.axes[i].config.eRestorePosition = RestorePosition.RestoreWithoutHome THEN
fbRestorePosition[i].Execute:=TRUE;
END_IF
END_IF
END_FOR
eStartUp:=eStartUp+1;
@@ -211,47 +226,49 @@ IF bPositionRestoreDone = FALSE THEN
// 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 NOT fbRestorePosition[i].Done 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:=eStartUp+1;
FinishRestore: // Remove execute = TRUE for fbRestorePosition
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}">
<Implementation>
<ST><![CDATA[IF bPositionRestoreDone = TRUE THEN
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
END_IF]]></ST>
<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="124" Count="9" />
<LineId Id="376" Count="11" />
<LineId Id="2" Count="0" />
<LineId Id="189" Count="0" />
<LineId Id="135" Count="0" />
<LineId Id="134" Count="0" />
<LineId Id="81" Count="1" />
<LineId Id="293" Count="0" />
</LineIds>
<LineIds Name="MAIN.AXES">
<LineId Id="11" Count="0" />
<LineId Id="1" Count="0" />
<LineId Id="4" Count="0" />
<LineId Id="10" Count="0" />
@@ -283,11 +300,16 @@ END_IF]]></ST>
<LineId Id="94" Count="0" />
<LineId Id="98" Count="0" />
<LineId Id="213" Count="0" />
<LineId Id="101" Count="1" />
<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" />
@@ -295,9 +317,7 @@ END_IF]]></ST>
<LineId Id="91" Count="1" />
<LineId Id="2" Count="7" />
<LineId Id="96" Count="0" />
<LineId Id="10" Count="0" />
<LineId Id="97" Count="0" />
<LineId Id="11" Count="5" />
<LineId Id="10" Count="6" />
<LineId Id="18" Count="2" />
<LineId Id="177" Count="0" />
<LineId Id="21" Count="3" />
@@ -330,21 +350,27 @@ END_IF]]></ST>
<LineId Id="56" Count="0" />
<LineId Id="181" Count="0" />
<LineId Id="57" Count="1" />
<LineId Id="60" Count="0" />
<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="5" />
<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="2" Count="0" />
<LineId Id="10" Count="0" />
<LineId Id="3" Count="0" />
<LineId Id="5" Count="1" />
@@ -352,7 +378,6 @@ END_IF]]></ST>
<LineId Id="7" Count="0" />
<LineId Id="4" Count="0" />
<LineId Id="1" Count="0" />
<LineId Id="13" Count="0" />
</LineIds>
</POU>
</TcPlcObject>