Adept s650H Quattro - Manual (Page 81)

Quattro Motions
Adept Quattro s650 Robot User’s Guide, Rev B 81
.PROGRAM a_move(target.loc)
; ABSTRACT: Move to the specified destination location, performing an
; intermediate motion if necessary because the destination
; rotation is "close to zero" and on the "other side" of the
; range of rotation.
;
; INPUT: target.loc Transformation defining the destination
;
; OUTPUTS: None
;_______________________________________________________________________
AUTO frac, ii, loc1[6], loc2[6], sav.spd[1], small
small = 0.1 ;Criterion for "close to 0"
; If the destination can be reached, consider whether or not an
; intermediate motion is needed.
IF INRANGE(target.loc) THEN
DECOMPOSE loc1[1] = HERE ;Components of HERE
DECOMPOSE loc2[1] = target.loc ;Components of destination
; If the destination rotation is very close to zero degrees,
; and it's on the "other side" of the range of rotation from
; the starting rotation, move to the center of the range of
; rotation. (Two IFs are used to improve efficiency.)
IF ABS(loc2[6]) <= small THEN
IF SIGN(loc2[6]) <> SIGN(loc1[6]) THEN
; Compute the fraction of the total motion to get
; to the center of the range of rotation.
frac = (180-ABS(loc1[6]))/(360-ABS(loc1[6]))
; Compute correspondingly scaled components of the
; destination transformation.
FOR ii = 1 TO 5
loc2[ii] = loc1[ii]+frac*(loc2[ii]-loc1[ii])
END
; Save temporary motion speeds so they can be
; applied to the "final" motion below.
;
; Note: More temporary settings (COARSE/FINE,
; NONULL/NULL, etc.) could be saved based on values
; returned by the CONFIG real- valued function.
sav.spd[0] = SPEED(4)
sav.spd[1] = SPEED(7)
; Move to the computed transit location.
MOVES TRANS(loc2[1],loc2[2],loc2[3],loc2[4],loc2[5],180)
; Re-apply the temporary speed settings so they
; will apply to the motion below.
SPEED sav.spd[0], sav.spd[1]
END
END
END
; Move to the final destination (which will fail if the location cannot
; be reached).
MOVES target.loc
RETURN
.END