COMMENT ⊗   VALID 00018 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	.TITLE RUN
C00005 00003	"GO"     - ARM MOTION IN SLEWING MODE
C00008 00004	"GRASP"  - CLOSES THE FINGERS AROUND AN OBJECT
C00011 00005	"OPEN"   - CHANGES HAND OPENING
C00013 00006	"DRIVE"  - SINGLE JOINT MOTION
C00018 00007	"DRAW"   - RELATIVE ARM MOTION USING STRAIGHT LINE MOTION
C00022 00008	"MOVE"   - ARM MOTION USING JOINT INTERPOLATION
C00024 00009	"DEPART" - RELATIVE MOTION ALONG ARM'S -Z AXIS
C00026 00010	"APPRO" - MOTION RELATIVE TO A TRANSFORM ALONG ARM'S -Z AXIS
C00028 00011	"READY"&"REST" - MOVES THE ARM TO THE READY&REST POSITIONS
C00029 00012	"JOINT"  - SUBR FOR GENERATED SET POINTS BY JOINT INTERPOLATION
C00032 00013	"SPEED"  - CHANGES THE SPEED AT WHICH MOTIONS ARE PERFORMED
C00034 00014	"WAITER" - WAITS FOR A SPECIFIED PERIOD OF TIME AND PROCEEDS
C00036 00015	"GOSUB"&"RETURN" - USER SUBROUTINE CALL AND RETURN
C00038 00016	"PAUSE","TYPE","COMMNT"
C00040 00017	"FLIP","NOFLIP","ABOVE","BELOW","RIGHTY","LEFTY"
C00042 00018	"STOP","COARSE","INTOFF","INTON","PULSE"
C00044 ENDMK
C⊗;
.TITLE RUN

;THE FOLLOWING ROUTINES ARE CALLED THE "RUN-TIME" CODE AND ARE
;RESPONSIBLE FOR PERFORMING THE OPERATIONS OF THE MOTION INSTRUCTIONS.
;AT THE TIME THAT THESE SUBROUTINES ARE CALLED, IT IS ASSUMED THAT R3
;CONTAINS A POINTER TO THE MOTION STEP BEING EXECUTED, AND R4 CONTAINS
;A POINTER TO THE NEXT SEQUENTIAL MOTION STEP.	EACH STEP IS STORED
;IN MEMORY IN THE FOLLOWING FORMAT:
;
;		|-----------------------|
;		| PTR TO NEXT STEP OR 0 |
;		|-----------------------|
;		|	STEP LABEL	|  ← THIS WORD MAY BE MISSING
;		|-----------------------|
;		| PTR TO FUNCTION BLK	|
;		|-----------------------|
;	R3 →	|   VARIBLE LENGTH	|
;		|			|
;		|     DATA AREA		|
;		|-----------------------|
;
;AT THE COMPLETION OF THESE ROUTINES THE ARM STATUS WORD ( "ARMS" )
;CONTAINS FLAG BITS WHICH INDICATE ANY RUN-TIME ERRORS WHICH MIGHT
;HAVE OCCURRED.  IF A RUN-TIME ROUTINE RESULTED IN A REQUEST FOR A
;CHANGE IN THE MOTION INSTRUCTION EXECUTION SEQUENCE ( EG. A BRANCH
;ON CONDITION ), THE NEXT STEP POINTER IN R4 WILL HAVE BEEN ADJUSTED
;TO THE ABSOLUTE ADDRESS OF OF NEXT STEP TO BE EXECUTED.

;REGISTERS USED:
;	R3,R4 PASS ARGUMENTS AND R3 MAY BE GARBAGED
;	ALL OTHER REGISTERS ARE AVAILABLE FOR USE
;"GO"     - ARM MOTION IN SLEWING MODE

;THE GO MOTION INSTRUCTION PERFORMS A SLEWING MOTION FROM THE CURRENT
;POSITION OF THE ARM TO ANOTHER LOCATION AND ORIENTATION SPECIFIED BY
;A GIVEN TRANFORMATION.  ITS MOTION STEP VARIABLE STORAGE AREA LOOKS
;LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|
;
;NO PATH IS COMPUTED FOR THIS MOTION SO THE ARM IS FREE TO GO FROM
;POINT TO POINT IN WHAT EVER FASHION IT CHOOSES.

GO:	MOV	(R3),R1		;TRANSFORM SYMBOL BLOCK
	MOV	TRNPTR(R1),R1	;TRANSFORM DATA?
	BEQ	GONOT
	MOV	R1,@#GOTRN	;SAVE TRANS POINTER
	MOV	#GOLST,R1	;"GO" SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	BR	GODNE

