COMMENT ⊗   VALID 00015 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	.TITLE FUNCTS
C00005 00003	"HERE"   - COMMAND INSTRUCTION
C00007 00004	"TRANS"  - COMMAND INSTRUCTION
C00009 00005	"WHERE"  - COMMAND INSTRUCTION
C00012 00006	"TF"     - COMMAND INSTRUCTION
C00014 00007	"CLEAR"  - COMMAND INSTRUCTION
C00016 00008	"LISTT"  - LISTS THE VALUES OF STORED TRANSFORMATION
C00019 00009	"LISTP"  - LISTS THE STEPS OF A USER PROGRAM
C00022 00010	"PUNCHT"&"PUNCHP" - SAME AS LISTT&LISTP TO PAPER TAPE
C00023 00011	"PROGS"  - LISTS THE NAMES OF ALL USER PROGRAMS
C00025 00012	"CALLS"  - LISTS THE SEQUENCE OF "GOSUB" CALLS
C00027 00013	"FREE"   - COMMAND INSTRUCTION
C00029 00014	"EXEC"   - COMMAND INSTRUCTION
C00034 00015	"PROCEED","SNGSTP" - COMMAND INSTRUCTIONS
C00036 ENDMK
C⊗;
.TITLE FUNCTS

;START OF TOP LEVEL ARM PROGRAM

START:	RESET
	MTPS	LOCK		;INITIALIZE PROCESSOR STATUS
	MOV 	#STKTOP,SP	;INITIALIZE STACK
.IFZ LSI
	MOV	#1000.,CLKSET	;SET UP THE REAL TIME CLOCK
	MOV	#111,CLKS
;	MOV	#PARSER,PARVEC	;ENABLE PARITY ERROR TRAPS
	MOV	#LOCK,PARVEC+2
	MOV 	#1,PARCSR
.ENDC
	MOV	#CLKSER,@#CLKTRP;SET UP CLOCK VECTOR
	MOV	#LOCK,@#CLKTRP+2
	MOV	#BUF1,BUF2	;SET UP "DRIVE" RING BUFFERS
	MOV	#BUF2,BUF1
	MOV	#BUF1,CBUF
	MOV	#HELLO,SG	;TELL EVERYONE WHO WE ARE
	JSR	PC,LINOUT
MAINL:	MOV	#QUERY,SG	;ASK FOR INSTRUCTION
	JSR	PC,TYPSTR
	MOV	#INBUF,SG	;READ IN A COMMAND INSTRUCTION
	JSR	PC,INSTR
	MOV	#FUNTAB,R0	;DECODE IT,NEED FUNCTION HASH TABLE
	MOV	#CMND,R1	;ONLY LOOK FOR TOP LEVEL COMMANDS
	JSR	PC,PUSARG
	BCC	GOTCOM		;BRANCH IF LEGAL COMMAND
	TST	R1		;TEST IF EMPTY STRING 
	BEQ	MAINL		;LOOP BACK IF EMPTY LINE
	JSR	PC,TYPERR 	;ELSE TYPE ERROR MESSAGE
	BR	MAINL
GOTCOM:	JSR	PC,@FUNPTR(R0)	;EXECUTE FUNCTION
	ADD	#MAXARG,SP	;CLEAR ARGUMENT LIST OFF STACK
	BR	MAINL

DONE:	MOV	#GOODBY,R1	;ALL DONE, STOP MONITOR
	JSR	PC,TYPERR
	ADD	#MAXARG+2,SP	;LEAVE STACK CLEAR
	MOV	#2,@#CLKTRP+2	;CLEAR CLOCK INTERRUPT VECTOR
	MOV	#CLKTRP+2,@#CLKTRP
	HLT

QUERY:	.ASCIZ	/./

;END OF TOP LEVEL SEQUENCE
;"HERE"   - COMMAND INSTRUCTION
 
;THIS ROUTINES SETS A GIVEN TRANSFORM EQUAL TO THE PRESENT POSITION
;OF THE ARM.  THE ONLY ARGUMENT REQUIRED FOR THIS FUNCTION IS A PTR
;TO THE TRANSFORMATION.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

HERE:	CLR	R0		;READ CURRENT JOINT ANGLES
	MOV	#6,R1		;SIX JOINTS IN ALL
	MOV	#JANGLES,R2
	JSR	PC,ANGLES	
	BCC	HERE1		;BRANCH IF NO ADC ERROR
	JSR	PC,TYPERR	;SIGNAL ERROR
	BR	HERDNE
