perm filename FUNCTS.PAL[V,VDS]1 blob
sn#264829 filedate 1977-02-15 generic text, type C, neo UTF8
COMMENT ā VALID 00013 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 "EXEC" - COMMAND INSTRUCTION
C00029 00013 "PROCEED"- COMMAND INSTRUCTION
C00031 ENDMK
Cā;
.TITLE FUNCTS
;START OF TOP LEVEL ARM PROGRAM
START: RESET
MOV #STKTOP,SP ;INITIALIZE STACK
MTPS LOCK ;INITIALIZE PROCESSOR STATUS
MOV #CLKSER,@#CLKTRP ;SET UP CLOCK VECTOR
MOV #LOCK,@#CLKTRP+2
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 #COMMND,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 #20,SP ;CLEAR ARGUMENT LIST OFF STACK
BR MAINL
DONE: MOV #GOODBY,R1 ;ALL DONE, STOP MONITOR
JSR PC,TYPERR
ADD #22,SP ;LEAVE STACK CLEAR
MOV #2,@#CLKTRP+2 ;CLEAR CLOCK INTERRUPT VECTOR
MOV #CLKTRP+2,@#CLKTRP
HALT
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: MOV #1,R0 ;READ CURRENT JOINT ANGLES
MOV #6,R1 ;SIX JOINTS IN ALL
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: MOV #1,R0 ;READ JT. ANGLES/HAND OPENING
MOV #7,R1 ;SEVEN CHANNELS IN ALL
JSR PC,ANGLES ;PUT INTO "JANGLES"
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 REQUIRED.
;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
CMPB #15,(SG) ;THIS SHOULD BE A CR
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
JSR PC,LINOUT
RTS PC
SURE: .ASCIZ /ARE YOU SURE (Y,N)? /
CLRFIN: .ASCII /ARM PROGRAM RE-INITIALIZED,/
.ASCIZ / 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
ISPNHT: 0 ;0 IF "LISTT", -1 IF "PUNCHT"
;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 #LSTPM2,SG ;TYPE PROGRAM NAME
JSR PC,PACNME
MOV #LSTPMS,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 #30000.,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
ISPNHP: 0 ;0 IF "LISTP", -1 IF "PUNCHP"
LSTPMS: .ASCII /DEFPRO /
LSTPM2: .ASCII / /
.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 #20040,@#OUTBUF ;2 SPACE CHARACTERS
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,PACNME
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"
;"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: MOV 2(SP),R2
BNE GOTPRG
MOV #NOPROG,R1 ;ERROR CODE IF NO PROGS EXECUTED YET
MOV @#EXPROG,R2 ;RE-TRY LAST PROGRAM
BEQ EXECER
GOTPRG: MOV R2,@#EXPROG
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 STRTOP ;START WITH FIRST STEP
MVESTR: MOV (R2),R2 ;MOVE DOWN TO STARTING STEP
BEQ .+4 ;CANT MOVE PAST END
SOB R0,MVESTR
STRTOP: MOV R2,@#NXTSTP ;SAVE PTR TO FIRST STEP TO EXEC
CLR @#NSPEED ;NORMAL SPEED
CLR @#CONFIG ;NO SPECIAL CONFIGURATION
CLR @#MODES ;NO PARTICULAR SERVO MODES
EXECST: MOV #1,R0 ;READ CURRENT JT. ANGLES/HAND OPENING
MOV #7,R1 ;SEVEN CHANNELS IN ALL
JSR PC,ANGLES
BCS EXECER ;BRANCH IF ADC DEAD
MOV #JANGLE,R0 ;DETERMINE CURRENT ARM CONFIGURATION
JSR PC,FLAGS
CLR @#ARMS ;CLEAR ANY ARM ERROR BITS
MOV @#NXTSTP,R1 ;ADDR. OF STEP TO EXECUTE
BR TSTSTP
TOTOP: MOV @#EXPROG,R1 ;RESTART AT TOP OF PROGRAM
MOV FSTSTP(R1),R1
GOSTEP: MOV 2(R1),R2 ;PTR TO MOTION FUNCTION
MOV (R1),R0 ;NEXT STEP TO EXECUTE
JSR PC,@FUNPTR(R2) ;EXECUTE MOTION FUNCTION
TST @#ARMS ;CHECK IF ANY ERROR BITS SET
BNE TELSTP ;BRANCH IF ERROR
MOV R0,R1 ;END OF PASS?
TSTSTP: BNE GOSTEP
DEC @#EXECNT ;LAST PASS?
BGT TOTOP
MOV #FINI,R1 ;SIGNAL ALL DONE
BR EXECER
TELSTP: MOV R0,@#NXTSTP ;SAVE PTR TO NEXT STEP
MOV @#EXPROG,R1 ;COMPUTE ERROR STEP NUMBER
MOV FSTSTP(R1),R1 ;START COUNTING FROM 1ST STEP
MOV R0,R2
CLR R0 ;FORM STEP NUMBER IN HERE
NOTSTP: MOV (R1),R1 ;KEEP MOVING
INC R0
CMP R1,R2 ;FOUND STEP
BNE NOTSTP
MOV #UHALT,R1 ;PRINT ERROR MESSAGE
EXECER: JSR PC,TYPERR ;TYPE ERROR MESSAGE
EXECDN: RTS PC
;END OF "EXEC"
;"PROCEED"- COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR CONTINUING THE EXECUTION OF AN ARM MOTION
;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 THIS FUNCTION TO OPERATE. NO ARGUMENTS ARE
;REQUIRED BY THIS ROUTINE
;REGISTERS USED:
; ALL REGISTERS AVAILABLE FOR USE
PROCED: BIT #CANPRO,ARMS ;CHECK IF PROCEEDING PERMITTED
BEQ .+6 ;SKIP IF NOT OK
JMP EXECST ;GO CONTINUE EXECUTION
MOV #CNTPRO,R1 ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPERR
RTS PC ;EXIT
;END OF "PROCED"