GONOT:	MOV	#CANPRO,@#ARMS	;INDICATE ERROR, CAN PROCEED
	MOV	#NOTDAT,R1
	JSR	PC,TYPERR

GODNE:	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

GOSUBR:	BIS	#FINAL,16(R1)	;ONLY ONE PASS THRU THIS RTN
	MOV	@#GOTRN,R0	;CONVERT TRANS TO JOINT ANGLES
	MOV	@#CONFIG,R2	;ASSERT ANY REQUESTED CHANGES IN CONF
	CLR	@#CONFIG
	JSR	PC,SOLVE
	TST	R0		;VALID SOLUTION?
	BEQ	GOSDN		;YES
	BIS	#NOSOL,R0	;INDICATE INVALID SOLUTION
	MOV	R0,R1
	BIS	#STPMVE+CANPRO,@#ARMS
GOSDN:	RTS	PC
	

;LOCAL STORAGE AREA

GOLST:	ARMDNE	;MASK BITS FOR ARM JOINTS
	1200.	;ALLOW 6 SECONDS FOR ARM MOTION
	GOSUBR	;20MSEC SUBROUTINE

;END OF "GO"
;"GRASP"  - CLOSES THE FINGERS AROUND AN OBJECT

;THE GRASP MOTION INSTRUCTION IS USED FOR CLOSING THE FINGERS AROUND
;OBJECTS.  THE FINGERS ARE COMMANDED TO CLOSE AND THE MOTION IS
;TERMINATED AFTER A VERY BRIEF PERIOD.	IF THE HAND CLOSES
;FURTHER THAN A SPECIFIED DISTANCE AN ERROR MESSAGE IS SIGNALED.  ITS
;MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   SCALED DISTANCE	|
;		|-----------------------|

GRASP:	MOV	(R3),R5		;MINIMUM HAND OPENING
	MOV	@#LHAND,@#DHAND	;SERVO TO MINIMUM HAND OPENING
	BIS	#HNDBRK,@#MODES
	MOV	#GPLST,R1
	JSR	PC,DRIVE
	TST	@#ARMS		;ANY ERRORS?
	BNE	2$		;EXIT IF YES
	MOV	#14,R0		;READ THE CURRENT HAND OPENING
	MOV	#1,R1
	MOV	#HAND,R2
	JSR	PC,ANGLES
	BCS	1$		;BRANCH IF ADC ERROR
	CMP	R5,@#HAND	;HAND OPENING < MINIMUM EXPECTED?
	BLE	2$		;NO ERROR IF GREATER THAN MINIMUM
	BIS	#CANPRO,@#ARMS	;ELSE ERROR
	MOV	#BADCLS,R1
1$:	JSR	PC,TYPERR
2$:	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

GPSUB:	MOV	R0,R2
	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	1$		;NO
	CMP	@#PULTME,R2	;YES, TIME TO STOP PULSE?
	BGE	1$		;NO
	MOV	#HITHND,R0	;YES
	JSR	PC,SETBRK
1$:	CMP	#80.,R2		;TIME TO STOP THE HAND MOTION?
	BGE	2$		;NO
	BIS	#FINAL,16(R1)	;YES
2$:	RTS	PC


;LOCAL STORAGE AREA

GPLST:	0	;NO NULLING
	1000.	;ALLOW 5 SECONDS FOR HAND OPERATION
	GPSUB	;20MSEC SUBROUTINE


;END OF "GRASP" ROUTINES
;"OPEN"   - CHANGES HAND OPENING

;THE OPEN MOTION INSTRUCTION CHANGES THE HAND OPENING.	THIS ROUTINE
;REQUIRES AS ITS ONLY ARGUMENT THE DISTANCE THAT THE HAND IS TO BE
;LEFT OPEN.  NEGATIVE DISTANCES CLOSE THE FINGERS.  ITS MOTION STEP
;VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   SCALED DISTANCE	|
;		|-----------------------|

OPEN:	MOV	(R3),R1		;HAND OPENING
	CMP	@#UHAND,R1	;GREATER THAN MAX?
	BGE	1$		;IN RANGE
	MOV	@#UHAND,R1	;ELSE SET EQUAL TO MAXIMUM
	BR	2$
1$:	CMP	@#LHAND,R1	;LESS THAN MIN?
	BLE	2$
	MOV	@#LHAND,R1	;SET EQUAL TO MINIMUM
