COMMENT ⊗   VALID 00026 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00004 00002	.TITLE RUN
C00006 00003	"GO"     - ARM MOTION IN SLEWING MODE
C00008 00004	"GRASP"  - CLOSES THE FINGERS AROUND AN OBJECT
C00010 00005	"OPEN"   - CHANGES HAND OPENING
C00012 00006	"DRIVE"  - SINGLE JOINT MOTION
C00017 00007	"DRAW"   - RELATIVE ARM MOTION USING STRAIGHT LINE MOTION
C00021 00008	"MOVE"   - ARM MOTION USING JOINT INTERPOLATION
C00023 00009	"DEPART" - RELATIVE MOTION ALONG ARM'S -Z AXIS
C00025 00010	"APPRO" - MOTION RELATIVE TO A TRANSFORM ALONG ARM'S -Z AXIS
C00027 00011	"READY"&"REST" - MOVES THE ARM TO THE READY&REST POSITIONS
C00028 00012	"JOINT"  - SUBR FOR GENERATING SET POINTS BY JOINT INTERPOLATION
C00032 00013	"SPEED"  - CHANGES THE SPEED AT WHICH MOTIONS ARE PERFORMED
C00034 00014	"DELAY" - HALTS EXECUTION FOR A SPECIFIED PERIOD OF TIME AND PROCEEDS
C00036 00015	"GOSUB"&"RETURN" - USER SUBROUTINE CALL AND RETURN
C00038 00016	"HERE"   - SETS A TRANSFORM EQUAL TO THE CURRENT ARM POSITION
C00040 00017	"SET"    - SETS A TRANSFORM EQUAL TO THE VALUE OF ANOTHER TRANSFORM
C00042 00018	"SHIFT"  - ADDS A X,Y,Z TRANSLATION TO A TRANFORMATION
C00044 00019	"SETI"   - INITIALIZES A INTEGER TO A VALUE
C00046 00020	"TYPEI"  - TYPES OUT AN INTEGER VALUE
C00047 00021	"IF"     - CONDITIONAL BRANCH STATEMENT
C00049 00022	"GOTO"   - UNCONDITIONAL JUMP INSTRUCTION
C00050 00023	"PAUSE","TYPE","COMMNT"
C00052 00024	"FLIP","NOFLIP","ABOVE","BELOW","RIGHTY","LEFTY"
C00054 00025	"STOP","COARSE","FINE","INTOFF","INTON","PULSE","NULL","NONULL"
C00056 00026	**UN-IMPLEMENTED RTNS**
C00060 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 DATA?
	BEQ	GONOT
	MOV	R1,@#GOTRN	;SAVE TRANS POINTER
	BIS	#ARMDNE,@#JTBITS;WAIT TILL ALL JTS DONE
	MOV	#1200.,@#TTIME	;ALLOW 6 SECS FOR MOTION
	MOV	#GOSUBR,@#RUNSUB;20MSEC SUBR
	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:	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	1$		;YES
	BIS	#NOSOL,R0	;INDICATE INVALID SOLUTION
	MOV	R0,R1
	BIS	#STPMVE+CANPRO,@#ARMS
1$:	BIS	#FINAL,@#ARMS	;ONLY ONE PASS THRU THIS RTN
	RTS	PC
	

;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
	CLR	@#JTBITS	;NO NULLING
	MOV	#1000.,@#TTIME	;ALLOW 5 SECS FOR OPERATION
	MOV	#GPSUB,@#RUNSUB	;20MSEC RTN
	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:	CMP	#80.,R0		;TIME TO STOP THE HAND MOTION?
	BGE	1$		;NO
	BIS	#FINAL,@#ARMS	;YES
1$:	RTS	PC


;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	#1,R0		;PERFORM HAND OPERATION DURING NEXT MOTION
	BR	.+4

OPENI:	CLR	R0		;PERFORM HAND OPERATION IMMEDIATELY

	MOV	(R3),R1		;HAND OPENING
	CMP	@#UHAND,R1	;GREATER THAN MAX?
	BGE	2$		;IN RANGE
	MOV	@#UHAND,R1	;ELSE SET EQUAL TO MAXIMUM
	BR	3$
2$:	CMP	@#LHAND,R1	;LESS THAN MIN?
	BLE	3$
	MOV	@#LHAND,R1	;SET EQUAL TO MINIMUM