HERE1:	MOV	2(SP),R2	;GET PTR TO TRANS SYMBOL BLOCK
	MOV	TRNPTR(R2),R0	;GET PTR TO DATA
	BNE	HERE2
	MOV	#12.,R0		;GET A BLOCK OF F.S. IF NOT DEFINED
	JSR	PC,GETBLK
	BCS	HERDNE		;BRANCH IF NO ROOM LEFT
	MOV	R0,TRNPTR(R2)	;SET PTR TO TRANS DATA AREA
HERE2:	MOV	#JANGLE,R1	;HERE ARE THE CURRENT ANGLES
	JSR	PC,UPDATE	;DO CONVERSION
	MOV	TRNPTR(R2),R0
	JSR	PC,MODTRN	;PERMIT EDITING OF TRANSFORM
HERDNE:	RTS	PC


;END OF "HERE"
;"TRANS"  - COMMAND INSTRUCTION
 
;THIS COMMAND IS USED FOR INITIALIZING AND EDITING THE X,Y,Z,O,A,T
;VALUES OF A SPECIFIED TRANSFORM.  THE TRANSFORMATION POINTER IS
;ASSUMED TO BE IN THE ARGUMENT LIST ON THE STACK.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

STRANS:	MOV	2(SP),R2	;GET PTR TO TRANS SYMBOL BLOCK
	MOV	TRNPTR(R2),R0	;GET PTR TO DATA
	BNE	STRAN2
	MOV	#12.,R0		;GET A BLOCK OF F.S. IF NOT DEFINED
	JSR	PC,GETBLK
	BCS	STNDNE		;BRANCH IF NO ROOM
	MOV	R0,TRNPTR(R2)	;SET PTR TO TRANS DATA AREA
	MOV	#SMPTRN,R3	;INIT. TRANSFORM TO REASONABLE POS
	MOV	R0,R1
	MOV	#12.,R4
	MOV	(R3)+,(R1)+
	SOB	R4,.-2
STRAN2:	JSR	PC,MODTRN	;PERMIT EDITING OF TRANSFORM
STNDNE:	RTS	PC


;END OF "TRANS"
;"WHERE"  - COMMAND INSTRUCTION
 
;THIS COMMAND IS USED FOR TYPING OUT THE CURRENT ARM POSITION.  THE
;ARM POSITION IS PRINTED BOTH IN EULER ANGLES AND JOINT ANGLES.  THE
;HAND OPENING IS ALSO LISTED IN INCHES.  AS A SIDE AFFECT, "WHERE"
;UPDATES "CTRANS" WITH THE CURRENT ARM TRANSFORM.  NO ARGUMENTS ARE
;REQUIRED BY THIS ROUTINE.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

WHERE:	CLR	R0		;READ JT. ANGLES/HAND OPENING
	MOV	#7,R1		;SEVEN CHANNELS IN ALL
	MOV	#JANGLE,R2
       	JSR	PC,ANGLES	;PUT INTO "JANGLE"
	BCC	WHER1 		;BRANCH IF NO ADC ERROR
	JSR	PC,TYPERR
	BR	WHEDNE		;EXIT
WHER1:	MOV	#CTRANS,R0	;PUT CURRENT TRANSFORM IN HERE
	MOV	#JANGLE,R1	;GET JOINT ANGLES FROM HERE
	JSR	PC,UPDATE	;CONVERT JT. ANGLES TO TRANSFORM
      	MOV     #HTRANS+7,SG	;TYPE OUT THE COLUMN HEADER 
	JSR	PC,LINOUT
	MOV	#CTRANS,R0
      	JSR	PC,PTRANS	;TYPE OUT THIS TRANSFORM
	MOV	#WHERC1,SG	;TYPE OUT JOINT ANGLES COLUMN HEADER
	JSR	PC,LINOUT
	MOV	#OUTBUF,SG	;CONVERT JOINT ANGLES TO ASCII
	MOV	#JANGLE,R2	;HERE ARE THE ANGLES
	MOV	#6,R3		;PRINT 6 ANGLES AND HAND OPENING
WHER2:	MOV	(R2)+,R0 	;GET AN ANGLE
	JSR	PC,PRTANG	;CONVERT TO ASCII
	MOVB	#40,(SG)+	;PUT IN A SPACE CHARACTER
	SOB	R3,WHER2
	MOV	(R2),R0		;CONVERT THE HAND OPENING TO INCHES
	JSR	PC,PRTDIS
	MOV	#OUTBUF,SG	;PRINT THE ASC STRING
	JSR	PC,LINOUT