2$:	MOV	R1,@#DHAND	;SET SET POINT
	BIS	#HNDBRK,@#MODES
	MOV	#OPLST,R1	;POINT TO SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE HAND
	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

OPSUB:	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	1$		;NO
	CMP	@#PULTME,R0	;YES, TIME TO STOP PULSING?
	BGE	2$		;NO
	MOV	#HITHND,R0	;YES
	JSR	PC,SETBRK
1$:	BIS	#FINAL,16(R1)	;SIGNAL START CHECKING JOINT DONE
2$:	RTS	PC


;LOCAL STORAGE AREA

OPLST:	HNDDNE	;MASK BIT  FOR HAND
	600.	;ALLOW 3 SECONDS FOR HAND OPERATION
	OPSUB	;20MSEC SUBROUTINE


;END OF "OPEN" ROUTINES
;"DRIVE"  - SINGLE JOINT MOTION

;THE DRIVE MOTION INSTRUCTION IS USED TO DRIVE A SINGLE JOINT OF THE
;ARM.  IT REQUIRES THREE ARGUMENTS: THE JOINT TO BE DRIVEN(1-7), THE
;TOTAL CHANGE IN JOINT ANGLE TO PERFORM ( OR HAND OPENING ), AND THE
;TOTAL TIME THE OPERATION IS TO TAKE.  ITS MOTION STEP VARIABLE
;STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|  JOINT NUMBER (1-7)	|
;		|-----------------------|
;		| CHANGE IN JT ANG(DEG) |
;		|-----------------------|
;		|  TIME IN 1/100 SEC	|
;		|-----------------------|


FDRIVE:	MOV	R4,-(SP)
	MOV	#FDRVSB,-(SP)	;CONSTRUCT "DRIVE" DATA LIST ON STACK
	CMP	-(SP),-(SP)
	MOV	(R3)+,R0	;JOINT NUMBER
	BLE	BADJT		;MUST BE IN RANGE
	CMP	#7,R0
	BLT	BADJT
	DEC	R0		;COMPUTE THE JOINT INDEX
	MOV	#1,R1
	ASH	R0,R1
	SWAB	R1		;SAVE IN RANGE BIT MASK
	MOV	R1,(SP)
	ASL	R0		;GET JOINT WORD INDEX
	MOV	R0,@#FDRVJT	;SET JOINT TO BE DRIVEN
	MOV	(R3)+,R1	;GET THE CHANGE IN JOINT ANGLE
	CMP	#12.,R0		;IF HAND, CONVERT NUMBER TO DISTANCE
	BNE	CHKCHG
	BIS	#HNDBRK,@#MODES	;INDICATE HAND OPERATION
	MOV	R1,R4		;SAVE SIGN
	BPL	.+4
	NEG	R4
	MUL	#45.,R4
	ASHC	#-2,R4
	TST	R4		;OVERFLOW?
	BEQ	.+6
	MOV	USTOP(R0),R5	;YES USE MAXIMUM HAND OPENING
	TST	R1		;ATTACH SIGN
	BPL	.+4
	NEG	R5
	MOV	R5,R1
CHKCHG:	ADD	DANGLE(R0),R1	;CHECK IF GOING BEYOND STOP LIMITS
	BVC	NOOVER
	MOV	#100000,R1	;OVERFLOW, SET TO EXTREME ANGLE
	TST	DANGLE(R0)
	BMI	.+6
	MOV	#77777,R1
NOOVER:	CMP	LSTOP(R0),R1	;LESS THAN LOWER?
	BLE	1$
	MOV	LSTOP(R0),R1	;USE LOWER LIMIT
	BR	2$
1$:	CMP	USTOP(R0),R1	;MORE THAN UPPER LIMIT?
	BGE	2$
	MOV	USTOP(R0),R1	;USE UPPER LIMIT
2$:	MOV	R1,@#FTH	;SAVE FINAL JOINT ANGLE
	SUB	DANGLE(R0),R1	;SAVE TOTAL CHANGE
	MOV	R1,@#DTH
	MOV	(R3)+,R0	;GET DURATION FOR MOTION
	BGT	.+6
	MOV	#1,R0		;DEFAULT = 1 TICK
	ASL	R0
	BVC	.+6
	MOV	#77000,R0
	MOV	R0,@#TIME1	;SAVE IT
	ADD	#600.,R0	;ALLOW 3 SECONDS TO NULL ERROR
	BVC	.+6		;CHECK FOR OVERFLOW
	MOV	#77777,R0	;REPLACE WITH MAXIMUM PERMITED TIME
	MOV	R0,2(SP)	;SAVE TOTAL TIME FOR MOTION
	MOV	SP,R1		;PERFORM MOTION
	JSR	PC,DRIVE
	BR	FDRDNE

