Stop Writing FANUC Homing Code Twice: Reuse Your Retreat Routines

I used to hate writing and testing the homing code for my robots.

It was repetitive and time-consuming, and it would often end up being ~50% of the codebase. I hated feeling like I was only half done when the robot did its main task perfectly.

So I stopped writing the same code twice.

In this article I’ll briefly cover how and why I write retreat code that can be used in both the normal cycle and homing, and how that idea influences how I write traversal code (getting from station to station) and clean programs/motion in general.

Homing the robot is essentially this:

Where am I?
How do I retreat to a known position?
How do I get home from the known position?

If you’re programming material handling applications like I often do, you have to write code for items 2 and 3 for the main cycle anyway. Why not just use the same code for homing?

Extracting the retreat

Let’s take a simple pick cycle:

! PICK ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
J PR[10:pick perch] 100% CNT100 ;
 ;
LBL[1] ;
WAIT (DI[10:pick ready]) TIMEOUT,LBL[501] ;
DO[10:pick clear]=(OFF) ;
 ;
L PR[11:pick] 1000mm/sec CNT100 Offset,PR[12:pick APOS] ;
L PR[11:pick] 250mm/sec FINE ;
 ;
CALL GRIP ;
 ;
L PR[11:pick] 500mm/sec CNT25 Offset,PR[13:pick RTOS] ;
J PR[10:pick perch] 100% FINE ;
 ;
DO[10:pick clear]=(ON) ;
END ;
 ;
LBL[501] ;
! TODO: alarm? ;
JMP LBL[1] ;

OK, so there are lots of things I would probably refactor here, but let’s just focus on extracting the retreat code:

! RETREAT_PICK ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
L PR[11:pick] 500mm/sec CNT25 Offset,PR[13:pick RTOS] ;
J PR[10:pick perch] 100% FINE ;

! PICK ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
J PR[10:pick perch] 100% CNT100 ;
 ;
LBL[1] ;
WAIT (DI[10:pick ready]) TIMEOUT,LBL[501] ;
DO[10:pick clear]=(OFF) ;
 ;
L PR[11:pick] 1000mm/sec CNT100 Offset,PR[12:pick APOS] ;
L PR[11:pick] 250mm/sec FINE ;
 ;
CALL GRIP ;
 ;
CALL RETREAT_PICK ;
 ;
DO[10:pick clear]=(ON) ;
END ;
 ;
LBL[501] ;
! TODO: alarm? ;
JMP LBL[1] ;

We’ve extracted the motion that gets us from the pick position to the pick perch position, but this code isn’t really ready for homing just yet.

What would happen if we called this routine when the robot is in between the retreat position and the perch position? The robot would move the wrong way initially, getting closer to the station before moving onto perch.

What if the robot was 99% of the way to perch and we called this? Lots of wasted time and even some crash potential.

We can do better.

! RETREAT_PICK ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
PR[5:LPOS]=LPOS ;
IF (PR[5,3:LPOS]>(PR[11,3:pick]+PR[13,3:pick RTOS])),JMP LBL[1] ;
PR[5,3:LPOS]=(PR[11,3:pick]+PR[13,3:pick RTOS]) ;
L PR[5:LPOS] 500mm/sec CNT20 ;
 ;
LBL[1] ;
J PR[10:pick perch] 100% FINE ;

We are checking for the condition I described earlier. If this routine is called when the robot is at PR[11:pick], we run the whole path. Otherwise we skip the first segment and move directly to PR[10:pick perch].

NOTE: Obviously this guard would have to change if your retreat path is not straight up in Z, but hopefully you get the idea.

Guarding against the wrong station

There’s an issue though. I don’t really like working with LPOS unless I know where the robot is. What if there was a bug and RETREAT_PICK was accidentally called when the robot was at some other station? Depending on the Z, the robot might move straight to the retreat position or perch position and blow through anything in its way… not ideal.

I usually implement guards like this:

! RETREAT_PICK ;
CALL AT_PICK(1) ;
IF (!F[1:at station]),CALL UNSAFE ;
 ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
PR[5:LPOS]=LPOS ;
! etc... ;

