1010 hmiAxisSelection: INT := 1; //Specifies the currently selected axis for HMI operations, allowing the user to control a specific axis
1111 hmiPneumaticAxisSelection: INT := 1; //Specifies the currently selected pneumatic axis for HMI operation, allowing the user to control the specific axis
1212
13- //Startup, Shutdown and UPS
13+ //Startup, Shutdown and UPS
1414 fbCX5130UPS: FB_S_UPS_CX51x0;
1515 fbC6017UPS: FB_S_UPS_BAPI;
1616 eSelUpsMode: E_S_UPS_Mode := eSUPS_WrPersistData_Shutdown; //Selected UPS mode
2020 bExecuteReadEncRefSys: BOOL := FALSE;
2121 nRetry: INT; //Counter for startup actions
2222 iAxes: UINT := 1; //Index for, for loops in position recovery actions
23- iCurrentAxis: UINT := 1; //Index for position recovery actions (not in for loops)
23+ iCurrentAxis: UINT := 1; //Index for position recovery actions (not in for loops)
2424 afbReadEncRefSys: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_ReadParameter;
2525 afbSetHomePosition: ARRAY[1..GVL_APP.nAXIS_NUM] OF MC_Home;
26- afbSetPosition: ARRAY[1..GVL_APP.nAXIS_NUM] OF MC_SetPosition;
26+ afbSetPosition: ARRAY[1..GVL_APP.nAXIS_NUM] OF MC_SetPosition;
2727 afbReadPositionBias: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_ReadParameter;
2828 afbWritePositionBias: ARRAY [1..GVL_APP.nAXIS_NUM] OF MC_WriteParameter;
2929 fbGetDeviceIdentification: FB_GetDeviceIdentificationEx;
3030 timeAsDTAtStartup: DT;
3131 nTestNum: UINT :=0 ; //Variable used in ESS's FAT_SAT_tools scripts to keep track of the test number
3232END_VAR
3333VAR CONSTANT
34- INCREMENTAL: UINT := 0;
35- ABSOLUTE: UINT := 1;
36- ABSOLUTE_MODULO: UINT := 2;
37- ABSOLUTE_MULTITURN_RANGE: UINT := 3; //(with single overflow)
38- INCREMENTAL_SINGLETURN_ABSOLUTE: UINT := 4;
39- ABSOLUTE_SINGLETURN_RANGE: UINT :=5; //(with single overflow)
34+ INCREMENTAL: UINT := 0;
35+ ABSOLUTE: UINT := 1;
36+ ABSOLUTE_MODULO: UINT := 2;
37+ ABSOLUTE_MULTITURN_RANGE: UINT := 3; //(with single overflow)
38+ INCREMENTAL_SINGLETURN_ABSOLUTE: UINT := 4;
39+ ABSOLUTE_SINGLETURN_RANGE: UINT :=5; //(with single overflow)
4040END_VAR
4141VAR PERSISTENT
4242 bRestoreOnStartup: BOOL; //Determines whether the axis positions should be restored automatically during system startup
@@ -46,6 +46,10 @@ END_VAR
4646 <ST ><![CDATA[ POSITION_RECOVERY();
4747PROG();
4848AXES();
49+
50+
51+
52+
4953]]> </ST >
5054 </Implementation >
5155 <Folder Name =" POSITION_RECOVERY" Id =" {3561f6ef-e145-4ed3-9839-f17334bd2d97}" />
@@ -173,44 +177,44 @@ IF bRestoreOnStartup AND eGlobalSUpsState = eSUPS_PowerOK THEN
173177 bRestoreOnStartup := FALSE;
174178 bRestoreExecute := TRUE;
175179 timeAsDTAtStartup := FILETIME64_TO_DT(F_GetSystemTime());
176- iCurrentAxis := 1;
180+ iCurrentAxis := 1;
177181END_IF
178182
179- IF iCurrentAxis > GVL_APP.nAXIS_NUM THEN
180- RETURN;
183+ IF iCurrentAxis > GVL_APP.nAXIS_NUM THEN
184+ RETURN;
181185END_IF
182186
183187//Upon startup bPositionRestoreDone will be set to FALSE, after successfully completing the following code it will be set TRUE
184188//and should stay TRUE for the rest of the time the PLC is operational, thus this routine should only be completed once.
185189IF bRestoreExecute AND NOT bPositionRestoreDone THEN
186-
187- IF NOT astAxes[iCurrentAxis].stStatus.bAxisInitialized OR NOT astAxesPersistent[iCurrentAxis].bHomedAtShutdown THEN
188- //If current axis has not been initialized or was not homed before power failure, perform position recovery for next axis
189- eStartUp := eNextAxis;
190- ELSE
191- //Cycle through function blocks that read the encoder reference system i.e. whether axis is incremental or absolute
192- //Cycle through set position and MC_WritePArameter function blocks for each axis
193- afbReadEncRefSys[iCurrentAxis](
194- Axis := GVL.astAxes[iCurrentAxis].Axis,
195- ParameterNumber := MC_AxisParameter.AxisEncoderReferenceSystem,
196- ReadMode := E_READMODE.READMODE_ONCE);
197-
198- IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
199- afbSetPosition[iCurrentAxis](
200- Axis := GVL.astAxes[iCurrentAxis].Axis,
201- Position := astAxesPersistent[iCurrentAxis].fPositionAtShutdown);
202- ELSIF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithHome THEN
203- afbSetHomePosition[iCurrentAxis](
204- Axis := GVL.astAxes[iCurrentAxis].Axis,
205- Position := astAxesPersistent[iCurrentAxis].fPositionAtShutdown,
206- HomingMode := MC_HomingMode.MC_Direct);
207- END_IF
208-
209- afbWritePositionBias[iCurrentAxis](
210- Axis := GVL.astAxes[iCurrentAxis].Axis,
211- ParameterNumber := E_AxisParameters.AxisEncoderOffset,
212- Value := astAxesPersistent[iCurrentAxis].fEncoderBiasAtShutdown);
213- END_IF
190+
191+ IF NOT astAxes[iCurrentAxis].stStatus.bAxisInitialized OR NOT astAxesPersistent[iCurrentAxis].bHomedAtShutdown THEN
192+ //If current axis has not been initialized or was not homed before power failure, perform position recovery for next axis
193+ eStartUp := eNextAxis;
194+ ELSE
195+ //Cycle through function blocks that read the encoder reference system i.e. whether axis is incremental or absolute
196+ //Cycle through set position and MC_WritePArameter function blocks for each axis
197+ afbReadEncRefSys[iCurrentAxis](
198+ Axis := GVL.astAxes[iCurrentAxis].Axis,
199+ ParameterNumber := MC_AxisParameter.AxisEncoderReferenceSystem,
200+ ReadMode := E_READMODE.READMODE_ONCE);
201+
202+ IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
203+ afbSetPosition[iCurrentAxis](
204+ Axis := GVL.astAxes[iCurrentAxis].Axis,
205+ Position := astAxesPersistent[iCurrentAxis].fPositionAtShutdown);
206+ ELSIF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithHome THEN
207+ afbSetHomePosition[iCurrentAxis](
208+ Axis := GVL.astAxes[iCurrentAxis].Axis,
209+ Position := astAxesPersistent[iCurrentAxis].fPositionAtShutdown,
210+ HomingMode := MC_HomingMode.MC_Direct);
211+ END_IF
212+
213+ afbWritePositionBias[iCurrentAxis](
214+ Axis := GVL.astAxes[iCurrentAxis].Axis,
215+ ParameterNumber := E_AxisParameters.AxisEncoderOffset,
216+ Value := astAxesPersistent[iCurrentAxis].fEncoderBiasAtShutdown);
217+ END_IF
214218
215219 CASE eStartUp OF
216220 eColdStart:
@@ -222,116 +226,116 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN
222226
223227 eReadAxisFeedbackType:
224228 //Exectute the function blocks to read the encoder reference system (0=inc OR 1=ABS)
225- afbReadEncRefSys[iCurrentAxis].Enable := TRUE;
226- eStartUp := eCheckReadDone;
229+ afbReadEncRefSys[iCurrentAxis].Enable := TRUE;
230+ eStartUp := eCheckReadDone;
227231
228232 eCheckReadDone:
229233 //Check the encoder reference system has been read for all axis -> if busy then continue with PLC cycle and check again next time.
230234 //If afbReadEncRefSys not started then go back a step.
231235 //If the current axis result in an error the code will get stuck here, this happens if GVL_APP.nAXIS_NUM is not set correctly
232- IF NOT afbReadEncRefSys[iCurrentAxis].Valid THEN
233- IF afbReadEncRefSys[iCurrentAxis].Busy THEN
234- //Exit MAIN.RESTORE_POSITIONS Action and wait till next cycle, needs to cycle through whole program in order for data to update
235- RETURN;
236- ELSE
237- //Sometimes the code gets here and the afbReadEncRefSys[iCurrentAxis] misses the rising edge. If the code gets here it means
238- //valid=FALSE and .busy=FALSE which indicateds the FB probably hasn't started and thus needs to see a rising edge.
239- //Set execute to low, Exit MAIN.STARTUP and go back a step in the CASE statement.
240- afbReadEncRefSys[iCurrentAxis].Enable := FALSE;
241- eStartUp := eReadAxisFeedbackType;
242- nRetry := nRetry + 1; //Counter used for troubleshooting to see how many cycles it takes before afbReadEncRefSys function blocks are read correctly
243- RETURN;
244- END_IF
245- END_IF
246-
236+ IF NOT afbReadEncRefSys[iCurrentAxis].Valid THEN
237+ IF afbReadEncRefSys[iCurrentAxis].Busy THEN
238+ //Exit MAIN.RESTORE_POSITIONS Action and wait till next cycle, needs to cycle through whole program in order for data to update
239+ RETURN;
240+ ELSE
241+ //Sometimes the code gets here and the afbReadEncRefSys[iCurrentAxis] misses the rising edge. If the code gets here it means
242+ //valid=FALSE and .busy=FALSE which indicateds the FB probably hasn't started and thus needs to see a rising edge.
243+ //Set execute to low, Exit MAIN.STARTUP and go back a step in the CASE statement.
244+ afbReadEncRefSys[iCurrentAxis].Enable := FALSE;
245+ eStartUp := eReadAxisFeedbackType;
246+ nRetry := nRetry + 1; //Counter used for troubleshooting to see how many cycles it takes before afbReadEncRefSys function blocks are read correctly
247+ RETURN;
248+ END_IF
249+ END_IF
250+
247251 //If the code gets here the current axis has .valid=TRUE
248252 eStartUp := eExecuteRestore;
249253
250254 eExecuteRestore:
251255 //Execute position and encoder BIAS restore by setting Execute = TRUE
252- IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition <> E_RestorePosition.eDontRestore THEN
253- IF NOT astAxesPersistent[iCurrentAxis].bMovingAtShutdown THEN
254- CASE LREAL_TO_UINT(afbReadEncRefSys[iCurrentAxis].Value) OF
255- INCREMENTAL, ABSOLUTE_MODULO, INCREMENTAL_SINGLETURN_ABSOLUTE:
256- // Restore position value for incremental encoders
257- IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
258- afbSetPosition[iCurrentAxis].Execute := TRUE;
259- ELSIF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithHome THEN
260- afbSetHomePosition[iCurrentAxis].Execute := TRUE;
261- END_IF
262- ABSOLUTE, ABSOLUTE_MULTITURN_RANGE, ABSOLUTE_SINGLETURN_RANGE:
263- // Restore encoder position BIAS for absolute encoders
264- afbWritePositionBias[iCurrentAxis].Execute := TRUE;
265- END_CASE
266- ELSE
267- RETURN;
268- END_IF
269- END_IF
270-
256+ IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition <> E_RestorePosition.eDontRestore THEN
257+ IF NOT astAxesPersistent[iCurrentAxis].bMovingAtShutdown THEN
258+ CASE LREAL_TO_UINT(afbReadEncRefSys[iCurrentAxis].Value) OF
259+ INCREMENTAL, ABSOLUTE_MODULO, INCREMENTAL_SINGLETURN_ABSOLUTE:
260+ // Restore position value for incremental encoders
261+ IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
262+ afbSetPosition[iCurrentAxis].Execute := TRUE;
263+ ELSIF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithHome THEN
264+ afbSetHomePosition[iCurrentAxis].Execute := TRUE;
265+ END_IF
266+ ABSOLUTE, ABSOLUTE_MULTITURN_RANGE, ABSOLUTE_SINGLETURN_RANGE:
267+ // Restore encoder position BIAS for absolute encoders
268+ afbWritePositionBias[iCurrentAxis].Execute := TRUE;
269+ END_CASE
270+ ELSE
271+ RETURN;
272+ END_IF
273+ END_IF
274+
271275 eStartUp := eCheckRestore;
272276
273277 eCheckRestore:
274278 //Check the set position and write enocder BIAS fbs are finished
275279 //Nothing actually happens if the restore is not done, the code just returns from here each cycle and the
276280 //bPositionRestoreDone will never get set to TRUE and will take up cycle time
277- IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition <> E_RestorePosition.eDontRestore THEN
278- IF NOT astAxesPersistent[iCurrentAxis].bMovingAtShutdown THEN
279- CASE LREAL_TO_UINT(afbReadEncRefSys[iCurrentAxis].Value) OF
280- INCREMENTAL, ABSOLUTE_MODULO, INCREMENTAL_SINGLETURN_ABSOLUTE:
281- IF afbSetPosition[iCurrentAxis].Busy THEN
282- //Exit MAIN.RESTORE_POSITIONS Action and wait till next cycle, needs to cycle through whole program in order for data to update
283- RETURN;
284- END_IF
285-
286- IF afbSetHomePosition[iCurrentAxis].Busy THEN
287- //Exit MAIN.RESTORE_POSITIONS Action and wait till next cycle, needs to cycle through whole program in order for data to update
288- RETURN;
289- END_IF
290-
291- IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
292- IF NOT afbSetPosition[iCurrentAxis].Done THEN
293- afbSetPosition[iCurrentAxis].Execute := FALSE;
294- eStartUp := eExecuteRestore;
295- RETURN;
296- END_IF
297- END_IF
298-
299- IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithHome THEN
300- IF NOT afbSetHomePosition[iCurrentAxis].Done THEN
301- afbSetHomePosition[iCurrentAxis].Execute := FALSE;
302- eStartUp := eExecuteRestore;
303- RETURN;
304- END_IF
305- END_IF
306-
307- ABSOLUTE, ABSOLUTE_MULTITURN_RANGE, ABSOLUTE_SINGLETURN_RANGE:
308- IF NOT afbWritePositionBias[iCurrentAxis].Done THEN
309- afbWritePositionBias[iCurrentAxis].Execute := FALSE;
310- eStartUp := eExecuteRestore;
311- RETURN;
312- END_IF
313- END_CASE
281+ IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition <> E_RestorePosition.eDontRestore THEN
282+ IF NOT astAxesPersistent[iCurrentAxis].bMovingAtShutdown THEN
283+ CASE LREAL_TO_UINT(afbReadEncRefSys[iCurrentAxis].Value) OF
284+ INCREMENTAL, ABSOLUTE_MODULO, INCREMENTAL_SINGLETURN_ABSOLUTE:
285+ IF afbSetPosition[iCurrentAxis].Busy THEN
286+ //Exit MAIN.RESTORE_POSITIONS Action and wait till next cycle, needs to cycle through whole program in order for data to update
287+ RETURN;
288+ END_IF
289+
290+ IF afbSetHomePosition[iCurrentAxis].Busy THEN
291+ //Exit MAIN.RESTORE_POSITIONS Action and wait till next cycle, needs to cycle through whole program in order for data to update
292+ RETURN;
293+ END_IF
294+
295+ IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithoutHome THEN
296+ IF NOT afbSetPosition[iCurrentAxis].Done THEN
297+ afbSetPosition[iCurrentAxis].Execute := FALSE;
298+ eStartUp := eExecuteRestore;
299+ RETURN;
300+ END_IF
301+ END_IF
302+
303+ IF GVL.astAxes[iCurrentAxis].stConfig.eRestorePosition = E_RestorePosition.eRestoreWithHome THEN
304+ IF NOT afbSetHomePosition[iCurrentAxis].Done THEN
305+ afbSetHomePosition[iCurrentAxis].Execute := FALSE;
306+ eStartUp := eExecuteRestore;
307+ RETURN;
308+ END_IF
309+ END_IF
310+
311+ ABSOLUTE, ABSOLUTE_MULTITURN_RANGE, ABSOLUTE_SINGLETURN_RANGE:
312+ IF NOT afbWritePositionBias[iCurrentAxis].Done THEN
313+ afbWritePositionBias[iCurrentAxis].Execute := FALSE;
314+ eStartUp := eExecuteRestore;
315+ RETURN;
316+ END_IF
317+ END_CASE
314318 END_IF
315- //ELSE
316- //RETURN;
317- END_IF
318-
319- afbSetHomePosition[iCurrentAxis].Execute := FALSE;
319+ //ELSE
320+ //RETURN;
321+ END_IF
322+
323+ afbSetHomePosition[iCurrentAxis].Execute := FALSE;
320324 afbWritePositionBias[iCurrentAxis].Execute := FALSE;
321325 eStartUp := eNextAxis;
322-
323- eNextAxis:
324- iCurrentAxis := iCurrentAxis + 1;
325- IF iCurrentAxis > GVL_APP.nAXIS_NUM THEN
326- eStartUp := eRestoreFinished;
327- bPositionRestoreDone := TRUE;
328- bRestoreExecute := FALSE;
329- ELSE
330- eStartUp := eReadAxisFeedbackType;
331- END_IF
332-
326+
327+ eNextAxis:
328+ iCurrentAxis := iCurrentAxis + 1;
329+ IF iCurrentAxis > GVL_APP.nAXIS_NUM THEN
330+ eStartUp := eRestoreFinished;
331+ bPositionRestoreDone := TRUE;
332+ bRestoreExecute := FALSE;
333+ ELSE
334+ eStartUp := eReadAxisFeedbackType;
335+ END_IF
336+
333337 eRestoreFinished:
334- //Do nothing
338+ //Do nothing
335339 END_CASE
336340END_IF
337341]]> </ST >
0 commit comments