BADJT:	MOV	#BADJTN,R1	;INDICATE SPECIFICATION ERROR
	JSR	PC,TYPERR
	BIS	#CANPRO,@#ARMS

FDRDNE:	ADD	#6,SP		;CLEAR STACK
	MOV	(SP)+,R4
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

FDRVSB:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	MOV	@#TIME1,R2	;EVALUATE PERCENT CHANGE IN SET POINT
	JSR	PC,EVAL
	MUL	@#DTH,R0	;x CHANGE IN JOINT ANGLE
	ASHC	#2,R0
	ADD	@#FTH,R0	;+ FINAL SET POINT
	ADD	@#FDRVJT,R5	;PTR TO JOINT ANGLE SET POINT
	MOV	R0,(R5)		;SAVE NEW SET POINT
	RTS	PC


;END OF "DRIVE"
;"DRAW"   - RELATIVE ARM MOTION USING STRAIGHT LINE MOTION

;THE DRAW INSTRUCTION PERFORMS A RELATIVE CHANGE IN THE POSITION OF
;THE HAND OF THE ARM USING STRAIGHT LINE MOTION.  IT REQUIRES AS ITS
;ARGUMENTS THE CHANGE IN X,Y, AND Z TO BE MADE DURING THE MOTION.
;ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		| CHANGE IN X DIRECTION |
;		|-----------------------|
;		| CHANGE IN Y DIRECTION |
;		|-----------------------|
;		| CHANGE IN Z DIRECTION |
;		|-----------------------|
;
;THE TOTAL MOTION TIME IS SET EQUAL TO THE TIME REQUIRED BY THE
;SLOWEST JOINT TO COMPLETE ITS CHANGE IN JOINT ANGLE.

DRAW:	MOV	R4,-(SP)
	MOV	#DRWSUB,-(SP)	;CONSTRUCT "DRIVE" DATA LIST ON STK
	TST	-(SP)
	MOV	#ARMDNE,-(SP)
	MOV	R3,@#RELXYZ
	MOV	#TTRANS,R0	;GET CURRENT ARM TRANSFORM
	MOV	#DANGLE,R1
	JSR	PC,UPDATE
	MOV	#TTRANS+22,R0	;FINAL XYZ ← PRESENT XYZ + RELXYZ
	MOV	#FINXYZ,R4
	MOV	#OUTRNG,R1	;FETCH ERROR MESSAGE,JUST IN CASE
	MOV	#3,R5
FINLOP:	ADD	(R3)+,(R0)
	BVS	DRWERR		;OVERFLOW?
	MOV	(R0)+,(R4)+
	SOB	R5,FINLOP
	MOV	#TTRANS,R0	;COMPUTE MOTION TIME
	MOV	#DRWANG,R1	;NEED FINAL JOINT ANGLES
	MOV	#ONPATH,R2	;MAINTAIN PRESENT CONFIGURATION
	JSR	PC,SOLVE
	TST	R0		;INVALID SOLUTION?
	BNE	DRWER2		;BRANCH IF NO SOLUTION
	MOV	#DRWANG,R1	;GET CHANGE IN JOINT ANGLES
	MOV	#DANGLE,R2
	MOV	#6,R5
	SUB	(R2)+,(R1)+
	SOB	R5,.-2
	MOV	#DRWANG,R0	;COMPUTE MOTION TIME
	JSR	PC,TIMER
	MOV	R0,@#DRWTME	;SAVE IT
	ADD	#600.,R0	;+ NULLING TIME
	BVC	.+6
	MOV	#77777,R0	;USE MAX TIME IF OVERFLOW
	MOV	R0,2(SP)	;TIME OUT TIME
	MOV	SP,R1		;GO PERFORM THE ARM MOTION
	JSR	PC,DRIVE
	BR	DRWDNE

DRWER2:	BIS	#NOSOL,R0	;INDICATE ERROR
	MOV	R0,R1
DRWERR:	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

DRWDNE:	ADD	#6,SP		;CLEAR STACK
	MOV	(SP)+,R4
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