WHEDNE:	RTS	PC

WHERC1:	.ASCII	/   JT 1    JT 2    JT 3    JT 4    JT 5    /
	.ASCIZ	/JT 6    HAND/
	.EVEN

;END OF "WHERE"
;"TF"     - COMMAND INSTRUCTION
 
;THIS ROUTINES INITIALIZES THE VALUE OF ONE TRANSFORM.  IT REQUIRES
;AS ITS ARGUMENTS, THE TRANSFORM NAME AND ITS 6 DEFINING VALUES.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

TF:	MOV	SP,R3		;GET PTR TO ARGUMENTS
	TST	(R3)+
      	MOV	(R3)+,R2	;GET PTR TO TRANS SYMBOL BLOCK
	MOV	TRNPTR(R2),R0	;GET PTR TO DATA
	BNE	TF2
	MOV	#12.,R0		;GET A BLOCK OF F.S. IF NOT DEFINED
	JSR	PC,GETBLK
	BCS	TFDNE 		;BRANCH IF NO ROOM
	MOV	R0,TRNPTR(R2)	;SET PTR TO TRANS DATA AREA
TF2:	MOV	R3,R1		;CONVERT EULER ANGLES TO TRANFORM
	JSR	PC,UNEUL 	;DO CONVERSION
TFDNE: 	RTS	PC


;END OF "TF"
;"CLEAR"  - COMMAND INSTRUCTION
 
;THIS COMMAND IS USED FOR RE-INITIALIZING THE ARM PROGRAM.  IT DOES
;THIS BY ZEROING ALL VARIABLE WORDS FROM "ZAPSTR" TO "ZAPEND".  NO
;ARGUMENT IS REQUIRED BY THIS ROUTINE, HOWEVER RE-CONFIRMATION OF
;THE CLEAR COMMAND IS REQUESTED.

;REGISTERS USED:
;	ALL REGISTERS ARE AVAILABLE FOR USE

CLEAR:	MOV	#SURE,SG	;REAFFIRM COMMAND REQUEST
	JSR	PC,TYPSTR
	MOV	#INBUF,SG	;INPUT RESPONSE
	JSR	PC,INSTR
	CMPB	#131,(SG)+	;"Y" ?
	BNE	NOCLR
	TSTB	(SG)		;THIS SHOULD BE A NULL
	BNE	NOCLR
	MOV	#ZAPEND,R0	;NUMBER OF WORDS TO ZERO
	SUB	#ZAPSTR,R0
	ASR	R0
	MOV	#ZAPSTR,R1	;START CLEARING AT THIS LOCATION
	CLR	(R1)+
	SOB	R0,.-2
	MOV	#CLRFIN,SG	;TELL EVERYONE IT'S DONE
	BR	.+6
NOCLR:	MOV	#CANCLR,SG	;CANCEL CLEAR COMMAND
	JMP	LINOUT

SURE:	.ASCIZ	/ARE YOU SURE (Y,N)? /
CLRFIN:	.ASCIZ	/ARM PROGRAM RE-INITIALIZED, ALL FREE STORAGE RECLAIMED/
CANCLR:	.ASCIZ	/CLEAR COMMAND ABORTED/
	.EVEN

;END OF "CLEAR"
;"LISTT"  - LISTS THE VALUES OF STORED TRANSFORMATION

;LISTS UP TO EIGHT TRANSFORMATIONS THAT ARE SPECIFIED BY THE USER.
;IT IS ASSUMED THAT THE TRANSFORMATIONS SYMBOL BLOCK POINTERS ARE
;ON THE STACK.  IF NO TRANSFORMATIONS ARE SPECIFIED, ALL EXISTING
;TRANSFORMATIONS ARE LISTED.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

LISTT:	MOV	#HTRANS,SG	;PRINT THE HEADER
	JSR	PC,LINOUT
	CLR	@#ISPNHT	;INDICATE "LISTT" INSTRUCTION

LSTSTR:	CLR	R4		;# OF TRANS' PRINTED
	MOV	#8.,R2		;LIMITED LIST OF 8 MAX
	MOV	SP,R3		;GET PTR TO LIST
	TST	(R3)+