The purpose of AT_PICK is to tell me if the robot is at the station. I write more guards to basically box the robot in before returning true or false to the given flag:

! AT_PICK ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
PR[5:LPOS]=LPOS ;
IF (PR[5,1:LPOS]<300),JMP LBL[500] ;
IF (PR[5,1:LPOS]>500),JMP LBL[500] ;
IF (PR[5,2:LPOS]<(-200)),JMP LBL[500] ;
IF (PR[5,2:LPOS]>200),JMP LBL[500] ;
IF (PR[5,3:LPOS]<0),JMP LBL[500] ;
IF (PR[5,3:LPOS]>1000),JMP LBL[500] ;
! made it through the gauntlet ;
! we must be at the station ;
F[AR[1]]=(ON) ;
END ;
 ;
LBL[500] ;
! not at station ;
F[AR[1]]=(OFF) ;
END ;

UNSAFE usually just throws a user alarm (or sends an error code to the PLC) and aborts the robot. In normal situations, UNSAFE should really never be called, so it’s really more of a panic()-type situation. We are just asserting that the robot is at the station before doing motion, and it’s OK to fault/abort if something’s not safe:

! UNSAFE ;
UALM[1] ;
ABORT ;

NOTE: You can set the appropriate $ualrm_sev[] to automatically abort, but I like to add an explicit ABORT statement just to be safe.

With this code added, we can be sure that the RETREAT_PICK code will only run when the robot is at the pick station.

Safe homing

What about homing? Remember the recipe:

Where am I?
How do I retreat to a known position?
How do I get home from the known position?

We’ve got subroutines that take care of items 1 and 2 for the pick station. We just need to tie them together and implement item 3:

! SAFE_HOME ;
! step 1: where am I ? ;
CALL AT_PICK(1) ;
IF (F[1:at station]),JMP LBL[1] ;
 ;
! i'm lost ;
CALL UNSAFE ;
! unreachable - UNSAFE aborts ;
ABORT ;
 ;
LBL[1] ;
! step 2: Pick station to known location ;
CALL RETREAT_PICK ;
! step 3: home from known position ;
! add intermediate positions if necessary ;
CALL HOME ;
END ;

Where HOME might just be something simple like:

! HOME ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
J PR[1:home] 100% FINE ;

Adding stations is easy: you just have to implement AT_* and RETREAT_* routines and add them to SAFE_HOME:

! SAFE_HOME ;
CALL AT_PICK(1) ;
IF (F[1:at station]),JMP LBL[1] ;
 ;
CALL AT_PLACE(1) ;
IF (F[1:at station]),JMP LBL[2] ;
 ;
CALL AT_HOME(1) ;
IF (F[1:at station]),JMP LBL[3] ;
 ;
! i'm lost ;
CALL UNSAFE ;
ABORT ;
 ;
LBL[1] ;
! at pick ;
CALL RETREAT_PICK ;
CALL HOME ;
END ;
 ;
LBL[2] ;
! at place ;
CALL RETREAT_PLACE ;
CALL HOME ;
END ;
 ;
LBL[3] ;
! already at home ;
CALL HOME ;
END ;

Why it’s worth it

There are a bunch of advantages to writing your motion code this way.

First of all, you don’t have two separate programs to maintain. Your production/main cycle code uses the same retreat code as your homing code. Fewer lines of code is usually better.

Your homing code gets run more frequently. I used to procrastinate and write/test my homing code at the very end as an afterthought. Yep, it’ll home, probably, most of the time, I hope. I definitely feel more confident about homing when it’s really just running the same retreat cycles it always runs.

Traversing between stations

This also sets you up for homing’s cousin: traversing from station to station. (I first wrote about this back in 2014 in Intelligent Traversing.) The recipe is almost the same:

Where am I?
How do I retreat to a known position (if necessary)?
How do I get from the known position to the next station?

It’s really just SAFE_HOME written again with another destination in mind. I usually take care of this with a MV_TO_STATION() program that ties everything together:

NOTE: I prefer to prefix any program with actual motion with the MV_ prefix. This keeps motion separate from logic and gives little reusable motion routines that you can use within your tasks, traversal and homing code as necessary. I’ll write another article about the various reasons behind why I do this.