DRWSUB:	MOV	R1,-(SP)	;SAVE PTR TO JOINT ANGLES
	MOV	@#DRWTME,R2	;EVALUATE PERCENT CHANGE IN XYZ
	JSR	PC,EVAL
	MOV	R0,-(SP)	;COMPUTE NEW XYZ
	MOV	@#RELXYZ,R0	;NEW XYZ ← % CHANGE*RELXYZ+FINXYZ
	MOV	#FINXYZ,R1
	MOV	#TTRANS+22,R4
	MOV	#3,R5
NEWXYZ:	MOV	(R0)+,R2	;CHANGE IN X, Y, OR Z
	MUL	(SP),R2
	ASHC	#2,R2
	ADD	(R1)+,R2	;+ FINAL XYZ
	MOV	R2,(R4)+	;NEW XYZ
	SOB	R5,NEWXYZ
	TST	(SP)+
	MOV	#TTRANS,R0	;COMPUTE NEW JOINT SET POINTS
	MOV	(SP)+,R1	;SEND BACK IN HERE
	MOV	#ONPATH,R2	;MAINTAIN PRESENT CONFIGURATION
	JSR	PC,SOLVE
	RTS	PC


;END OF "DRAW"
;"MOVE"   - ARM MOTION USING JOINT INTERPOLATION

;THE MOVE INSTRUCTION MOVES THE ARM TO A SPECIFIED LOCATION AND
;ORIENTATION USING JT. ANGLE INTERPOLATION TO DETERMINE INTERMEDIATE
;SET POINTS.  ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|
;
;THE TOTAL MOTION TIME IS SET EQUAL TO THE TIME REQUIRED BY THE
;SLOWEST JOINT TO COMPLETE ITS CHANGE IN JOINT ANGLE.

MOVE: 	MOV	(R3),R1		;TRANSFORM SYMBOL BLOCK
	MOV	TRNPTR(R1),R1	;TRANSFORM DATA?
	BEQ	MOVNT
	JSR	PC,JOINT	;GO DRIVE THE ARM
	BR	MOVDN

MOVNT:	MOV	#NOTDAT,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

MOVDN:	RTS	PC


;END OF "MOVE"
;"DEPART" - RELATIVE MOTION ALONG ARM'S -Z AXIS

;THIS INSTRUCTION MOVES THE ARM A SPECIFIED DISTANCE ALONG ITS OWN
;-Z AXIS RELATIVE TO THE ARMS CURRENT POSITION.  ITS MOTION STEP
;VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|       DISTANCE        |
;		|-----------------------|
;

DEPART:	MOV	R4,-(SP)	;SAVE PTR TO NEXT STEP
	MOV	(R3),R4		;SAVE -DISTANCE TO CHANGE
	NEG	R4
	MOV	#TTRANS,R0	;GET CURRENT ARM TRANSFORM
	MOV	#DANGLE,R1
	JSR	PC,UPDATE	

RELMVE:	MOV	#TTRANS+14,R0	;NEW XYZ ← OLD XYZ + DIS*Z VECT
	MOV	#TTRANS+22,R1
	MOV	#3,R5
DEPLOP:	MOV	(R0)+,R2	;COMPONENT OF Z VECTOR
	MUL	R4,R2		; x DISTANCE
	ASHC	#2,R2		;NORMALIZE AND ROUND
	TST	R3
	BPL	.+4
	INC	R2
	ADD	R2,(R1)+	;CORRECT OLD XYZ
	BVS	DEPERR		;OVERFLOW?
	SOB	R5,DEPLOP
	MOV	#TTRANS,R1	;GO TO THIS NEW POSITION
	JSR	PC,JOINT
	BR	DEPDNE

DEPERR:	MOV	#OUTRNG,R1	;INDICATE ERROR
APRERR:	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

DEPDNE:	MOV	(SP)+,R4
	RTS	PC


;END OF "DEPART"
;"APPRO" - MOTION RELATIVE TO A TRANSFORM ALONG ARM'S -Z AXIS

;THIS INSTRUCTION MOVES THE ARM A SPECIFIED DISTANCE RELATIVE TO A
;DEFINED TRANSFORM LOCATION.  ITS MOTION VARIABLE STORAGE AREA
;LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   TRANSFORM POINTER   |
;		|-----------------------|
;		|       DISTANCE        |
;		|-----------------------|
;