LISTT1:	MOV	(R3)+,R0
	BEQ	DNTPTR
	MOV	@#ISPNHT,R1	;PRINT ALL SPECIFIED TRANSFORMS
	JSR	PC,PTRTRN
	INC	R4		;ONE MORE TRANS PRINTED
DNTPTR:	SOB	R2,LISTT1
	TST	R4		;PRINT ALL TRANS' IF NONE SPECIFIED
	BNE	LSTDNE
LSTALL:	MOV	#32.,R2		;CHECK ALL 32. HASH BUCKETS FOR TRANS
	MOV	#VARTAB,R3	;PTR TO FIRST BUCKET
LSTAL1:	MOV	(R3)+,R4	;GET FIRST POINTER
	BEQ	LSTAL4
LSTAL2:	BITB	#TRANS,TYPBIT(R4)	;CHECK IF TRANS VARIABLE
	BEQ	LSTAL3
	MOV	R4,R0		;GOT A TRANS, PRINT IT
	MOV	@#ISPNHT,R1
	JSR	PC,PTRTRN
LSTAL3:	MOV	(R4),R4		;NEXT ITEM IN BUCKET
	BNE	LSTAL2		
LSTAL4:	SOB	R2,LSTAL1	;REPEAT FOR ALL BUCKETS
LSTDNE:	TST	@#ISPNHT	;NEED MORE BLANK TAPE?
	BEQ	.+6
	JSR	PC,NULLS
	RTS	PC

;END OF "LISTT"
;"LISTP"  - LISTS THE STEPS OF A USER PROGRAM

;LISTS THE SPECIFIED STEPS OF A USER PROGRAM.  IF NO FIRST STEP IS
;SPECIFIED, STEP ONE IS ASSUMED.  IF NO LAST STEP IS SPECIFIED,
;PRINTING IS CONTINUED UNTIL THE END OF THE PROGRAM IS ENCOUNTERED.
;THE ARGUMENTS FOR THIS ROUTINE ARE ASSUMED TO BE ON THE STACK IN THE
;FOLLOWING ASCENDING ORDER: PROGRAM PTR, 1ST STEP, LAST STEP.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

LISTP:	CLR	@#ISPNHP	;INDICATE "LISTP"

LSPSTR:	MOV	2(SP),R0	;PTR TO PROGRAM SYMBOL BLOCK
	MOV	#LSTPMS,SG	;TYPE PROGRAM NAME
	JSR	PC,TYPSTR
	MOV	#OUTBUF,SG
	JSR	PC,PACNMS
	MOV	#OUTBUF,SG
	JSR	PC,LINOUT
	MOV	R0,R1		;PTR TO PROGRAM SYMBOL BLOCK
	ADD	#FSTSTP,R1	;PTR TO FIRST PROGRAM STEP
	TST	(R1)
	BEQ	NOSTPS
	MOV	4(SP),R2	;FIRST STEP NUMBER
	BGT	.+6
	MOV	#1,R2		;DEFAULT = STEP 1
	MOV	6(SP),R3	;FINAL STEP NUMBER
	BGT	.+6
	MOV	#77777,R3	;DEFAULT = LAST PROGRAM STEP
	SUB	R2,R3		;NUMBER OF STEPS TO PRINT-1
	BGE	LISTP2
	MOV	#BADSTP,R1	;SIGNAL ERROR IF FINAL<FIRST
	JSR	PC,TYPERR
	BR	LSTPDN
LISTP2:	INC	R3
       	MOV	R2,R4		;GET FIRST REQUESTED STEP
LISTP3:	MOV	(R1),R1
	BEQ	LSTPDN		;NOTHING TO DO IF PAST END
	SOB	R4,LISTP3
LISTP4:	MOV	R2,R0		;PRINT THE REQUESTED STEPS
	JSR	PC,PSTEP
	INC	R2		;INCREASE STEP NUMBER
	MOV	(R1),R1
	BEQ	LSTPDN		;DONE IF END OF PROGRAM
	SOB	R3,LISTP4
	BR	LSTPDN
NOSTPS:	MOV	#NULPRG,R1	;INDICATE NO PRG STEPS DEFINED
	JSR	PC,TYPERR
LSTPDN:	JSR	PC,CRLF
	TST	@#ISPNHP	;NEED MORE BLANK TAPE?
	BEQ	.+6
	JSR	PC,NULLS
	RTS	PC