! MV_TO_STATION ;
! AR[1] - destination station id ;
! 1 - home ;
! 2 - pick ;
! 3 - place ;
CALL GET_STATION_ID ;
! populates R[x:current station id] ;
SELECT R[x:current station id]=1,CALL MV_HOME ;
       =2,CALL MV_RETREAT_PICK ;
       =3,CALL MV_RETREAT_PLACE ;
       ELSE,CALL UNSAFE ;
 ;
! we are at a perch position now ;
 ;
R[y:dest station id]=AR[1] ;
IF R[x:current station id]=R[y:dest station id],JMP LBL[999] ;
SELECT R[y:dest station id]=1,JMP LBL[101] ;
       =2,JMP LBL[102] ;
       =3,JMP LBL[103] ;
! bad destination ;
CALL UNSAFE ;
ABORT ;
 ;
LBL[101] ;
! to home ;
CALL MV_HOME ;
END ;
 ;
LBL[102] ;
! to pick ;
CALL MV_PICK_PERCH ;
END ;
 ;
LBL[103] ;
! to place ;
CALL MV_PLACE_PERCH ;
END ;
 ;
LBL[999] ;
! already at station ;
END ;

GET_STATION_ID just uses the AT_* programs to populate R[x:current station id]:

! GET_STATION_ID ;
! 0 - unknown ;
! 1 - home ;
! 2 - pick ;
! 3 - place ;
R[x:current station id]=0 ;
 ;
CALL AT_HOME(1) ;
IF (F[1:at station]),R[x:current station id]=(1) ;
IF (R[x:current station id]<>0),JMP LBL[999] ;
 ;
CALL AT_PICK(1) ;
IF (F[1:at station]),R[x:current station id]=(2) ;
IF (R[x:current station id]<>0),JMP LBL[999] ;
 ;
CALL AT_PLACE(1) ;
IF (F[1:at station]),R[x:current station id]=(3) ;
IF (R[x:current station id]<>0),JMP LBL[999] ;
 ;
LBL[999] ;

NOTE: Sometimes you must take care when ordering these AT_* calls. Particularly when stations overlap (e.g. sometimes I will have a general “CLEAR” station that encompasses all perch positions). You’ll have to decide which takes priority.

Maybe you use R[x:current station id] to conditionally add an intermediate position here and there depending on your layout, but it’s easier if you record your positions in such a way that you can get from a perch position to any other perch position safely (and from a perch position to home) with just a joint move.

Putting it all together

Coming full circle, our pick routine then becomes:

! PICK ;
! 2 - pick ;
CALL MV_TO_STATION(2) ;
 ;
LBL[1] ;
WAIT (DI[10:pick ready]) TIMEOUT,LBL[501] ;
DO[10:pick clear]=(OFF) ;
 ;
UFRAME_NUM=0 ;
UTOOL_NUM=1 ;
L PR[11:pick] 1000mm/sec CNT100 Offset,PR[12:pick APOS] ;
L PR[11:pick] 250mm/sec FINE ;
 ;
CALL GRIP ;
 ;
CALL RETREAT_PICK ;
 ;
DO[10:pick clear]=(ON) ;
END ;
 ;
LBL[501] ;
! TODO: alarm? ;
JMP LBL[1] ;

And if we want to make it really clean:

! PICK ;
! 2 - pick ;
CALL MV_TO_STATION(2) ;
 ;
LBL[1] ;
WAIT (DI[10:pick ready]) TIMEOUT,LBL[501] ;
DO[10:pick clear]=(OFF) ;
 ;
CALL MV_PICK ;
CALL GRIP ;
CALL MV_RETREAT_PICK ;
 ;
DO[10:pick clear]=(ON) ;
END ;
 ;
LBL[501] ;
! TODO: alarm? ;
JMP LBL[1] ;

Now we have a really clean PICK program that you can view on the pendant without scrolling. We don’t have multiple programs implementing the same motion path, and end up with homing code that is continuously tested every time the robot runs through its normal cycle. I’d call that a win.