APPRO:	MOV	R4,-(SP)
	MOV	2(R3),R4	;SAVE -DISTANCE TO CHANGE
	NEG	R4
	MOV	(R3),R0		;TRANSFORM TO APPROACH
	MOV	#NOTDAT,R1	;GET ERROR MESSAGE
	MOV	TRNPTR(R0),R0	;TRANSFORM DATA?
	BEQ	APRERR		;SIGNAL ERROR
	MOV	#TTRANS,R1	;COPY TRANS INTO HERE
	MOV	#12.,R3
	MOV	(R0)+,(R1)+
	SOB	R3,.-2
	BR	RELMVE		;JUST LIKE A DEPART FROM THIS POINT

;END OF "APPRO"
;"READY"&"REST" - MOVES THE ARM TO THE READY&REST POSITIONS

;THESE INSTRUCTIONS MOVE THE ARM TO THE PRE-DEFINED READY AND
;REST POSITIONS.  THEY REQUIRE NO ARGUMENTS.


READY:	MOV	#RDYTRN,R1	;READY POSITION
	MOV	@#RDYASS,@#CONFIG
	JMP	JOINT

REST:	TST	@#NSPEED	;USER REQUESTED SPECIAL SPEED?
	BNE	.+10
	MOV	#2000,@#NSPEED	;NO, MORE TO REST AT HALF SPEED
	MOV	#RSTTRN,R1	;REST POSITION
	MOV	@#RSTASS,@#CONFIG
	JMP	JOINT


;END OF "READY"&"REST"
;"JOINT"  - SUBR FOR GENERATED SET POINTS BY JOINT INTERPOLATION

;THE FOLLOWING SUBROUTINE CAN BE USED TO GENERATE A SMOOTH PATH BY
;INTERPOLATING BETWEEN THE CURRENT ARM POSITION AND A FINAL
;TRANSFORM.  IT REQUIRES AS ITS ONLY ARGUMENT, A POINTER TO A
;TRANSFORM.  A SAMPLE CALLING SEQUENCE FOLLOW:
;
;		MOV	#TRANS,R1
;		JSR	PC,JOINT

;REGISTERS USED:
;	R1 PASSES ARGUMENTS AND IS MODIFIED

JOINT:	MOV	R4,-(SP)
	MOV	R1,R0		;CONVERT TRANS TO NEW JOINT ANGLES
	MOV	#FANGLE,R1
	MOV	@#CONFIG,R2	;ASSERT ANY REQUESTED CHANGES IN CONF
	CLR	@#CONFIG
	JSR	PC,SOLVE
	TST	R0		;INVALID SOLUTION?
	BNE	JTERR		;BRANCH IF NO SOLUTION
	MOV	#FANGLE,R1	;GET CHANGE IN JOINT ANGLES
	MOV	#DANGLE,R2
	MOV	#DELANG,R3	;SAVE IN HERE
	MOV	#6,R4
DIFANG:	MOV	(R1)+,R0
	SUB	(R2)+,R0
	MOV	R0,(R3)+
	SOB	R4,DIFANG
	MOV	#DELANG,R0	;COMPUTE MOTION TIME
	JSR	PC,TIMER
	MOV	R0,@#INTTME	;SAVE IT
	ADD	#600.,R0	;+ NULLING TIME
	MOV	R0,@#JTTIME	;TIME OUT TIME
	MOV	#JTDATA,R1	;"JOINT" SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	BR	JTDNE

JTERR:	BIS	#NOSOL,R0	;INVALID SOLUTION
	MOV	R0,R1
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

JTDNE:	MOV	(SP)+,R4
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

JOINTS:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	MOV	@#INTTME,R2	;EVALUATE PERCENT CHANGE IN SET POINT
	JSR	PC,EVAL
	MOV	R0,-(SP)	;COMPUTE NEW SET POINTS
	MOV	#DELANG,R0	;NEW JT ANG ← % CHANGE*GANGC+GANGF
	MOV	#FANGLE,R1
	MOV	#6,R4
FINTLP:	MOV	(R0)+,R2	;CHANGE IN ANGLE
	MUL	(SP),R2
	ASHC	#2,R2
	ADD	(R1)+,R2	;+ FINAL SET POINT
	MOV	R2,(R5)+	;SAVE NEW SET POINT
	SOB	R4,FINTLP
	TST	(SP)+
	RTS	PC

JTDATA:	ARMDNE	;MASK BITS FOR ARM JOINTS
JTTIME:	0	;TIME OUT TIME
	JOINTS	;20MSEC SUBROUTINE


;END OF "JOINT"
;"SPEED"  - CHANGES THE SPEED AT WHICH MOTIONS ARE PERFORMED