LSTPMS:	.ASCIZ	/DEFPRO /
	.EVEN

;END OF "LISTP"
;"PUNCHT"&"PUNCHP" - SAME AS LISTT&LISTP TO PAPER TAPE

;THESE ROUTINES ARE IDENTICAL TO "LISTT" AND "LISTP" EXCEPT THAT
;NO HEADERS ARE TYPED OUT AND INSTEAD NULL CHARACTERS ARE PRINTED
;BEFORE AND AFTER THE DATA TO PROVIDE SOME BLANK LEADER.

;REGISTERS USED:
;
;	ALL REGISTERS ARE AVAILABLE FOR USE

PUNCHT:	JSR	PC,NULLS	;PUNCH OUT A LEADER TAPE
	MOV	#-1,@#ISPNHT	;INDICATE PUNCHT COMMAND
	JMP	LSTSTR		;NOW JUST LIKE "LISTT"

PUNCHP:	JSR	PC,NULLS	;PUNCH OUT A LEADER TAPE
	MOV	#-1,@#ISPNHP	;INDICATE PUNCHT COMMAND
	JMP	LSPSTR		;NOW JUST LIKE "LISTP"

;END OF "PUNCHT"&"PUNCHP"
;"PROGS"  - LISTS THE NAMES OF ALL USER PROGRAMS

;THIS ROUTINE LISTS THE NAMES OF ALL USER PROGRAMS THAT HAVE BEEN
;DEFINED.  IT REQUIRES NO ARGUMENTS.

;REGISTERS USED:
;	ALL REGISTERS ARE AVAILABLE FOR USE

PROGS:	MOV	#32.,R2		;CHECK ALL 32. HASH BUCKETS FOR PROGS
	MOV	#VARTAB,R3	;PTR TO FIRST BUCKET
	MOV	#"  ,@#OUTBUF
PROGS1:	MOV	(R3)+,R4	;GET FIRST POINTER
	BEQ	PROGS4
PROGS2:	BITB	#PROG,TYPBIT(R4)  ;CHECK IF PROGRAM NAME
	BEQ	PROGS3
	MOV	R4,R0		;GOT A PROGRAM NAME, PRINT IT
	MOV	#OUTBUF+2,SG
	JSR	PC,PACNMS
	MOV	#OUTBUF,SG
	JSR	PC,LINOUT
PROGS3:	MOV	(R4),R4		;NEXT ITEM IN BUCKET
	BNE	PROGS2
PROGS4:	SOB	R2,PROGS1	;REPEAT FOR ALL BUCKETS
	RTS	PC

;END OF "PROGS"
;"CALLS"  - LISTS THE SEQUENCE OF "GOSUB" CALLS

;THIS ROUTINE LISTS THE LEVELS OF "GOSUB" CALLS THAT ARE NOW ACTIVE.
;IT REQUIRES NO ARGUMENTS.

;REGISTERS USED:
;	ALL REGISTERS ARE AVAILABLE FOR USE

CALLS:	MOV	#SUBSTK+2,R3	;SUBR STACK
	MOV	#CALHDR,SG	;PRINT HEADER
	JSR	PC,LINOUT
	MOV	#"  ,@#OUTBUF
1$:	MOV	#OUTBUF+2,SG
	MOV	-(R3),R0	;GET SUBR NAME
	BEQ	4$
	JSR	PC,PACNME	;SAVE IN OUTBUF
	MOV	-(R3),R2	;RETURN ADDR
	BEQ	3$
	MOV	FSTSTP(R0),R1	;COMPUTE STEP NUMBER
	CLR	R0
2$:	MOV	(R1),R1		;KEEP MOVING
	INC	R0
	CMP	R1,R2		;FOUND STEP?
	BNE	2$		;NO
	JSR	PC,PTSINT	;YES, CONVERT TO ASCII
3$:	MOV	#OUTBUF,SG
	JSR	PC,LINOUT
	CMP	R3,@#SUBPTR	;END OF LIST?
	BGT	1$		;NO
4$:	RTS	PC

CALHDR:	.ASCIZ	/RTN   LAST STEP/
	.EVEN

;END OF "CALLS"
;"FREE"   - COMMAND INSTRUCTION
 
;THIS ROUTINE TYPES OUT THE AMOUNT OF FREE STORAGE SPACE THAT IS
;NOT CURRENTLY BEING USED.  NO ARGUMENTS ARE REQUIRED.