3$:	MOV	R1,@#DHAND	;SET SET POINT
	BIS	#HNDBRK,@#MODES
	BIS	#HNDDNE,@#JTBITS
	TST	R0		;IMMEDIATE INSTRUCTION?
	BNE	4$		;NO
	MOV	#600.,@#TTIME	;ALLOW 3 SECS FOR OPERATION
	MOV	#OPSUB,@#RUNSUB	;20MSEC RTN
	JSR	PC,DRIVE	;GO DRIVE THE HAND
4$:	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

OPSUB:	BIS	#FINAL,@#ARMS	;SIGNAL START CHECKING JOINT DONE
	RTS	PC


;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	(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
	BIS	R1,@#JTBITS
	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	@#K45,R4	;* 45.
	ASHC	@#KM2,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,@#TTIME	;TOTAL TIME FOR MOTION
	MOV	#FDRVSB,@#RUNSUB;20MSEC RTN
	JSR	PC,DRIVE
	BR	FDRDNE

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

FDRDNE:	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	@#K2,R0
	ROL	R1		;ROUND
	ADC	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	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,@#TTIME	;TIME OUT TIME
	MOV	#DRWSUB,@#RUNSUB;20MSEC RTN
	BIS	#ARMDNE,@#JTBITS
	JSR	PC,DRIVE
	BR	DRWDNE

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

DRWDNE:	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	@#K2,R2
	ROL	R3		;ROUND
	ADC	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 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	@#K2,R2		;NORMALIZE AND ROUND
	ROL	R3
	ADC	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	#NOTDAT,R1	;GET ERROR MESSAGE
	MOV	@(R3)+,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,R3	;READY POSITION
	MOV	@#RDYASS,@#CONFIG
	BR	TCOPY

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

TCOPY:	MOV	#TTRANS,R0	;COPY TRANS INTO RAM STORAGE
	MOV	R0,R1
	MOV	#12.,R2
	MOV	(R3)+,(R0)+
	SOB	R2,.-2
	BR	JOINT

;END OF "READY"&"REST"
;"JOINT"  - SUBR FOR GENERATING 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	R1,@#JTTRAN	;SAVE TRANS POINTER
	CLR	@#JTPAS1	;FIRST LOOP THOUGH
	MOV	#600.,@#TTIME	;NEED AT LEAST 3 SEC FOR MOTION
	BIS	#ARMDNE,@#JTBITS
	MOV	#JOINTS,@#RUNSUB;20MSEC RTN
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

JOINTS:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	TST	@#JTPAS1	;NEED TO COMPUTE FINAL SET POINTS?
	BNE	JTINTP		;NO
	MOV	R0,-(SP)	;YES
	MOV	@#JTTRAN,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,@#TTIME	;TIME OUT TIME
	INC	@#JTPAS1
	MOV	(SP)+,R0
	MOV	R5,R1

JTINTP:	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	@#K2,R2
	ROL	R3
	ADC	R2
	ADD	(R1)+,R2	;+ FINAL SET POINT
	MOV	R2,(R5)+	;SAVE NEW SET POINT
	SOB	R4,FINTLP
	BR	JTITDN

JTERR:	BIS	#NOSOL,R0	;INVALID SOLUTION
	MOV	R0,R1
	BIS	#STPMVE+CANPRO,@#ARMS

JTITDN:	TST	(SP)+
	RTS	PC


;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 32767.( 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 VARIABLE    |
;		|-----------------------|
;		|   ≠0 IF <ALWAYS>      |
;		|-----------------------|

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

;END OF "SPEED"
;"DELAY" - HALTS EXECUTION FOR A SPECIFIED PERIOD OF TIME AND PROCEEDS

;THIS INSTRUCTION IS USED FOR INTRODUCING SHORT TIME DELAYS INTO
;MOTION PROGRAMS.  THE DELAY 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   |
;		|-----------------------|

DELAY:	MOV	(R3),R0		;TIME TO DELAY
	BLE	1$		;NOTHING TO DO IF NEGATIVE
	ASL	R0		;CONVERT TO CLOCK COUNTS
	BVC	.+6
	MOV	#77777,R0
	MOV	R0,@#DELYTM
	MOV	#100100,@#TTIME
	MOV	#DELYSB,@#RUNSUB
	JSR	PC,DRIVE
1$:	RTS	PC


;SUBROUTINE THAT DELAYS EXECUTION FOR A WHILE

DELYSB:	SUB	#4,R0
	CMP	@#DELYTM,R0	;STILL DELAYING?
	BHI	1$
	BIS	#STPMVE,@#ARMS	;NO, STOP INSTRUCTION
	CLR	R1
1$:	RTS	PC


;END OF "DELAY"
;"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	(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"
;"HERE"   - SETS A TRANSFORM EQUAL TO THE CURRENT ARM POSITION
 
;THIS IS JUST LIKE THE HERE MONITOR COMMAND, EXCEPT THAT THIS
;IS A USER PROGRAM INSTRUCTION AND NO POST EDITING OF THE TRANSFORM
;DATA IS INITIATED.  ITS VARIABLE DATA BLOCK LOOKS LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|

FHERE:	MOV	(R3),R3		;TRANS POINTER
	JSR	PC,HERESB
	BCC	10$		;ERROR?
	JSR	PC,TYPERR	;YES
	SEC
10$:	RTS	PC


HERESB:	CLR	R0		;READ CURRENT JOINT ANGLES
	MOV	#6,R1		;SIX JOINTS IN ALL
	MOV	#JANGLES,R2
	JSR	PC,ANGLES	
	BCS	20$		;BRANCH IF ADC ERROR
	MOV	(R3),R0		;GET PTR TO DATA
	BNE	10$
	MOV	#12.,R0		;GET A BLOCK OF F.S. IF NOT DEFINED
	JSR	PC,GETBLK
	BCS	20$		;BRANCH IF NO ROOM LEFT
	MOV	R0,(R3)		;SET PTR TO TRANS DATA AREA
10$:	MOV	#JANGLE,R1	;HERE ARE THE CURRENT ANGLES
	JSR	PC,UPDATE	;DO CONVERSION
	CLC			;ALL DONE NOW
20$:	RTS	PC


;END OF "FHERE"
;"SET"    - SETS A TRANSFORM EQUAL TO THE VALUE OF ANOTHER TRANSFORM

;THIS INSTRUCTION IS USED TO SET THE VALUE OF TRANSFORM "A" EQUAL TO
;THE VALUE OF TRANSFORM "B".  THE VARIABLE PORTION OF THIS
;INSTRUCTION IS STORED AS FOLLOWS:
;
;		|-----------------------|
;		| ADDR OF TRANS A S.B.  |
;		|-----------------------|
;		| ADDR OF TRANS B S.B.  |
;		|-----------------------|

SET:	MOV	(R3)+,R2	;TRANS A SYM BLK
	MOV	@(R3)+,R3	;TRANS B DATA
	BEQ	20$		;ERROR IF B UNDEFINED
	MOV	(R2),R0		;A DATA
	BNE	10$
	MOV	#12.,R0		;GET A BLOCK OF F.S. IF NOT DEFINED
	JSR	PC,GETBLK
	BCS	30$		;BRANCH IF NO ROOM LEFT
	MOV	R0,(R2)		;SET PTR TO TRANS DATA AREA
10$:	MOV	#12.,R1		;TRANSFER 12 WORDS IN ALL
	MOV	(R3)+,(R0)+
	SOB	R1,.-2
	BR	SETDNE

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

SETDNE:	RTS	PC

;END OF "SET"
;"SHIFT"  - ADDS A X,Y,Z TRANSLATION TO A TRANFORMATION

;THE SHIFT INSTRUCTION ALTERS THE X,Y,Z POSITION OF A TRANSFORMATION.
;ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|    PTR TO A TRANS.	|
;		|-----------------------|
;		| CHANGE IN X DIRECTION |
;		|-----------------------|
;		| CHANGE IN Y DIRECTION |
;		|-----------------------|
;		| CHANGE IN Z DIRECTION |
;		|-----------------------|

SHIFT:	MOV	@(R3)+,R0	;TRANS DATA DEFINED?
	BEQ	2$
	ADD	#18.,R0		;PTR TO X
	ADD	(R3)+,(R0)+	;SHIFT X
	BVS	1$		;OVERFLOW?
	ADD	(R3)+,(R0)+	;SHIFT Y
	BVS	1$
	ADD	(R3),(R0)	;SHIFT Z
	BVC	4$
1$:	MOV	#OUTRNG,R1	;OVERFLOW ERROR MESSAGE
	BR	3$

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

4$:	RTS	PC

;END OF "SHIFT"
;"SETI"   - INITIALIZES A INTEGER TO A VALUE

;THIS INSTRUCTION IS USED TO PERFORM INTEGER ARITHMETIC OPERATIONS OF
;THE FORM A ← B <OPERATION> C.  THE VARIABLE PORTION OF THIS INSTRUCTION
;IS STORED AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|
;		|   ARITHMETIC OPER.    |
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|

SETII:	MOV	(R3)+,R5	;RETURN VALUE IN HERE
	MOV	@(R3)+,R0	;VALUE OF A
	ADD	(R3)+,PC	;JUMP TO CORRECT OPERATION
	BR	SAVVAL		;SIMPLE ASSIGHMENT
	BR	10$		;*
	BR	20$		;/
	BR	30$		;+
	BR	40$		;-

	MOV	R0,R1		;%
	SXT	R0
	DIV	@(R3)+,R0
	BVC	15$		;ARITHMETIC OVERFLOW?
	BR	OVERFL

10$:	MUL	@(R3)+,R0	;*
	BCS	OVERFL
15$:	MOV	R1,R0
	BR	SAVVAL

20$:	MOV	R0,R1		;/
	SXT	R0
	DIV	@(R3)+,R0
	BR	45$

30$:	ADD	@(R3)+,R0	;+
	BR	45$

40$:	SUB	@(R3)+,R0	;-
45$:	BVS	OVERFL


SAVVAL:	MOV	R0,(R5) 	;SAVE VALUE
	RTS	PC

OVERFL:	MOV	#ARITHO,R1	;INDICATE ERROR
	BIS	#CANPRO,@#ARMS
	JMP	TYPERR

;END OF "SETI"
;"TYPEI"  - TYPES OUT AN INTEGER VALUE

;THIS INSTRUCTION IS USED FOR TYPING OUT AN INTEGER VALUE.
;THE VARIABLE PORTION OF THIS INSTRUCTION IS STORED AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|

TYPEI:	MOV	@(R3)+,R0	;GET VALUE
	MOV	#OUTBUF,SG	;PRINT IT
	JSR	PC,PRTINT
	MOV	#OUTBUF,SG
	JSR	PC,LINOUT
	RTS	PC

;END OF "TYPEI"
;"IF"     - CONDITIONAL BRANCH STATEMENT

;THIS INSTRUCTION PERFORMS A PROGRAM BRANCH INSTRUCTION BASED UPON THE
;RESULTS OF A ARITHMETIC COMPARISON.  THE COMPARISON IS OF THE FORM
;A <COMPARISON OPERATOR> B.  THE VARIABLE PORTION OF THIS INSTRUCTION
;IS STORED AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|
;		|   COMPARISON OPER.    |
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|
;		|   BRANCH POINTER      |
;		|-----------------------|

IFI:	MOV	@(R3)+,R0	;VALUE OF A
	MOV	(R3)+,R1	;COMPARISON OPERATOR
	CMP	R0,@(R3)+	;COMPARE A TO B
	CLC
	JSR	PC,@(R1)+	;C ← SET IF BRANCH CONDITION NOT TRUE
	BCC	GOTO		;EXECUTE BRANCH?
	RTS	PC


;END OF "IF"
;"GOTO"   - UNCONDITIONAL JUMP INSTRUCTION

;THE SEQUENCE OF PROGRAM STEP EXECUTION IS ALTERED SO THAT THE
;STEP SPECIFIED BY THE LABEL ARGUMENT IS THE NEXT PROGRAM STEP
;EXECUTED.  THE VARIABLE STORAGE AREA OF THIS INSTRUCTION LOOKS AS
;FOLLOWS:
;
;		|-----------------------|
;		|      STEP LABEL       |
;		|-----------------------|

GOTO:	MOV	@(R3)+,R0	;GET JUMP ADDRESS
	BNE	10$
	MOV	#MISLBL,R1		;NO ADDR, SIGNAL ERROR
	BIS	#CANPRO,@#ARMS
	JSR	PC,TYPERR
	BR	20$
10$:	MOV	R0,R4		;SET NEXT INSTRUCTION ADDR
20$:	RTS	PC

;END OF "GOTO"
;"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","FINE","INTOFF","INTON","PULSE","NULL","NONULL"

;THE FOLLOWING MOTION INSTRUCTIONS TAKE AT MOST ONE ARGUMENT ["ALWAYS"].

;"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
	TST	(R3)		;ALWAYS?
	BEQ	.+10
	BIS	#LOWTOL,@#PMODES ;YES
	RTS	PC

;"FINE" - SETS HIGH TOLERANCE MODE FOR SIGNALING JOINTS IN RANGE

FINE:	BIC	#LOWTOL,@#MODES
	TST	(R3)		;ALWAYS?
	BEQ	.+10
	BIC	#LOWTOL,@#PMODES ;YES
	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

;"NONULL" - TERMINATES MOTIONS IMMEDIATELY WITH NULLING FINAL ERRORS

NONULL:	BIS	#NNUL,@#MODES
	TST	(R3)		;ALWAYS?
	BEQ	.+10
	BIS	#NNUL,@#PMODES ;YES
	RTS	PC

;"NULL" - OPPOSITE OF NONULL

NULL:	BIC	#NNUL,@#MODES
	TST	(R3)		;ALWAYS?
	BEQ	.+10
	BIC	#NNUL,@#PMODES ;YES
	RTS	PC

;END OF RUN-TIME ROUTINES
;**UN-IMPLEMENTED RTNS**
.IFNZ 0

;"FOR","NEXT" - CONTROL STATEMENTS FOR PROGRAM LOOPS

;THESE INSTRUCTIONS ARE USED FOR CREATING A EXECUTION LOOP
;WITHIN A PROGRAM.  THE "FOR" STATEMENT TAKES 4 ARGUMENTS:
;A INTEGER VARIABLE TO STEP, AN INITIAL AND FINAL VALUE,
;AND A STEP SIZE.  THE VARIABLE PORTION OF THIS INSTRUCTION
;IS STORED AS FOLLOWS:
;
;		|-----------------------|
;		|   PTR TO INT. VAR.    |
;		|-----------------------|
;		|     INITIAL VALUE	|
;		|-----------------------|
;		|      FINAL VALUE	|
;		|-----------------------|
;		|       STEP SIZE	|
;		|-----------------------|
;
;THE "NEXT" STATEMENT HAS ONLY ONE ARGUMENT, A POINTER TO THE
;SYMBOL BLOCK OF THE INTEGER VARIABLE TO BE STEPED.  ITS
;VARIABLE PORTION IS STORED AS FOLLOWED:
;
;		|-----------------------|
;		|       STEP SIZE	|
;		|-----------------------|

FOR:	MOV	@#FORPTR,R0	;PTR TO LIST OF ACTIVE LOOPS
	BNE	.+6
	MOV	#FORLST-2,R0	;INIT. IF NO ACTIVE LOOPS
	TST	(R0)+		;ROOM FOR ONE MORE?
	CMP	R0,#FOREND
	BGE	FORERR		;NO
	MOV	R2,(R0)		;YES, SAVE PTR TO TOP OF LOOP
	MOV	R0,@#FORPTR
	MOV	(R2)+,R0	;SET VARIABLE TO INITIAL VALUE
	MOV	(R2),DATPTR(R0)
	BR	FORDN

FORERR:	MOV	#FORFUL,R1	;SIGNAL NO ROOM LEFT FOR LOOPS
	MOV	#STPARM,@#ARMS	;CANT PROCEED
	JSR	PC,TYPERR
FORDN:	RTS	PC

NEXT:	


;END OF "FOR","NEXT"



;"PUTADC" -  SETS INT. VARIABLE EQUAL TO THE VALUE OF A ADC CHAN

;THIS INSTRUCTIONS READS ONE ADC CHANNEL AND SAVES ITS VALUE
;IN THE SPECIFIED INTEGER VARIABLE.  THE VARIABLE PORTION OF
;THIS INSTRUCTION IS STORED AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|
;		|   INTEGER VARIABLE    |
;		|-----------------------|

PUTADC:	MOV	(R2)+,R0	;GET CHANNEL NUMBER IN R0
	JSR	PC,FCHVAR
	MOV	#BADCHN,R1	;ERROR MESSAGE IF BAD CHAN #
	TST	R0		;0 ≤ CHANNEL # ≤ 31?
	BMI	PUTAER
	CMP	#31.,R0
	BLT	PUTAER
	MOV	R0,R5		;SAVE CHANNEL INDEX
	MOV	#1,R1		;READ THE 1 CHANNEL
	JSR	PC,ANGLES
	BCS	PUTAER		;ADC ERROR?
	ASL	R5		;NO, GET CHANNEL VALUE
	MOV	JANGLE(R5),R0
	MOV	(R2),R1		;SAVE THE VALUE IN HERE
	JSR	PC,PUTVAR
	BCC	PUTAOK

PUTAER:	BIS	#CANPRO,@#ARMS	;INDICATE ERROR
	JSR	PC,TYPERR

PUTAOK:	RTS	PC

;END OF "PUTADC"
.ENDC