;THE SPEED INSTRUCTION IS USED FOR SPECIFYING THE SPEED AT WHICH
;THE NEXT ARM MOTION IS TO BE PERFORMED.  THE SPEED IS EXPRESSED
;AS A PERCENTAGE RANGING FROM 1 (VERY SLOW) TO 32678.( AS FAST
;AS THE HARDWARE SERVO CAN GO).  THE NORMAL OPERATING SPEED IS
;SET TO 100.  THE MOTION STEP VARIABLE STORAGE AREA FOR THIS
;INSTRUCTION IS AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER PERCENTAGE  |
;		|-----------------------|

SPEED:	MOV	(R3),R5		;% SPEED
	CMP	#1,R5		;IF SPEED ≤ 1% USE MINIMUM SPEED
	BGE	USEMIN
	CLR	R0		;NORMAL SPEED IS 100
	MOV	#144000,R1
	DIV	R5,R0		;FRACTIONAL CHANGE
	BR	.+6
USEMIN:	MOV	#77777,R0	;USE MINIMUM SPEED
	MOV	R0,@#NSPEED	;SAVE FOR "TIMER" ROUTINE
	RTS	PC

;END OF "SPEED"
;"WAITER" - WAITS FOR A SPECIFIED PERIOD OF TIME AND PROCEEDS

;THIS INSTRUCTION IS USED FOR INTRODUCING SHORT TIME DELAYS INTO
;MOTION PROGRAMS.  THE WAIT TIME IS REPRESENTED AS A INTEGER 
;CONSTANT IN THE RANGE FROM .01 TO 163.83 SECONDS.  THE MOTION
;STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   TIME IN 1/100 SEC   |
;		|-----------------------|

WAITER:	MOV	(R3),R0		;TIME TO WAIT
	BLE	1$		;NOTHING TO DO IF NEGATIVE
	ASL	R0		;CONVERT TO CLOCK COUNTS
	BVC	.+6
	MOV	#77777,R0
	MOV	R0,@#WAITTM
	MOV	#WAITLT,R1	;DO WAIT
	MOV	@#MODES,-(SP)	;SAVE MODES FOR NEXT MOTION
	JSR	PC,DRIVE
	MOV	(SP)+,@#MODES
1$:	RTS	PC


;SUBROUTINE THAT WAITS FOR A WHILE

WAITSB:	CMP	@#WAITTM,R0	;STILL WAITING?
	BHI	1$
	BIS	#FINAL,16(R1)	;NO, STOP INSTRUCTION
1$:	RTS	PC


;LOCAL STORAGE AREA

WAITLT:	0	;DUMMY DRIVE LIST
	77777	;MAXIMUM TIME
	WAITSB	;SUBR TO WAIT

;END OF "WAITER"
;"GOSUB"&"RETURN" - USER SUBROUTINE CALL AND RETURN

;THESE INSTRUCTIONS ARE USED FOR CALLING AND RETURNING FROM USER
;SUBROUTINES.  "RETURN" REQUIRES NO ARGUMENT AND THE MOTION STEP
;VARIABLE FOR "GOSUB" LOOKS LIKE THIS:
;
;		|-----------------------|
;		| PTR TO PROG SYM BLK.  |
;		|-----------------------|

;GOSUB:	MOV	@#SUBPTR,R0	;TRY TO ADD ONE TO SUBR CALL STK
;	CMP	R0,#STKEND	;MORE ROOM?
;	BLE	1$		;NOPE
;	MOV	R4,-(R0)	;SAVE RETURN ADDR
;	MOV	(R3),R1		;START EXECUTING THIS GUY
;	MOV	R1,-(R0)
;	MOV	FSTSTP(R1),R4	;HERE IS THE 1ST PROG STEP
;	MOV	R0,@#SUBPTR
;	RTS	PC

;1$:	MOV	#CATERR,@#ARMS	;SIGNAL ERROR
;	MOV	#SUBERR,R1
;	JMP	TYPERR


;RETURN:	MOV	@#SUBPTR,R0	;SUBR CALL STACK PTR
;	CMP	R0,#SUBSTK	;ANYWHERE TO RETURN TO?
;	BEQ	1$		;NOPE
;	TST	(R0)+		;POP PROG. PTR
;	MOV	(R0)+,R4	;RETURN ADDR.
;	MOV	R0,@#SUBPTR
;	RTS	PC

;1$:	MOV	#CATERR,@#ARMS	;SIGNAL ERROR
;	MOV	#RETERR,R1
;	JMP	TYPERR