;REGISTERS USED:
;	ALL REGISTERS ARE AVAILABLE FOR USE

FREE:	CLR	R5		;NUMBER OF FREE BYTES
	MOV	#10000.,R0	;100% FREE?
	TST	@#FSPTR
	BEQ	ALLFRE		;YES
	MOV	#FREEST+2,R2	;START ADDING FROM HERE
	MOV	@#HICORE,R4	;END OF F.S.
FREELP:	MOV	(R2),R3		;NEXT BOUNDARY TAG
	BEQ	FREEER		;CAN'T EVER BE THIS
	BPL	MOREFR		;>0 INDICATES NOT BEING USED
	NEG	R3		;ON TO NEXT
	BR	NXTBLK
MOREFR:	ADD	R3,R5		;ADD TO FREE SUM
NXTBLK:	MOV	R2,R1		;CHECK FOR VALID BOUNDARY TAGS
	ADD	R3,R2
	CMP	(R1),-2(R2)	;HI=LOW?
	BNE	FREEER
	CMP	R4,R2		;END OF F.S.?
	BHI	FREELP
	SUB	#FREEST,R4	;TOTAL SIZE OF F.S AREA
	TST	-(R4)
	MUL	R5,R0		;COMPUTE PERCENTAGE FREE
	DIV	R4,R0
ALLFRE:	MOV	#FREMES,SG	;TYPE OUTPUT MESSAGE
	JSR	PC,TYPSTR
	MOV	#OUTBUF,SG	;CONVERT PERCENTAGE TO ASCII
	JSR	PC,PTSHUN
	MOVB	#45,(SG)+	;%
	CLRB	(SG)
	MOV	#OUTBUF,SG
	JSR	PC,LINOUT
	BR	FREDNE

FREEER:	MOV	#BADFRE,R1	;SAY F.S. AREA IN WRONG FORMAT
	JSR	PC,TYPERR

FREDNE:	RTS	PC

FREMES:	.ASCIZ	/UNUSED FREE STORAGE = /
	.EVEN

;END OF "FREE"
;"EXEC"   - COMMAND INSTRUCTION

;THIS COMMAND IS USED FOR INITIATING ARM MOTION PROGRAMS.  IT
;REQUIRES THREE ARGUMENTS:  A USER PROGRAM NAME, A LOOP COUNT, AND
;A STARTING STEP NUMBER FOR THE FIRST PASS.  IF THE PROGRAM NAME IS
;OMITTED, THE LAST PROGRAM EXECUTED IS AGAIN RUN.  IF THE COUNT IS
;MISSING, ONE PASS IS ASSUMED.  A PASS ENDS WHENEVER A "STOP"
;INSTRUCTION IS ENCOUNTERED.  FOR MULTIPLE PASS COMMANDS, THE STOP
;MESSAGE IS SUPPRESSED UNTIL THE FINAL PASS IS COMPLETED.
;IF THE STARTING STEP NUMBER IS OMITTED, EXECUTION BEGINS
;WITH THE FIRST PROGRAM INSTRUCTION.

;REGISTERS USED:
;	ALL REGISTERS ARE AVAILABLE FOR USE

EXEC:	CLR	@#ARMS		;CLEAR ALL FLAGS
	MOV	2(SP),R2	;PROGRAM PTR
	BNE	1$
	MOV	#NOPROG,R1	;ERROR CODE IF NO PROGS EXECUTED YET
	MOV	@#SUBSTK,R2	;RE-TRY LAST PROGRAM
	BEQ	EXECER
1$:	MOV	R2,@#SUBSTK	;SET-UP SUBROUTINE STACK
	MOV	#SUBSTK-2,@#SUBPTR
       	MOV	#NULPRG,R1	;ERROR MESSAGE FOR NO PROGRAM STEPS
	MOV	FSTSTP(R2),R2	;PTR TO FIRST STEP TO EXECUTE
	BEQ	EXECER
	MOV	4(SP),R0
	BGT	.+6
	MOV	#1,R0		;DEFAULT = 1 PASS
	MOV	R0,@#EXECNT	;PASS COUNT
	MOV	6(SP),R0	;GET STARTING STEP NUMBER
	DEC	R0
	BLE	3$		;START WITH FIRST STEP
2$:	MOV	(R2),R2		;MOVE DOWN TO STARTING STEP
	BEQ	.+4		;CANT MOVE PAST END
	SOB	R0,2$