;END OF "GOSUB"&"RETURN"
;"PAUSE","TYPE","COMMNT"

;THE FOLLOWING MOTION INSTRUCTIONS TAKE AS THEIR ONLY ARGUMENT A
;STRING.  THE STRINGS ARE STORED IN THE STEP VARIABLE AS FOLLOWS:
;
;		|-----------------------|
;		| 2ND CHAR  |  1ST CHAR |
;		|-----------------------|
;		|           :           |
;		|-----------------------|
;		|     0     | LAST CHAR	|
;		|-----------------------|
;
;A ZERO BYTE IS USED TO MARK THE END OF THE STRING.

;"COMMNT" - DOESN'T DO ANYTHING, NO OPERATION

COMNT: 	RTS	PC


;"TYPE" - TYPES OUT MESSAGE STRING

FTYPE:	MOV	R3,SG
	JSR	PC,LINOUT
	RTS	PC


;"PAUSE" - TYPES OUT MESSAGE AND HALTS USER PROGRAM EXECUTION

PAUSE:	MOV	R3,SG   	;GET POINTER TO MESSAGE STRING
	JSR	PC,TYPSTR	;TYPE MESSAGE
	MOV	#PASC1,SG	;TYPE CRLF AND "PAUSE"
	JSR	PC,TYPSTR
	MOV	#CANPRO,@#ARMS	;INDICATE STOP EXECUTION, CAN PROCEED
	RTS	PC

PASC1:	.ASCIZ	/
PAUSE:  /
	.EVEN

;END OF "PAUSE","TYPE","COMMNT"
;"FLIP","NOFLIP","ABOVE","BELOW","RIGHTY","LEFTY"

;THE FOLLOWING INSTRUCTIONS ARE FOR ASSERTING THAT THE CONFIGURATION
;OF THE ARM IS TO BE CHANGED DURING THE NEXT ARM MOTION THAT IS NOT A
;"MOVE".  NONE OF THESE INSTRUCTIONS HAVE VARIABLE STOP AREAS.

;"RIGHTY/LEFTY" - RIGHT OR LEFT ARM CONFIGURATION

RIGHTY:	BIS	#LFRT,@#CONFIG
	BIS	#ART,@#CONFIG
	RTS	PC

LEFTY:	BIC	#ART,@#CONFIG
	BIS	#LFRT,@#CONFIG
	RTS	PC

;"ABOVE/BELOW" - APPROACH OBJECTS FROM ABOVE OR BELOW

BELOW:	BIS	#ABBL,@#CONFIG
	BIS	#ABELOW,@#CONFIG
	RTS	PC

ABOVE:	BIC	#ABELOW,@#CONFIG
	BIS	#ABBL,@#CONFIG
	RTS	PC

;"NOFLIP/FLIP" - FLIP OUTER JOINTS, MAKES THETA 5 POSITIVE/NEGATIVE

FLIP:	BIS	#FNOF,@#CONFIG
	BIS	#AFLIP,@#CONFIG
	RTS	PC

NOFLIP:	BIC	#AFLIP,@#CONFIG
	BIS	@#FNOF,@#CONFIG
	RTS	PC

;"STOP","COARSE","INTOFF","INTON","PULSE"

;THE FOLLOWING MOTION INSTRUCTIONS DO NOT TAKE ANY ARGUMENTS AND AS
;SUCH THEY DO NOT HAVE A VARIABLE STORAGE AREA IN THEIR MOTION STEP
;BLOCKS.

;"STOP" - MARKS THE END OF A PASS THROUGH A PROGRAM

FSTOP:	CLR	R4		;TERMINATE WITH NEXT STEP
	CLR	@#ARMS
	RTS	PC


;"COARSE" - SETS LOW TOLERANCE MODE FOR SIGNALING JOINTS IN RANGE

COARSE:	BIS	#LOWTOL,@#MODES
	RTS	PC


;"INTOFF" - SETS MODE TO TURN OFF HARDWARE INTEGRATION FEATURE

INTOFF:	BIS	#NOINTG,@#MODES
	RTS	PC


;"INTON" - SETS MODE TO FORCE HARDWARE INTEGRATION ON ALL OF THE TIME

INTON: 	BIS	#ONINTG,@#MODES
	RTS	PC

;"PULSE" - SETS MODE FOR HARDWARE PULSING OF HAND FOR A FEW MSEC

PULSE:	BIS	#HITHND,@#MODES
	RTS	PC

;END OF RUN-TIME ROUTINES