3$:	MOV	R2,@SUBPTR	;SAVE PTR TO FIRST STEP TO EXEC
	CLR	@#NSPEED	;NORMAL SPEED
	CLR	@#CONFIG	;NO SPECIAL CONFIGURATION
	CLR	@#MODES		;NO PARTICULAR SERVO MODES
	CLR	@#PMODES


EXECST:	CLR	R0		;READ CURRENT JT. ANGLES+HAND OPENING
	MOV	#7,R1		;SEVEN CHANNELS IN ALL
	MOV	#DANGLE,R2	;SAVE IN HERE
	JSR	PC,ANGLES
	BCS	EXECER		;BRANCH IF ADC DEAD
	CLR	R0		;INIT. DACS TO THE CURRENT POSITION
	MOV	#7,R1
	JSR	PC,DACOUT
	CLR	@#BRAKES	;RESET ALL HARDWARE BITS
	MOV	@#SWAIT,@#WAITNG;SET START WAIT COUNT
	MTPS	UNLOCK		;LET THE CLOCK INTERRUPT
	MOV	#DANGLE,R0	;DETERMINE CURRENT ARM CONFIGURATION
	JSR	PC,FLAGS
	MOV	@SUBPTR,R4	;ADDR. OF STEP TO EXECUTE
	ADD	#2,@#SUBPTR
	BR	TSTSTP
TOTOP:	MOV	@#SUBSTK,R4	;RESTART AT TOP OF PROGRAM
	MOV	#SUBSTK,@#SUBPTR;RESET SUBR CALL STACK
	MOV	FSTSTP(R4),R3
GOSTEP:	MOV	(R3)+,R4	;NEXT STEP TO EXECUTE
	MOV	(R3)+,R1	;PTR TO MOTION FUNCTION
	BIT	#1,R1		;SKIP OVER LABEL
	BEQ	.+4
	MOV	(R3)+,R1
	JSR	PC,@FUNPTR(R1)	;EXECUTE MOTION FUNCTION
	TST	@#ARMS  	;ANY ERROR BITS SET?
	BNE	TELSTP		;YES
TSTSTP:	MOV	R4,R3		;END OF PASS?
	BNE	GOSTEP
	DEC	@#EXECNT	;LAST PASS?
	BGT	TOTOP 
	MOV	#FINI,R1	;SIGNAL ALL DONE
	CLR	@#ARMS
	BR	.+6

TELSTP:	MOV	#UHALT,R1	;PRINT ERROR MESSAGE

	SUB	#2,@#SUBPTR	;SAVE PTR TO NEXT STEP
	MOV	R4,@SUBPTR

EXECER:	MOV	#-1,R0		;SET ALL OF THE BRAKES
	JSR	PC,SETBRK
	MTPS	LOCK		;STOP THE CLOCK INTERRUPTS
	JMP	TYPERR		;TYPE ERROR AND RETURN


;END OF "EXEC"
;"PROCEED","SNGSTP" - COMMAND INSTRUCTIONS

;THESE COMMANDS ARE USED FOR CONTINUING THE EXECUTION OF AN ARM
;PROGRAM AFTER IT HAS BEEN TERMINATED BY EITHER A "PAUSE" COMMAND
;OR ANY ONE OF A NUMBER OF ERROR CONDITIONS.  ONLY TERMINATION
;CONDITIONS THAT LEAVE THE "CANPRO" BIT IN THE ARM STATUS WORD
;( "ARMS" ) ON PERMIT THESE FUNCTIONS TO OPERATE.  NO ARGUMENTS
;ARE REQUIRED BY THESE ROUTINES.

;REGISTERS USED:
;	ALL REGISTERS AVAILABLE FOR USE

PROCED:	CLR	R0		;ARMS←0 IF CAN PROCEED
	MOV	#CNTPRO,R1	;ERROR MESSAGE OTHERWISE
	BR	TRYGO

SNGSTP:	MOV	#CANPRO,R0	;ARMS←CANPRO IF CAN PROCEED
	MOV	#CNTSGS,R1

TRYGO:	BIT	#CANPRO,ARMS	;CHECK IF PROCEEDING PERMITTED
	BEQ	NOPROC		;BRANCH IF NOT OK
	MOV	R0,@#ARMS
	JMP	EXECST		;GO CONTINUE EXECUTION

NOPROC:	JMP	TYPERR		;ELSE TYPE ERROR MESSAGE


;END OF "PROCED","SNGSTP"