perm filename ARM.PAL[2,VDS]2 blob
sn#252097 filedate 1976-12-02 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00050 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00006 00002 .TITLE ARM
C00011 00003 REQUIRED FILES AND MACRO DEFINITIONS
C00012 00004 START OF ARM PROGRAM
C00015 00005 "VECT" - COMMAND INSTRUCTION
C00019 00006 "TRANS" - COMMAND INSTRUCTION
C00021 00007 "RELTRN" - COMMAND INSTRUCTION
C00024 00008 "HERE" - COMMAND INSTRUCTION
C00026 00009 "WHERE" - COMMAND INSTRUCTION
C00029 00010 "LIST_" - COMMAND INSTRUCTIONS
C00033 00011 "EXEC" - COMMAND INSTRUCTION
C00042 00012 "PROCEED"- COMMAND INSTRUCTION
C00044 00013 "CLEAR" - COMMAND INSTRUCTION
C00047 00014 "EDIT" - COMMAND INSTRUCTION
C00052 00015 "DELETE" - EDIT INSTRUCTION
C00056 00016 "INSERT" - EDIT INSTRUCTION
C00060 00017 "FIND" - EDIT INSTRUCTION
C00064 00018 MISC - EDIT INSTRUCTIONS
C00067 00019 MOTION INSTRUCTION SUBROUTINE CALLING SEQUENCES
C00072 00020 "DRAW" - MOTION INSTRUCTION
C00082 00021 "MOVE" - MOTION INSTRUCTION
C00091 00022 "OPEN" - MOTION INSTRUCTION
C00095 00023 "GRASP" - MOTION INSTRUCTION
C00100 00024 "PAUSE" - MOTION INSTRUCTION
C00103 00025 "STOP" - MOTION INSTRUCTION
C00106 00026 "GO" - MOTION INSTRUCTION
C00109 00027 "DRIVE" - MOTION INSTRUCTION
C00118 00028 "LIST" - SUBR. USED TO PRINT A LIST OF ITEMS
C00121 00029 "DECODE" - SUBR. TO DECODE STRING INPUT INTO INTERNAL MOTION INSTR. FORMAT
C00124 00030 "DRIVE" - SUBR. TO GENERATE AND CONTROL JOINT MOTIONS
C00129 00031 [CONT. OF "DRIVE"]
C00134 00032 "PVECT" - SUBR. TO PRINT VECTOR OUT ON TTY
C00136 00033 "PTRANS" - SUBR. TO PRINT TRANSFORM OUT ON TTY
C00139 00034 "PSTEP" - SUBR. TO PRINT MOTION INSTRUCTION OUT ON TTY
C00141 00035 "LINNUM" - SUBR. TO CONVERT ADDR. OF MOTION INSTRUCTION TO ASC LINE #
C00143 00036 "GETVEC" - SUBR. TO SELECT FIRST FREE VECTOR
C00145 00037 "GETTRN" - SUBR. TO DECODE TRANSFORM NUMBER/SELECT FREE TRANSFORM
C00149 00038 "SAVSTG" - SUBR. TO SAVE STRING MESSAGES IN STRING ARRAY
C00153 00039 "MODTRN" - SUBR. TO PERMIT MODIFICATION OF EXISTING TRANSFORMS
C00156 00040 "TIMES" - SUBR. TO MULTIPLY TO TRANSFORMS TOGETHER
C00160 00041 "ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING
C00164 00042 "DACOUT" - SUBR. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES
C00167 00043 "RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS
C00170 00044 COMMAND HASH TABLE, CONTAINS FUNCTIONS NUMBERS OR 377 IF NOT USED
C00172 00045 MOTION INSTRUCTION HASH TABLE, CONTAINS FUNCTION NUMBERS OR 377 IF NOT USED
C00174 00046 EDIT FUNCTION HASH TABLE, CONTAINS FUNCTIONS NUMBERS OR 377 IF NOT USED
C00176 00047 PROGRAM,TRANSFORM, AND VECTOR STORAGE AREA ( MUST BE SAVED )
C00178 00048 VARIABLE STORAGE AREA ( MUST BE SAVED )
C00180 00049 PROGRAM CONSTANTS AND TEMPORARY STORAGE AREA ( NEED NOT BE SAVED )
C00182 00050 JOINT CALIBATION DATA ( NEED NOT BE SAVED )
C00186 ENDMK
C⊗;
.TITLE ARM
;ABSOLUTE CORE LOCATIONS
.=1000 ;STARTING ADDRESS OF PROGRAMS
ERRTRP ==4 ;TIME OUT AND ERROR TRAP
ILGINS ==10 ;ILLEGAL INSTRUCTION
CLKTRP ==104 ;CLOCK TRAP
DRATRP ==530 ;DR11 A VECTOR
DRBTRP ==534 ;DR11 B VECTOR
DEBUG =130000 ;RESTART OF DEBUGGING PROGRAM
DR11S =167770 ;DR11 STATUS WORD
DR11O =167772 ;DR11 OUTPUT REGISTER
DR11I =167774 ;DR11 INPUT REGISTER
CLKCNT =172544 ;CLOCK COUNTER
CLKSET =172542 ;CLOCK SET REGISTER
CLKS =172540 ;CLOCK STATUS
KBIS =177560 ;KEYBOARD INPUT STATUS
KBIR =177562 ;KEYBOARD INPUT REGISTER
KBOS =177564 ;KEYBOARD OUTPUT STATUS
KBOR =177566 ;KEYBOARD OUTPUT REGISTER
PS =177776 ;PROCESSOR STATUS WORD
;REGISTER DEFINITIONS
PC=%7 ;PROGRAM COUNTER
SP=%6 ;STACK POINTER
R5=%5 ;SAVED ACROSS PROCEDURE CALLS
SG=%5 ;STRING POINTER
R4=%4 ;SAVED ACROSS PROCEDURE CALLS
R3=%3 ;SAVED ACROSS PROCEDURE CALLS
R2=%2 ;SAVED ACROSS PROCEDURE CALLS
R1=%1 ;TEMP
R0=%0 ;TEMP
AC5==%5 ;TEMP FLOATING POINT REGISTERS
AC4==%4 ; " " " "
AC3==%3 ; " " " "
AC2==%2 ; " " " "
AC1==%1 ; " " " "
AC0==%0 ; " " " "
;INTERRUPT VECTOR STATUS WORDS
JTINTS ==340 ;CPU STATUS OF ARM INTERFACE INTERRUPT ROUTINE, LEVEL 7
CKINTS ==140 ;CPU STATUS OF CLOCK INTERRUPT ROUTINE, LEVEL 3
;DR11C STATUS REGISTER BIT DEFINITIONS
DACMDE ==0 ;CSR MODE BITS, DIGITAL TO ANALOG CONVERTER MODE
BRKMDE ==1 ; " " " , BRAKE REGISTER MODE
ADCMDE ==2 ; " " " , ANALOG TO DIGITAL CONVERTER MODE
STTMDE ==3 ; " " " , STATUS REGISTER READ MODE
ADCENB ==100 ;ADC INTERRUPT ENABLE BIT
ADCDNE ==200 ;ADC DONE BIT
STTENB ==40 ;STATUS REGISTER INTERRUPT ENABLE BIT
STTREQ ==100000 ;STATUS REGISTER REQUEST INTERRUPT BIT
;DR11C ARM STATUS REGISTER BIT DEFINITIONS
PANICB =200 ;PANIC BUTTON BIT MASK
ISON =100 ;INTERFACE ENABLED
TOUCH1 =1 ;TOUCH SENSOR BIT MASKS
TOUCH2 =2
ARMDNE =37400 ;BIT MASK OF ARM JOINTS IN RANGE BITS
HNDDNE =40000 ;BIT MASK OF HAND JOINT IN RANGE BIT
;DR11C BRAKE REGISTER BIT ASSIGNMENTS
ARMBRK ==77 ;ARM BRAKE BITS
HNDBRK ==100 ;HAND BRAKE BIT
NOINTG ==200 ;NO HARDWARE INTEGRATION
ENABLE ==400 ;INTERFACE ENABLE BIT
;DR11C DAC MODE BIT DEFINITIONS
CURRNT ==10000 ;CURRENT RATHER THAN POSITION MODE
;STORAGE AREA LENGTHS
MAXVEC==64. ;NUMBER OF VECTORS STORED, 0.5 K
MAXTRN==64. ;NUMBER OF TRANSFORMS STORED, 2 K
MAXSTP==256. ;NUMBER OF PROGRAM STEPS PERMITTED, 1 K
MAXSTG==10. ;NUMBER OF MESSAGE STRINGS STORED, 0.3 K
;REQUIRED FILES AND MACRO DEFINITIONS;
.EVEN
.INSRT IO.PAL
.INSRT EULER.PAL
.INSRT ARITH.PAL
.INSRT ARMSOL.PAL
.EVEN
;Absolute address initialization
TEMP == . ;Save location counter for a bit.
. = 244 ;Floating exception
.WORD 246
RTI ;No action taken
. = TEMP ;Restore location counter
;START OF ARM PROGRAM
START: RESET
MOV #1000,SP ;INITIALIZE STACK
CLR PS ;INITIALIZE PROCESSOR STATUS
CLR CLKCNT ;CLEAR CLOCK REGISTERS
CLR CLKSET
CLR CLKS
CLR DR11S ;DISABLE ALL ARM INTERFACE INTERRUPTS
LDFPS FSTAT ;SET FLOATING POINT STATUS
MOV #CKINTR,CLKTRP ;SET UP CLOCK INTERRUPT ROUTINE
MOV #CKINTS,CLKTRP+2
MOV #7,R0 ;SET THE OUTPUT NUMBER CONVERSION FORMAT
MOV #2,R1
JSR PC,FORMAT
MOV #HELLO,SG ;TELL EVERYONE WHO WE ARE
JSR PC,TYPSTR
MAINL: MOV #QUERY,SG ;ASK FOR INSTRUCTION
JSR PC,TYPSTR
MOV #INBUF,SG ;READ IN A COMMAND INSTRUCTION
JSR PC,INSTR
MOV #INBUF,SG ;GO DECODE IT
MOV #CHASH,R0 ;NEED HASH TABLE OF FUNCTIONS
MOV #CNAMES,R1 ;NEED TABLE OF PTRS TO FUNCTION STG NAMES
JSR PC,IDENT
BCC GOTCOM ;BRANCH IF LEGAL COMMAND
TST R1 ;TEST IF EMPTY STRING
BEQ MAINL ;LOOP BACK IF EMPTY LINE
MOV #BADCOM,SG ;ELSE INDICATE ILLEGAL COMMAND
JSR PC,TYPSTR
BR MAINL
GOTCOM: ASL R0 ;CONVERT FUNCTION NUMBER INTO INDEX
JSR PC,@CFUNCT(R0) ;EXECUTE FUNCTION
BR MAINL
DONE: MOV #GOODBY,SG ;ALL DONE, STOP MONITOR
ADD #2,SP ;POP RETURN ADDR. OFF STACK
JSR PC,TYPSTR
JMP DEBUG
HELLO: .ASCII /ARM CONTROL PROGRAM, VERSION A, 17NOV75/
.BYTE 15,12,15,12,0
QUERY: .ASCIZ /*/
BADCOM: .ASCII /ILLEGAL COMMAND, TRY AGAIN!/
.BYTE 15,12,0
GOODBY: .ASCII /EXITING TO DEBUGGER!/
.BYTE 15,12,0
.EVEN
;"VECT" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR SPECIFYING A VECTOR CONTAINING 3 ELEMENTS.
;SG MUST BE A POINTER TO A STRING WHICH CONTAINS THE VECTOR NUMBER.
;IF THE STRING DOES NOT SPECIFY A VECTOR NUMBER, THE FIRST UNASSIGNED
;VECTOR WILL BE SELECTED. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; JSR PC,VECT
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
VECT: JSR PC,INTSCN ;PICK UP VECTOR NUMBER SPECIFIED
BCC VECT1 ;BRANCH IF VECTOR NUMBER SPECIFIED
JSR PC,GETVEC ;ELSE, GO SELECT FIRST FREE VECTOR
BCS VCTDNE ;NO FREE VECTORS, RETURN TO COMMAND LEVEL
BR VECT4
VECT1: TST R0 ;CHECK FOR VECT NUM>0
BGE VECT3
VECT2: MOV #VECERR,SG ;OUT OF RANGE, TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR VCTDNE ;EXIT THIS ROUTINE
VECT3: CMP #MAXVEC,R0 ;CHECK FOR VECT NUM TOO LARGE
BLE VECT2
ASH #4,R0 ;CONVERT TO ABSOLUTE ADDRESS
ADD #VECLST,R0
VECT4: MOV #40200,14(R0) ;ASSIGN VECTOR
CLR 16(R0)
MOV R0,LSTVEC
MOV #HVECT,SG ;TYPE OUT COLUMN HEADER
JSR PC,TYPSTR
VECT5: JSR PC,PVECT ;TYPE OUT THIS VECTOR
CMP #VEC4,R0 ;CHECK IF LOOKING AT RESERVED VECTORS
BGT VCTDNE ;NO CHANGES IF RESERVED VECTORS
MOV #VECC1,SG ;ELSE ASK FOR CHANGES
JSR PC,TYPSTR
MOV #INBUF,SG
JSR PC,INSTR
MOV #INBUF,SG ;POINT TO START OF INPUT STRING
MOV R0,R1 ;STORE NEW VECTOR ELEMENT IN VECLST
MOV #3,R2 ;GET LOOP COUNTER
MOV #1,R3 ;KEEP TRACK OF ANY CHANGES
VECT6: JSR PC,RELSCN ;CONVERT FROM ASCII TO F.P.
BCS NOCHNG ;BRANCH IF NO CHANGE TO BE MADE
STF AC0,(R1) ;CHANGE VECTOR ELEMENT
CLR R3 ;INDICATE CHANGE TO AT LEAST ONE NUM
NOCHNG: ADD #4,R1 ;POINT TO NEXT VECTOR ELEMENT
JSR PC,CLRCMA ;SKIP OVER COMMA
SOB R2,VECT6 ;REPEAT FOR ALL NUMBERS
SOB R3,VECT5 ;REPEAT IF CORRECTIONS MADE
MOV R0,R1 ;CHECK IF VECTOR ALL ZEROS
MOV #6,R2 ;TEST 3 FLOATING POINT NUMBERS
VECT7: TST (R1)+
BNE VCTDNE ;LEAVE VECTOR DEFINED IF ANY ELEMENT ≠0
SOB R2,VECT7
CLR (R1)+ ;RELEASE VECTOR CELL IF ALL ELEMENTS =0
CLR (R1)+
SUB #20,LSTVEC
MOV #VECC2,SG ;INFORM OPERATOR OF THIS FACT
JSR PC,TYPSTR
VCTDNE: RTS PC
VECERR: .ASCII /VECTOR NUMBER SPECIFIED OUT OF RANGE/
.BYTE 15,12,0
VECC1: .ASCII /CHANGE?/
.BYTE 15,12,0
VECC2: .ASCII /VECTOR CELL RELEASED FROM USE/
.BYTE 15,12,0
.EVEN
;END OF "VECT"
;"TRANS" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR SETTING THE VALUES OF THE ELEMENTS OF A
;TRANFORM THAT REPRESENTS AN ARM POSITION AND ORIENTATION IN SPACE.
;SG MUST BE A POINTER TO A STRING WHICH CONTAINS THE TRANSFORM NUMBER.
;IF THE STRING DOES NOT SPECIFY A TRANS NUMBER, THE FIRST UNASSIGNED
;TRANS WILL BE SELECTED. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; JSR PC,TRANS
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
TRANS: JSR PC,GETTRN ;PICK UP TRANS NUMBER
BCS TRNDNE ;ERROR, RETURN TO COMMAND LEVEL
CMP #40200,74(R0) ;CHECK IF WE ARE ASSIGNING A NEW TRANSFORM
BEQ TRAN1 ;BRANCH IF OLD ONE
MOV R0,R1 ;ELSE INITIALIZE TRANS TO A REASONABLE POSITION
MOV #SMPTRN,R2
MOV #32.,R3 ;16 FLOATING POINT NUMBERS IN ALL
MOV (R2)+,(R1)+
SOB R3,.-2
TRAN1: JSR PC,MODTRN ;ALLOW EDITING OF TRANSFORM COEF.
TRNDNE: RTS PC
;END OF "TRANS"
;"RELTRN" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR RELEASING DEFINED TRANSFORMS FOR FUTURE
;USE. SG MUST BE A POINTER TO A STRING WHICH CONTAINS THE TRANS-
;FORM NUMBERS. THIS ROUTINE ACCEPTS AN ARBITRARY NUMBER OF ARGU-
;MENTS. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; JSR PC,RELTRN
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
RELTRN: MOV SG,R1 ;SAVE POINTER TO REMAINING ARGUMENTS
JSR PC,INTSCN ;PICK UP A TRANSFORM NUMBER
BCS RLTDNE ;EXIT IF NO MORE ARGUMENTS
ASH #6,R0 ;CONVERT TO ABSOLUTE ADDRESS
ADD #TRNLST,R0
CMP #TRN1,R0 ;CAN'T DELETE TRANSFORM 0
BLE RELT2
RELT1: MOV #RELC1,SG ;PRINT ERROR MESSAGE
JSR PC,TYPSTR
MOV R1,SG ;PRINT REMAINING TRANSFORMS NOT DELETED
JSR PC,TYPSTR
MOV #CRLFX,SG ;TYPE CR & LF
JSR PC,TYPSTR
BR RLTDNE ;EXIT
RELT2: CMP #TRNEND,R0 ;THIS IS THE LAST TRANSFORM
BLE RELT1 ;SIGNAL ERROR IF OUT OF RANGE
MOV #32.,R1 ;ELSE ZERO TRANSFORM CELL
CLR (R0)+
SOB R1,.-2
JSR PC,CLRCMA ;SKIP OVER COMMA IN INPUT STRING
BR RELTRN ;GO CHECK FOR MORE ARGUMENTS
RLTDNE: RTS PC
RELC1: .ASCII /INVALID TRANSFORM ARGUMENT, DELETING TERMINATED WITH:/
.BYTE 15,12
.ASCIZ / T = /
.EVEN
;END OF "RELTRN"
;"HERE" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR SETTING THE VALUES OF THE ELEMENTS OF A
;TRANFORM THAT REPRESENTS AN ARM POSITION AND ORIENTATION IN SPACE.
;SG MUST BE A POINTER TO A STRING WHICH CONTAINS THE TRANSFORM NUMBER.
;IF THE STRING DOES NOT SPECIFY A TRANS NUMBER, THE FIRST UNASSIGNED
;TRANS WILL BE SELECTED. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; JSR PC,TRANS
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
HERE: JSR PC,GETTRN ;GET THE TRANSFORM NUMBER
BCS HERDNE ;EXIT IF ERROR
MOV R0,-(SP) ;SAVE TRANS POINTER
MOV #1,R0 ;READ CURRENT JOINT ANGLES STARTING WITH JT 1
MOV #6,R1 ;SIX JOINTS IN ALL
JSR PC,ANGLES
MOV (SP)+,R0 ;CONVERT JT ANGLES TO TRANSFORM
BCC HERE1 ;BRANCH IF NO ADC ERROR
MOV #ADCMES,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR HERDNE ;EXIT
HERE1: MOV #JANGLE,R1 ;HERE ARE THE CURRENT ANGLES
JSR PC,UPDATE ;DO CONVERSION
JSR PC,MODTRN ;PERMIT EDITING OF TRANSFORM COEFFICIENTS
HERDNE: RTS PC
;END OF "HERE"
;"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, THIS ROUTINE
;UPDATES TRANSFORM 0 WITH THE CURRENT ARM TRANSFORM. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; JSR PC,TRANS
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; ALL REGISTERS ARE AVAILABLE FOR USE
WHERE: MOV #1,R0 ;READ CURRENT JOINT ANGLES AND HAND OPENING
MOV #7,R1 ;SEVEN CHANNELS IN ALL
JSR PC,ANGLES ;READ CURRENT JOINT ANGLES INTO ARRAY JANGLE
BCC WHER1 ;BRANCH IF NO ADC ERROR
MOV #ADCMES,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR WHEDNE ;EXIT
WHER1: MOV #TMPTRN,R0 ;PUT CURRENT TRANSFORM IN HERE
MOV #JANGLE,R1 ;GET JOINT ANGLES FROM HERE
JSR PC,UPDATE ;CONVERT JT. ANGLES TO TRANSFORM
MOV #HTRANS+4,SG ;TYPE OUT THE COLUMN HEADER TO PRINT NEW TRANS
JSR PC,TYPSTR
JSR PC,PTRANS ;TYPE OUT THIS TRANSFORM
MOV #WHERC1,SG ;TYPE OUT COLUMN HEADER TO PRINT JOINT ANGLES
JSR PC,TYPSTR
MOV #OUTBUF,SG ;CONSTRUCT STRING OF ASCII JOINT ANGLES
MOV #JANGLE,R0 ;HERE ARE THE ANGLES
MOV #7,R1 ;PRINT 6 ANGLES AND HAND OPENING
WHER2: LDF (R0)+,AC0 ;GET AN ANGLE
JSR PC,CVF ;CONVERT TO ASCII
MOVB #40,(SG)+ ;PUT IN A SPACE CHARACTER
SOB R1,WHER2
MOV #OUTBUF,SG ;PRINT THE ASC STRING
JSR PC,TYPSTR
JSR PC,CRLF
WHEDNE: RTS PC
WHERC1: .ASCII / JT 1 JT 2 JT 3 JT 4 JT 5 JT 6 HAND/
.BYTE 15,12,0
.EVEN
;END OF "WHERE"
;"LIST_" - COMMAND INSTRUCTIONS
;THE FOLLOWING COMMANDS ARE USED FOR TYPING OUT VECTORS, TRANS-
;FORMS, AND MOTION INSTRUCTION STEPS ON THE TELETYPE. FOR ALL
;OF THE COMMANDS EXCEPT "LISTA", SG MUST CONTAIN A POINTER TO
;A STRING THAT CONTAINS THE UPPER AND LOWER LIMITS TO BE TYPED
;OUT. IF ONLY ONE ARGUMENT IS TYPED IN, THE SINGLE ELEMENT
;SPECIFIED IS TYPED. NO ARGUMENT IS NECESSARY FOR "LISTA" SINCE
;THIS ROUTINE TYPES OUT ALL INITIALIZED VECTORS, TRANSFORMS AND
;INSTRUCTION STEPS. IN ALL FOUR COMMANDS, ANY VECTOR, TRANSFORM
;OR INSTRUCTION THAT IS NOT INITIALIZED IS NOT TYPED OUT. A
;SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; JSR PC,LIST<V,T,S,A>
;
;NOTE THAT "LISTA" DOES NOT REQUIRE SG INITIALIZED.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
;COMMAND TO PRINT VECTORS
LISTV: MOV #LTVLST,R3 ;SET POINTER TO TABLE OF CONSTANTS
JSR PC,LIST ;GO LIST VECTORS
RTS PC ;RETURN
LTVLST: MAXVEC-1 ;MAXIMUM VECTOR NUMBER PERMITTED
0 ;FIRST PERMITTED VECTOR
20 ;LENGTH OF EACH VECTOR
VECLST ;STARTING ADDRESS OF VECTOR LIST
HVECT ;COLUMN HEADING
PVECT ;PRINT ROUTINE
;COMMAND TO PRINT TRANSFORMS
LISTT: MOV #LTTLST,R3 ;SET POINTER TO TABLE OF CONSTANTS
JSR PC,LIST ;GO LIST
RTS PC ;RETURN
LTTLST: MAXTRN-1 ;MAXIMUM TRANSFORM NUMBER PERMITTED
0 ;FIRST PERMITTED TRANSFORM
100 ;LENGTH OF EACH TRANSFORM
TRNLST ;STARTING ADDRESS OF TRANSFORM LIST
HTRANS ;COLUMN HEADING
PTRANS ;PRINT ROUTINE
;COMMAND TO PRINT MOTION INSTRUCTION STEPS
LISTS: MOV #LTSLST,R3 ;SET POINTER TO TABLE OF CONSTANTS
JSR PC,LIST ;GO LIST
RTS PC ;RETURN
LTSLST: MAXSTP ;MAXIMUM INSTRUCTION NUMBER PERMITTED
1 ;FIRST PERMITTED INSTRUCTION NUMBER
10 ;LENGTH OF EACH INSTRUCTION
STPLST ;STARTING ADDRESS OF INSTRUCTION SPACE
HSTEP ;COLUMN HEADING
PSTEP ;PRINT ROUTINE
;COMMAND TO PRINT EVERYTHING
LISTA: MOV #CRLFX,SG ;ASK FOR ALL INSTRUCTIONS
JSR PC,LISTS
MOV #CRLFX,SG ;ASK FOR ALL TRANSFORMS
JSR PC,LISTT
MOV #CRLFX,SG ;ASK FOR ALL VECTORS
JSR PC,LISTV
RTS PC
;END OF "LISTV","LISTT","LISTS","LISTA"
;"EXEC" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR INITIATING ARM MOTION PROGRAMS. THE STRING
;POINTER REGISTER (SG) IS EXPECTED TO BE POINTING AT AN INPUT STRING
;THAT CONTAINS THREE ARGUMENTS. THE FIRST ARGUMENT SPECIFIES THE LINE
;NUMBER OF THE FIRST MOTION INSTRUCTION STEP TO BE EXECUTED. THE
;SECOND ARGUMENT, IF PRESENT, SPECIFIES THE LAST MOTION INSTRUCTION
;STEP TO BE EXECUTED. IF THE SECOND ARGUMENT IS NOT PRESENT, A "STOP"
;OR "PAUSE" MOTION INSTRUCTION IS ASSUMED TO TERMINATE THE ARM PROGRAM.
;THE THIRD ARGUMENT, IF PRESENT, SPECIFIES THE NUMBER OF TIMES THAT
;THE ARM PROGRAM IS TO BE REPEATED. IF THIS ARGUMENT IS OMITTED ONLY
;ONE PASS THROUGH THE CODE IS EXECUTED. IF NO ARGUMENTS ARE GIVEN THEN
;EXECUTION STARTS AT STEP 1 AND CONTINUES UNTIL THE FIRST "STOP" OR
;"PAUSE" IS ENCOUNTERED. THIS SEQUENCE IS ONLY EXECUTED ONCE.
;
;AS AN ALTERNATE METHOD OF SPECIFYING INSTRUCTION EXECUTION, A SINGLE
;MOTION INSTRUCTION CAN BE GIVEN AS AN ARGUMENT TO THIS ROUTINE. THE
;EXECUTION DOES NOT AFFECT THE CURRENTLY EXECUTING ARM PROGRAM OR ANY
;ARM ERROR CONDITIONS THAT MAY EXIST.
;
;IN EITHER CASE, THE CALLING SEQUENCE IS AS FOLLOWS:
;
; MOV #STRING,SG ;PTR TO INPUT DATA STRING
; JSR PC,EXEC
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
EXEC: MOV SG,R2 ;SAVE STRING POINTER
JSR PC,DECODE ;CHECK IF STRING DECODES INTO MOTION INSTRUCTION
BCS EXECSS ;BRANCH IF NOT MOTION INSTRUCTION
MOV #1,SNGINS ;INDICATE WE'VE GOT A SINGLE INSTRUCTION TO DO
MOV ARMSTT,-(SP) ;SAVE CURRENT STATE OF ARM
MOV NXTSTP,-(SP)
MOV R1,NXTSTP ;POINT TO MOTION INSTRUCTION
BR EXECST ;GO EXECUTE THIS ONE INSTRUCTION
EXECSS: MOV R2,SG ;MUST BE GIVEN IN LINE NUMBER FORM, GET STG AGAIN
MOV #1,R2 ;ASSUME FIRST STEP=1
CLR R3 ;ASSUME NO FINAL STEP GIVEN
MOV #1,R4 ;ASSUME NUMBER OF LOOPS=1
JSR PC,INTSCN ;PICK UP STARTING STEP NUMBER
BCS EXEC3 ;BRANCH IF NO ARGUMENTS GIVEN
TST R0 ;ELSE CHECK IF STEP NUMBER IN RANGE
BGT EXEC1 ;BRANCH IF GREATER THAN 0
EXEERR: MOV #EXECM1,SG ;ELSE TYPE OUT ERROR MESSAGE
JSR PC,TYPSTR
JMP EXEDNE ;EXIT ROUTINE
EXEC1: CMP #MAXSTP,R0 ;MUST BE LESS THAN MAXSTP
BLT EXEERR
MOV R0,R2 ;SAVE STARTING STEP NUMBER
JSR PC,CLRCMA ;PICK UP SECOND ARGUMENT
JSR PC,INTSCN
BCS EXEC3 ;BRANCH IF NO OTHER ARGUMENTS SPECIFIED
CMP R0,R2 ;ELSE CHECK THAT FINAL STEP≥INITIAL STEP
BLT EXEERR ;SIGNAL ERROR IF NOT TRUE
CMP #MAXSTP,R0 ;CHECK THAT FINAL STEP ≤ LAST POSSIBLE STEP
BLT EXEERR
MOV R0,R3 ;SAVE FINAL STEP NUMBER
JSR PC,CLRCMA ;PICK UP THIRD INPUT ARGUMENT
JSR PC,INTSCN
BCS EXEC3 ;BRANCH IF NOT SPECIFIED
TST R0 ;LOOP COUNT MUST BE GREATER THAN 0
BGT EXEC2 ;BRANCH IF OK
MOV #EXECM2,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR EXEDNE ;EXIT ROUTINE
EXEC2: MOV R0,R4 ;SAVE LOOP COUNT
EXEC3: DEC R2 ;CONVERT STEP NUMBERS TO ABS. ADDRESSES
ASH #3,R2
ADD #STPLST,R2
DEC R3
ASH #3,R3
ADD #STPLST,R3
MOV R2,INISTP ;SAVE INITIAL STEP ADDRESS
MOV R3,FINSTP ;SAVE FINAL STEP ADDRESS
MOV R4,LPSTP ;SAVE STEP LOOP COUNT
MOV R2,NXTSTP ;SET NEXT STEP TO EXEC = INITIAL STEP
EXECST: MOV #1,R0 ;READ CURRENT JOINT ANGLES AND HAND OPENING
MOV #7,R1 ;SEVEN CHANNELS IN ALL
JSR PC,ANGLES
BCC EXECLP ;BRANCH IF A/D READ OK
MOV #ADCMES,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
TST SNGINS ;ELSE EXIT, CHECK IF DOING EXPLICIT MOTION INST
BNE EXESDN ;BRANCH IF SO
BR EXEDNE ;ELSE TAKE STANDARD EXIT PATH
EXECLP: CLR ARMSTT ;CLEAR ANY ARM ERROR BITS & PROCEED BIT
MOV NXTSTP,R0 ;PICK UP ADDR. OF STEP TO EXECUTE
ADD #10,NXTSTP ;INCREMENT STEP POINTER FOR NEXT TIME
MOVB (R0),R1 ;CHECK IF VALID MOTION INSTRUCTION
BNE EXEC4 ;BRANCH IF OK
MOV #EXECM3,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
JSR PC,LINNUM ;CONVERT LINE NUMBER TO ASCII STRING
MOV #OUTBUF,SG ;TYPE OUT LINE NUMBER
JSR PC,TYPSTR
JSR PC,CRLF
BR EXEDNE ;EXIT
EXEC4: DEC R1 ;CONVERT MOTION FUNCTION NUMBER TO WORD INDEX
ASL R1
MOV R0,-(SP)
JSR PC,@IFUNCT(R1) ;GO EXECUTE MOTION FUNCTION
MOV (SP)+,R0
TST SNGINS ;CHECK IF ONLY DOING ONE EXPLICIT MOTION INST
BNE EXESDN ;BRANCH IF SO
CMP R0,FINSTP ;CHECK IF END OF EXECUTION LOOP
BNE EXEC5 ;BRANCH IF NOT END
MOV INISTP,NXTSTP ;IF END, STEP NEXT STEP= INITIAL STEP
DEC LPSTP ;DECREMENT LOOP COUNT
BGT EXEC5 ;BRANCH IF NOT FINAL LOOP
MOV #EXECM5,SG ;ELSE TYPE DONE MESSAGE
JSR PC,TYPSTR
CLR ARMSTT ;INDICATE CAN'T PROCEED FROM THIS STEP
BR EXEDNE ;EXIT
EXEC5: TST ARMSTT ;CHECK IF ANY ERROR BITS SET
BEQ EXECLP ;GO TO NEXT MOTION INSTRUCTION IF NO ERRORS
MOV #EXECM4,SG ;ELSE TYPE EXECUTION STOP MESSAGE
JSR PC,TYPSTR
JSR PC,LINNUM ;CONVERT LINE NUMBER TO ASCII STRING
MOV #OUTBUF,SG ;TYPE OUT LINE NUMBER
JSR PC,TYPSTR
JSR PC,CRLF
EXEDNE: RTS PC ;EXIT
EXESDN: CLR SNGINS ;INDICATE END OF SINGLE INSTRUCTION MODE
BIT #400,(R0) ;CHECK IF INSTRUCTION USED A MESSAGE STRING
BEQ .+6 ;SKIP IF IT DIDN'T
CLRB @2(R0) ;ELSE RELEASE STRING FROM USE
MOV (SP)+,NXTSTP ;RESTORE STATE OF ARM
MOV (SP)+,ARMSTT
RTS PC ;EXIT
;LOCAL STORAGE AREA
SNGINS: 0
EXECM1: .ASCII /INVALID STEP NUMBER SPECIFIED/
.BYTE 15,12,0
EXECM2: .ASCII /INVALID LOOP COUNT SPECIFIED/
.BYTE 15,12,0
EXECM3: .ASCIZ /**ERROR**, NO MOTION INSTRUCTION FOUND AT LINE /
EXECM4: .ASCIZ /EXECUTION TERMINATED AT LINE /
EXECM5: .ASCII /MOTION PROGRAM EXECUTION COMPLETED/
.BYTE 15,12,0
.EVEN
;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
;( "ARMSTT" ) ON PERMIT THIS FUNCTION TO OPERATE. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; JSR PC,PROCED
;
;THERE ARE NO REQUIRED ARGUMENTS FOR THIS ROUTINE. ALSO, THIS ROUTINE
;NEVER RETURNS AN ERROR CONDITION.
;REGISTERS USED:
;
; ALL REGISTERS AVAILABLE FOR USE
PROCED: BIT #CANPRO,ARMSTT ;CHECK IF PROCEEDING PERMITTED
BEQ .+6 ;SKIP IF NOT OK
JMP EXECST ;GO CONTINUE EXECUTION
MOV #PROCM1,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
RTS PC ;EXIT
PROCM1: .ASCII /CAN'T PROCEED FROM THIS POINT, USE "EXEC" INSTRUCTION/
.BYTE 15,12,0
.EVEN
;END OF "PROCED"
;"CLEAR" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR RESETING ALL OF THE VECTORS, TRANSFORMS,
;AND MOTIONS INSTRUCTIONS TO ZERO. THIS IS A COMPLETELY DESTRUCTIVE
;OPERATION. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; JSR PC,CLEAR
;
;NO ARGUMENTS MUST BE PASSED TO THIS ROUTINE
;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,INBUF ;CHECK IF RESPONSE IS "Y"
BNE CLEDNE
CMPB #15,INBUF+1 ;THIS SHOULD BE A CR
BNE CLEDNE
MOV #TRN1,R0 ;OK, CLEAR TRANSFORM AREA
MOV #TRNEND,R1 ;THIS IS JUST PAST THE END OF TRANS AREA
JSR PC,CLRSUB
MOV #VEC4,R0 ;REPEAT FOR VECTOR STORAGE SPACE
MOV #VECEND,R1
JSR PC,CLRSUB
MOV #STPLST,R0 ;REPEAT FOR INSTRUCTION SPACE
MOV #STPEND,R1
JSR PC,CLRSUB
MOV #STGLST,R0 ;REPEAT FOR STRING SPACE
MOV #STGEND,R1
JSR PC,CLRSUB
MOV #TRNLST,LSTTRN ;RESET POINTERS TO START OF ARRAYS
MOV #STPLST,LSTSTP
MOV #VEC3,LSTVEC
CLR ARMSTT ;RESET ARM STATUS WORD
MOV #CLRFIN,SG ;TELL EVERYONE IT'S DONE
JSR PC,TYPSTR
CLEDNE: RTS PC
CLRSUB: SUB R0,R1 ;GET NUMBER OF WORDS TO CLEAR
ASR R1
CLR (R0)+ ;CLEAR AREA
SOB R1,.-2
RTS PC
SURE: .ASCIZ /ARE YOU SURE (Y,N)? /
CLRFIN: .ASCII /VECTOR, TRANSFORM, AND INSTRUCTION ARRAYS ZEROED/
.BYTE 15,12,0
.EVEN
;END OF "CLEAR"
;"EDIT" - COMMAND INSTRUCTION
;THIS COMMAND IS USED FOR MODIFYING EXISTING MOTION INSTRUCTION
;STEPS. SG MUST BE A POINTER TO A STRING WHICH CONTAINS THE STEP
;NUMBER OF THE FIRST INSTRUCTION TO BE EDITED. IF NO STEP IS GIVEN
;THE LAST STEP EDITED IS AGAIN SELECTED. A CALLING SEQUENCE
;FOLLOWS:
;
; MOV #STRING,SG
; JSR PC,EDIT
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG PASSES ARGUMENTS
; ALL REGISTERS ARE AVAILABLE FOR USE
EDIT: JSR PC,INTSCN ;PICK UP STEP NUMBER
BCC EDIT1 ;BRANCH IF NUMBER SPECIFIED
MOV LSTSTP,R0 ;ELSE, RETURN TO LAST STEP ADDRESSED
BR EDIT4
EDIT1: TST R0 ;STEP NUMBERS START WITH 1
BGT EDIT3
EDIT2: MOV #EDITC1,SG ;OUT OF RANGE, TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR EDTDNE ;EXIT THIS ROUTINE
EDIT3: CMP #MAXSTP,R0 ;CHECK FOR STEP NUMBER TOO LARGE
BLT EDIT2
DEC R0 ;CONVERT TO ABSOLUTE ADDRESS
ASH #3,R0
ADD #STPLST,R0
EDIT4: JSR PC,PSTEP ;PRINT THE CURRENT MOTION INSTRUCTION
MOV #OUTBUF,SG ;STEP NUMBER STILL IN OUTBUF
MOVB #77,OUTBUF+4 ;PUT IN A QUESTION MARK
CLRB OUTBUF+5 ;END STRING
JSR PC,TYPSTR ;TYPE STEP NUMBER ?
MOV #INBUF,SG ;READ IN A RESPONSE
JSR PC,INSTR
MOV #INBUF,SG
MOV R0,-(SP) ;SAVE STEP POINTER
MOV #EHASH,R0 ;CHECK IF THIS IS A EDIT INSTRUCTION
MOV #ENAMES,R1
JSR PC,IDENT ;GET EDIT FUNCTION NUMBER
BCS EDIT5 ;BRANCH IF NOT EDIT FUNCTION
ASL R0 ;ELSE CONVERT FUNCTION NUMBER TO WORD INDEX
MOV R0,R1
MOV (SP)+,R0 ;RESTORE STEP POINTER
JSR PC,@EFUNCT(R1) ;GO EXECUTE EDIT FUNCTION
BCS EDTDNE ;EXIT IF ERROR RETURNED
BR EDIT8 ;ELSE SAVE STEP NUMBER AND REPEAT CYCLE
EDIT5: MOV (SP)+,R0
TST R1 ;CHECK IF JUST C/R TYPED
BEQ EDIT7 ;GO TO NEXT LINE IF C/R
MOV #INBUF,SG ;ELSE TRY TO DECODE INTO MOTION INSTRUCTION
JSR PC,DECODE
BCC EDIT6 ;BRANCH IF FOUND MOTION INSTRUCTION
MOV #EDITC3,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR EDIT8 ;GO REPEAT SAME STEP NUMBER
EDIT6: BIT #400,(R0) ;CHECK IF OLD MOTION INSTRUCTION USES STRINGS
BEQ .+6 ;SKIP IF DIDN'T
CLRB @2(R0) ;ELSE RELEASE STRING CELL
MOV (R1)+,(R0) ;REPLACE OLD MOTION INSTRUCTION WITH NEW
MOV (R1)+,2(R0)
MOV (R1)+,4(R0)
MOV (R1)+,6(R0)
EDIT7: ADD #10,R0 ;GO TO NEXT LINE
CMP #STPEND,R0 ;CHECK IF AT END OF INSTRUCTION STEP AREA
BGT EDIT8
MOV #EDITC2,SG ;TELL OPERATOR AT END OF INSTRUCTION AREA
JSR PC,TYPSTR
BR EDTDNE
EDIT8: MOV R0,LSTSTP ;UPDATE POINTER TO LAST STEP MODIFIED
BR EDIT4
EDTDNE: RTS PC
EDITC1: .ASCII /STEP NUMBER OUT OF RANGE/
.BYTE 15,12,0
EDITC2: .ASCII /END OF MOTION INSTRUCTION STORAGE SPACE/
.BYTE 15,12,0
EDITC3: .ASCII /INPUT ERROR, PLEASE TYPE LINE AGAIN/
.BYTE 15,12,0
.EVEN
;END OF "EDIT"
;"DELETE" - EDIT INSTRUCTION
;THIS EDIT FUNCTION DELETES AN ARBITRARY NUMBER OF MOTION INSTRUCTIONS
;STARTING WITH THE CURRENT LINE. SG MUST BE A POINTER TO A STRING WHICH
;CONTAINS THE NUMBER OF LINES TO BE DELETED. IF NO NUMBER IS FOUND,
;ONLY THE CURRENT LINE IS DELETED. SPECIFYING A NEGATIVE NUMBER OF LINES
;DELETES THE CURRENT LINE AND PREVIOUS LINES ( LINES WITH SMALLER STEP
;NUMBERS ). SPECIFYING A POSITIVE NUMBER OF LINES HAS THE OPPOSITE AFFECT.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; MOV STEP,R0
; JSR PC,DELETE
;
;THIS EDIT ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; SG,R0 PASS ARGUMENTS AND ARE ALTERED
; R1,AC0,AC1 GARBAGED
DELETE: MOV R2,-(SP)
MOV R0,-(SP) ;SAVE STEP POINTER
JSR PC,INTSCN ;GET NUMBER OF LINES TO DELETE
BCC .+6
MOV #1,R0 ;ONLY DELETE CURRENT LINE IF NO NUMBER GIVEN
ASH #3,R0 ;GET NUMBER OF BYTES TO DELETE
MOV R0,R1 ;CHECK IF - OR + NUMBER SPECIFIED
BEQ DELDNE ;EXIT IF NO LINES TO DO
BPL DELE2 ;BRANCH IF + NUMBER OF LINES SPECIFIED
NEG R1 ;GET ABS(NUMBER OF LINES TO DELETE)
ADD #10,R0 ;START WITH CURRENT LINE
ADD (SP),R0 ;GET THE ABS. ADDR OF FIRST INST TO DELETE
CMP #STPLST,R0 ;CHECK IF BEYOND START OF LIST
BLE DELE1
SUB #STPLST,R0 ;CORRECT FOR BEING BEYOND START OF LIST
ADD R0,R1
MOV #STPLST,R0
DELE1: MOV R0,(SP) ;SET NEW CURRENT LINE NUMBER
BR DELE3
DELE2: ADD (SP),R0 ;CHECK IF BEYOND END OF LIST
SUB #STPEND,R0
BLE .+4 ;BRANCH IF IN RANGE
SUB R0,R1 ;ELSE CORRECT DELETE BYTE COUNT
MOV (SP),R0 ;START DELETING FROM CURRENT LOCATION
DELE3: MOV R1,-(SP) ;SAVE THE NUMBER OF BYTES TO FILL WITH ZEROS
ASH #-3,R1 ;COMPUTE THE NUMBER OF INSTRUCTIONS TO DELETE
DELE4: BIT #400,(R0) ;CHECK IF ANY INSTR. TO DELETE USES STRINGS
BEQ .+6 ;SKIP IF IT DOESN'T
CLRB @2(R0) ;ELSE RELEASE STRING CELL
ADD #10,R0 ;POINT TO NEXT INSTRUCTION
SOB R1,DELE4
MOV (SP),R1 ;GET NUMBER OF BYTES TO DELETE
MOV 2(SP),R0 ;GET ADDR. OF FIRST WORD TO DELETE
ADD R0,R1 ;COMPUTE ADDR. OF FIRST WORD TO TRANSFER
MOV #STPEND,R2 ;COMPUTE THE NUMBER OF WORDS TO MOVE DOWN
SUB R1,R2
ASR R2
BEQ DELE5 ;BRANCH IF ONLY FILLING ZEROS AT END OF LIST
MOV (R1)+,(R0)+ ;TRANSFER WORDS
SOB R2,.-2
DELE5: MOV (SP)+,R1 ;GET THE NUMBER OF WORDS TO FILL WITH ZEROS
ASR R1
CLR (R0)+ ;FILL END OF LIST WITH ZEROS
SOB R1,.-2
DELDNE: MOV (SP)+,R0 ;SET NEW CURRENT ADDRESS
MOV (SP)+,R2
CLC
RTS PC
;END OF "DELETE"
;"INSERT" - EDIT INSTRUCTION
;THIS EDIT FUNCTIONS ALLOWS THE USER TO INSERT NEW MOTION INSTRUCTIONS
;IN BETWEEN EXISTING INSTRUCTIONS. R0 MUST BE A POINTER TO THE CURRENT
;MOTION INSTRUCTION. THE INSERTED LINE IS PLACED BEFORE THE CURRENT
;LINE. THIS MODE IS TERMINATED BY TYPING ANY ILLEGAL MOTION INSTRUCTION
;( BUT A EMPTY LINE IS THE CLEANEST WAY TO EXIT ). A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV STEP,R0
; JSR PC,DELETE
;
;THIS EDIT FUNCTION NEVER RETURNS AN ERROR CONDITION.
;REGISTERS USED:
;
; R0 PASSES ARGUMENTS AND IS ALTERED
; R1,SG,AC0,AC1 GARBAGED
INSERT: MOV R2,-(SP)
MOV R3,-(SP)
INSR1: TST STPEND-10 ;CHECK IF LAST INSTRUCTION LINE EMPTY
BEQ INSR2
MOV #INSRC1,SG ;TYPE OUT ERROR MESSAGE IF LAST LINE NOT EMPTY
BR ISRERR
INSR2: JSR PC,LINNUM ;CONVERT STEP ADDR. TO ASCII LINE NUMBER
MOVB #77,-1(SG) ;PUT IN A QUESTION MARK
MOV #OUTBUF,SG ;TYPE OUT LINE NUMBER
JSR PC,TYPSTR
MOV #INBUF,SG ;READ IN A RESPONSE
JSR PC,INSTR
MOV #INBUF,SG
JSR PC,DECODE ;DECODE MOTION INSTRUCTION
BCC INSR3 ;SKIP IF MOTION DECODED PROPERLY
MOV #INSRC2,SG ;TELL OPERATOR END OF INSERT MODE
ISRERR: JSR PC,TYPSTR
BR ISRDNE ;EXIT
INSR3: MOV #STPEND,R3 ;ELSE, MOVE ALL INSTRUCTIONS DOWN ONE
MOV #STPEND-10,R2 ;START TRANSFER FROM THIS INSTRUCTION
SUB #STPEND-10,R0 ;COMPUTE THE NUMBER OF WORDS TO TRANSFER
NEG R0
BEQ INSR4 ;BRANCH IF THIS IS THE NEXT TO LAST LINE
ASR R0
MOV -(R2),-(R3) ;SHIFT LINES DOWN ONE
SOB R0,.-2
INSR4: MOV (R1)+,(R2)+ ;INSERT NEW INSTRUCTION JUST TYPED IN
MOV (R1)+,(R2)+
MOV (R1)+,(R2)+
MOV (R1)+,(R2)+
MOV R2,R0 ;SAVE PTR TO NEXT INSTRUCTION
CMP #STPEND,R0 ;CHECK IS THIS IS THE LAST INSTRUCTION LINE
BGT INSR1 ;GO READ IN ANOTHER INSTRUCTION IF MORE LINES
ISRDNE: MOV (SP)+,R3
MOV (SP)+,R2
CLC
RTS PC
INSRC1: .ASCII /CAN'T CONTINUE INSERTING WITHOUT LOSING LAST INSTRUCTION/
.BYTE 15,12,0
INSRC2: .ASCII /INSERT MODE TERMINATED/
.BYTE 15,12,0
.EVEN
;END OF "INSERT"
;"FIND" - EDIT INSTRUCTION
;THIS EDIT FUNCTION PERFORMS A LINEAR SEARCH FOR THE FIRST OCCURANCE
;OF A MOTION INSTRUCTION WHOSE FUNCTION NAME MATCHES THE REQUESTED NAME.
;SG MUST BE A POINTER TO A STRING WHICH CONTAINS THE REQUESTED MOTION
;FUNCTION NAME. IF NO NAME IS FOUND, THE NAME REQUESTED IN THE LAST
;SEARCH IS USED. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG
; MOV STEP,R0
; JSR PC,FIND
;
;THE SEARCH STARTS FROM MOTION INSTRUCTION #STEP
;REGISTERS USED:
;
; SG,R0 PASS ARGUMENTS AND ARE ALTERED
; R1,AC0,AC1 GARBAGED
FIND: MOV R0,-(SP) ;SAVE STEP POINTER
MOV #IHASH,R0 ;GET MOTION INSTRUCTION NUMBER
MOV #INAMES,R1
JSR PC,IDENT
BCC FIND2 ;BRANCH IF INSTR. NUMBER FOUND
TST R1 ;ELSE TEST IF ONLY C/R TYPED
BEQ FIND1 ;BRANCH IF SAME SEARCH REQUESTED
MOV #FINDC1,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR FIDDNE ;EXIT
FIND1: MOV FINST,R0 ;IF SAME SEARCH REQUESTED, GET OLD INST.#
BPL FIND3 ;BRANCH IF SET
MOV #FINDC2,SG ;OTHERWISE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR FIDDNE
FIND2: INC R0
MOV R0,FINST ;SAVE MOTION INST. NUMBER FOR NEXT TIME
FIND3: MOV (SP),R1 ;GET PRESENT STEP NUMBER
FIND4: ADD #10,R1 ;POINT TO NEXT MOTION INSTRUCTION
CMP #STPEND,R1 ;CHECK IF AT END OF AREA
BGT FIND5 ;BRANCH IF STILL SOME INSTRUCTIONS TO CHECK
MOV #FINDC3,SG ;ELSE OUTPUT ERROR MESSAGE
JSR PC,TYPSTR
DEC R0 ;CONVERT MOTION NUMBER INTO WORD INDEX
ASL R0
MOV INAMES(R0),SG ;GET POINTER TO NAME OF FUNCTION
JSR PC,TYPSTR
MOV #FINDC4,SG
JSR PC,TYPSTR
BR FIDDNE ;EXIT
FIND5: CMPB R0,(R1) ;COMPARE FUNCTION NUMBERS
BNE FIND4 ;REPEAT IF NOT FOUND
MOV R1,(SP) ;ELSE RETURN THIS STEP POINTER
FIDDNE: MOV (SP)+,R0 ;RESTORE STEP POINTER
CLC
RTS PC
FINST: -1
FINDC1: .ASCII /UNKNOWN INSTRUCTION REQUESTED IN SEARCH/
.BYTE 15,12,0
FINDC2: .ASCII /SEARCH ARGUMENT NEVER SPECIFIED/
.BYTE 15,12,0
FINDC3: .ASCIZ /INSTRUCTION "/
FINDC4: .ASCII /" NOT FOUND/
.BYTE 15,12,0
.EVEN
;END OF "FIND"
;MISC - EDIT INSTRUCTIONS
;THE FOLLOWING EDIT FUNCTIONS ALL REQUIRE A POINTER TO THE CURRENT
;MOTION STEP IN R0 AND SOME OF THE FUNCTIONS REQUIRE A STRING OF
;ARGUMENTS POINTED TO BY THE SG REGISTER. A SAMPLE CALLING SEQUENCE
;FOLLOWS:
;
; MOV #STRING,SG
; MOV #STEP,R0
; JSR PC,FIND
;
;IF THE EDIT MODE IS TO BE TERMINATED, THE C BIT IS SET ON EXITING THESE
;ROUTINES, OTHERWISE THE C BIT IS CLEARED
;REGISTERS USED:
;
; R0,SG PASS ARGUMENTS AND ARE ALTERED
; R1 IS GARBAGED
;"LAST" DECREMENTS THE MOTION STEP POINTER
LAST: SUB #10,R0 ;MOVE STEP POINTER BACK ONE
CMP #STPLST,R0 ;CHECK IF BEYOND STEP NO.1
BLE .+6 ;EXIT IF OK
MOV #STPLST,R0 ;ELSE POINT TO STEP NO.1
CLC
RTS PC
;"TERMIN" SIGNALS THE TERMINATION OF THE EDIT MODE
TERMIN: SEC ;TERMINATE EDITING
RTS PC
;"SKIP" MOVES THE CURRENT LINE MARKER UP OR DOWN AN ARBITRARY NUMBER OF STEPS
SKIP: MOV R0,-(SP) ;SAVE STEP POINTER
JSR PC,INTSCN ;GET NUMBER OF LINES TO SKIP
BCC .+6 ;BRANCH IF NUMBER FOUND
MOV #1,R0 ;ELSE SKIP DOWN ONE LINE
ASH #3,R0 ;CONVERT TO WORD INDEX
ADD (SP)+,R0 ;ADD OLD POINTER TO NUMBER OF LINES TO SKIP
CMP #STPLST,R0 ;CHECK IF TOO LOW
BLE .+6
MOV #STPLST,R0 ;CAN'T GO BELOW THIS
CMP #STPEND,R0 ;CHECK IF TOO HIGH
BGT .+6
MOV #STPEND-10,R0 ;CAN'T GO ABOVE THIS
CLC
RTS PC
;MOTION INSTRUCTION SUBROUTINE CALLING SEQUENCES
; ****** DECODING ROUTINES *******
;THE SUBROUTINES USED TO DECODE MOTION INSTRUCTIONS READ IN AS STRINGS
;ARE ALL CALLED "D____", EG. "DDRAW", "DMOVE". THESE ROUTINES REQUIRE
;THAT SG CONTAIN A POINTER TO THE INPUT STRING CONTAINING THE ARGUMENTS
;REQUIRED FOR THE SPECIFIC MOTION INSTRUCTION. ALSO, R0 MUST CONTAIN THE
;ABSOLUTE ADDRESS OF THE PROGRAM SPACE WHERE THE MOTION INSTRUCTION IS
;TO BE STORED. A SAMPLE CALLING SEQUENCE FOLLOWS.
;
; MOV #STRING,SG ;PTR TO STRING CONTAINING DRAW ARG.
; MOV #STEP,R0 ;ABS. ADDR TO STORE FORMED INSTRUCTION
; JSR PC,DDRAW
; BCS ERROR ;C BIT SET IF DRAW INSTRUCTION DECODES
; ; INPROPERLY
;
;IF THE ARGUMENTS SPECIFIED IN THE INPUT STRING ARE INCORRECT, THESE
;ROUTINES EXIT WITH THE C BIT SET, OTHERWISE THE C BIT IS CLEARED.
;REGISTERS USED:
;
; R0,SG PASS ARGUMENTS AND SG IS ALTERED
; R1,AC0,AC1 ARE GARBAGED
; ****** PRINT ROUTINES *******
;THE ROUTINES USED FOR CONVERTING A STORED MOTION INSTRUCTION INTO A
;OUTPUT STRING ARE ALL CALLED "P____", EG. "PDRAW","PMOVE". THESE
;SUBROUTINES ASSUME THAT THE STRING POINTER REGISTER, SG, POINTS TO
;A STRING THAT HAS ALREADY BEEN FILLED WITH THE INSTRUCTION STEP NUMBER
;AND FUNCTION NAME. THESE ROUTINES THEN APPEND THE ARGUMENTS OF THE
;MOTION INSTRUCTION ONTO THIS EXISTING STRING. THE ABSOLUTE ADDRESS
;OF THE INSTRUCTION STEP TO BE CONVERTED MUST BE LOADED INTO R0.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #OUTBUF,SG ;PTR TO OUTPUT STRING
; MOV STEP,R0 ;ABS. ADDR OF INSTR. STEP
; JSR PC,PDRAW
;
;REGISTER SG IS LEFT POINTING AT A NULL CHARACTER AT THE END OF
;THE STRING. NO CARRIAGE RETURN OR LINE FEED CHARACTERS ARE PLACED
;IN THE STRING BY THESE ROUTINES.
;REGISTERS USED:
;
; R0,SG PASS ARGUMENTS AND ARE ALTERED
; R1,AC0,AC1 ARE GARBAGED
; ****** RUN-TIME ROUTINES *******
;THE ROUTINES THAT EXECUTE THE MOTION FUNCTIONS SPECIFIED BY THE STORED
;MOTIONS INSTRUCTIONS ARE ALL CALLED "F____", EG. "FDRAW", AND ARE
;REFERRED TO AS THE RUN-TIME CODE. THESE ROUTINES ASSUME THAT R0
;CONTAINS THE ABSOLUTE ADDRESS OF THE STORE MOTION INSTRUCTION TO BE
;EXECUTED. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV STEP,R0 ;ABS. ADDR. OF INSTR. STEP
; JSR PC,FDRAW
;
;AT THE COMPLETION OF THESE ROUTINES THE ARM STATUS WORD ( "ARMSTT" )
;CONTAINS FLAG BITS WHICH INDICATE ANY RUN-TIME ERRORS WHICH MIGHT
;HAVE OCCURRED. IN ADDITION, THESE FLAG BITS ARE USED FOR INDICATING
;IF A "STOP" OR "PAUSE" INSTRUCTION WAS EXECUTED. 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 "NXTSTP" WILL HAVE BEEN ADJUSTED TO THE ABSOLUTE ADDRESS OF
;OF NEXT STEP TO BE EXECUTE.
;REGISTERS USED:
;
; R0 PASSES ARGUMENTS AND IS ALTERED
; R1,SG, AND ALL FLOATING AC'S MAY BE GARBAGED
;END OF DOCUMENTATION
;"DRAW" - MOTION INSTRUCTION
;THE DRAW INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENTS:
;
; DRAW SCALE,VECT,DURATION,NO INTEGRATION FLAG
;
;THE INTERNAL STRUCTURE OF THE DRAW INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 or 002| 001 |
; |-----------------------|
; | VECTOR POINTER |
; |-----------------------|
; | SCALE FACTOR X 1000 |
; |-----------------------|
; | EXECUTION TIME X 100 |
; |-----------------------|
;
;THE UPPER BYTE OF THE FIRST WORD IS SET TO 002 IF THE HARDWARE
;INTEGRATION IS TO BE TURNED OFF DURING THE MOTION, OTHERWISE IT
;IS SET TO 000.
;DECODING ROUTINE
DDRAW: MOV R0,R1 ;SAVE POINTER TO STEP NUMBER
MOV #1,(R1) ;SET THE FUNCTION NUMBER AND MODE BITS
JSR PC,RELSCN ;PICK UP SCALE FACTOR
BCC DDRW2 ;BRANCH IF SPECIFIED
DDRW1: MOV #DDRWC1,SG ;ELSE SIGNAL ERROR IF NOT SPECIFIED
BR DDRW6
DDRW2: MULF #42572,AC0 ;MULT SCALE FACTOR BY 1000
STCFI AC0,4(R1) ;PUT INTEGER SCALE FACTx1000 IN INSTRUCTION
BCC DDRW3
MOV #DDRWC2,SG ;SIGNAL ERROR IF NUMBER TOO LARGE
BR DDRW6
DDRW3: JSR PC,CLRCMA ;PICK UP VECTOR NUMBER
JSR PC,INTSCN
BCS DDRW1 ;SIGNAL ERROR IF NOT SPECIFIED
TST R0 ;CHECK FOR VECTOR NUMBER IN RANGE
BGE DDRW5
DDRW4: MOV #DDRWC3,SG ;SIGNAL ERROR
BR DDRW6 ;EXIT
DDRW5: CMP #MAXVEC,R0
BLE DDRW4
ASH #4,R0 ;CONVERT TO ABSOLUTE ADDRESS
ADD #VECLST,R0
MOV R0,2(R1) ;SAVE VECTOR POINTER
JSR PC,CLRCMA ;GET LENGTH OF TIME FOR OPERATION
JSR PC,RELSCN
BCS DDRW1 ;SIGNAL ERROR IF NOT SPECIFIED
MULF #41710,AC0 ;MULT TIME BY 100
STCFI AC0,6(R1) ;SAVE IN INSTRUCTION SPACE
BCS .+4 ;BRANCH AND SIGNAL ERROR IF TIME TOO LARGE
BGT DDRW7 ;BRANCH IF TIME GREATER THAN ZERO
MOV #DDRWC4,SG ;SIGNAL ERROR IF TIME OUT OF RANGE
DDRW6: JSR PC,TYPSTR
SEC
BR DDRDNE
DDRW7: JSR PC,CLRCMA ;GET HARDWARE INTEGRATION FLAG
JSR PC,INTSCN
BCS DDRW1 ;BRANCH IF NOT SPECIFIED
TST R0 ;IF SPECIFIED, CHECK IF ON OR OFF
BEQ .+10
BISB #2,1(R1) ;TURN HARDWARE INTEGRATION OFF DURING MOTION
DDRDNE: MOV R1,R0 ;RESTORE STEP POINTER
RTS PC
;PRINT ROUTINE
PDRAW: MOV R0,R1 ;SAVE STEP POINTER
LDCIF 4(R1),AC0 ;FLOAT SCALE FACTOR
DIVF #42572,AC0 ;DIVIDE SCALE FACTOR BY 1000
JSR PC,CVG ;CONVERT TO ASC AND STORE
MOVB #54,(SG)+ ;PUT IN A COMMA
MOVB #40,(SG)+ ;AND TWO SPACE CHARACTERS
MOVB #40,(SG)+
MOV 2(R1),R0 ;GET VECTOR ADDRESS
SUB #VECLST,R0 ;CONVERT TO NUMBER
ASH #-4,R0
JSR PC,CVI ;CONVERT TO ASC AND STORE
MOVB #54,(SG)+ ;PUT IN A COMMA
LDCIF 6(R1),AC0 ;FLOAT TIME
DIVF #41710,AC0 ;DIVID TIME BY 100
JSR PC,CVG ;CONVERT TO ASC
MOVB #54,(SG)+ ;PUT IN A COMMA
MOVB #40,(SG)+ ;AND TWO SPACE CHARACTERS
MOVB #40,(SG)+
CLR R0 ;ASSUME INTEGRATION ON
BITB #2,1(R1) ;CHECK IF ON OR OFF
BEQ .+4
INC R0 ;SIGNAL NO HARDWARE INTEGRATION
JSR PC,CVI ;CONVERT TO ASC AND STORE
CLRB (SG)
RTS PC ;RETURN
;RUN-TIME ROUTINE
FDRAW: MOV R2,-(SP)
MOV #ARMBRK,R1 ;ASSUME INTEGRATION DURING MOTION
BITB #2,1(R0) ;CHECK NO INTEGRATION FLAG
BEQ .+6
BIS #NOINTG,R1 ;SET NO INTEGRATION FLAG
MOV R1,INTFLG ;SAVE WITH BRAKE BITS
MOV 2(R0),FDVECT ;SAVE VECTOR POINTER
LDCIF 4(R0),AC0 ;CONVERT SCALE FACTOR TO FLOATING POINT
DIVF #42572,AC0 ;SCALE UP BY A FACTOR OF 1000
STF AC0,FDSCLE ;SAVE IT
MOV 6(R0),R1 ;GET THE TOTAL TIME OF MOTION IN 10 MSEC COUNTS
LDCIF R1,AC0 ;FLOAT IT
MOV R1,FDTINC ;SAVE IT
STF AC0,FDTIME
ADD #500.,R1 ;ALLOW 5 SECONDS FOR FINAL SETTLING OF ARM
BVC .+6 ;SKIP IF NO OVERFLOW
MOV #77777,R1 ;ELSE ALLOW MAXIMUM POSSIBLE TIME FOR MOTION
MOV R1,FDRTME
MOV #TMPTRN,R0 ;CONVERT CURRENT JOINT ANGLES TO TRANSFORM
MOV #JANGLE,R1
JSR PC,UPDATE
MOV #TMPTRN+60,R0 ;SAVE START POSITION
MOV #6,R1 ;THREE F.P. NUMBERS IN ALL
MOV #FDPOS,R2 ;PUT IN HERE
MOV (R0)+,(R2)+
SOB R1,.-2
MOV #FDRLST,R0 ;POINT TO SERVO DATA
JSR PC,DRIVE ;GO DRIVE ARM JOINTS
FDWDNE: MOV (SP)+,R2
RTS PC
;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 10 MILLISECONDS
FDRSUB: MOV R2,-(SP)
MOV R3,-(SP)
LDCIF PTIME,AC0 ;GET CURRENT TIME
DIVF FDTIME,AC0 ;NORMALIZE BETWEEN 0 AND 1 FOR ENTIRE MOTION
LDF #40700,AC1 ;EVALUATE 5TH ORDER POLY TO GET SMOOTH MOTION
MULF AC0,AC1 ;6.0 x T
ADDF #141160,AC1 ;-15.0
MULF AC0,AC1 ; x T
ADDF #41040,AC1 ;+10.0
MULF AC0,AC1 ;x T
MOV FDVECT,R0 ;GET POINTER TO CHANGE VECTOR
MULF AC0,AC1 ;x T
MOV #FDPOS,R1 ;GET POINTER TO INITIAL ARM POSITION
MULF AC0,AC1 ;x T
MOV #TMPTRN+60,R2 ;GET POINTER TO ARM TRANSFORM
MULF FDSCLE,AC1 ;x VECTOR SCALE FACTOR
MOV #3,R3 ;CORRECT ALL THREE POSITION COMPONENTS
FDRS1: LDF (R0)+,AC0 ;GET VECTOR COMPONENT
MULF AC1,AC0 ;MULTIPLY TIMES SCALE AND TIME FACTORS
ADDF (R1)+,AC0 ;ADD CHANGE TO INITIAL ARM POSITION
STF AC0,(R2)+ ;SAVE IN TRANSFORM
SOB R3,FDRS1
MOV #TMPTRN,R0 ;COMPUTE NEW JOINT ANGLES
MOV #JANGLE,R1
JSR PC,SOLVE
TST R0 ;CHECK IF SOLUTION FOUND
BEQ FDRS2 ;BRANCH IF NO ERRORS
BIS #BADSOL+CANPRO+CHKDNE,ARMSTT ;ELSE SET ERROR BITS
BR FDSDNE
FDRS2: CMP PTIME,FDTINC ;CHECK IF INCREMENTAL MOTION TIME OVER
BLT FDSDNE ;SKIP IF STILL MOVING
BIS #CHKDNE,ARMSTT ;ELSE, INDICATE CHECK FOR IN RANGE BITS
MOV DR11S,-(SP) ;TURN ON HARDWARE INTEGRATION
MOV #BRKMDE,DR11S ;SET BRAKE BIT MODE
MOV #ARMBRK+ENABLE,DR11O
MOV (SP)+,DR11S
FDSDNE: MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;LOCAL STORAGE AREA
FDRLST: ARMDNE ;ARM IN RANGE MASK BITS
FDRTME: 0 ;ARM MOTION TIME
FDRSUB ;SUBROUTINE TO OUTPUT INCREMENTAL MOTION
INTFLG: ARMBRK ;BRAKE BITS
1 ;FIRST JOINT TO BE SERVOED
6 ;TOTAL NUMBER OF JOINTS TO BE SERVOED
FDVECT: 0 ;POINTER TO CHANGE VECTOR
FDSCLE: .BLKW 2 ;VECTOR SCALE FACTOR IN F.P.
FDTINC: 0 ;TOTAL TIME COUNT OF INCREMENTAL MOTION
FDTIME: .BLKW 2 ;SAME AS FDTINC EXCEPT IN F.P.
FDPOS: .BLKW 6 ;INITIAL ARM POSITION
DDRWC1: .ASCII /ERROR, INSTRUCTION FORM IS "DRAW SF,VECT,TIME,NO INTG FLAG"/
.BYTE 15,12,0
DDRWC2: .ASCII /ERROR, SCALE FACTOR OUT OF RANGE/
.BYTE 15,12,0
DDRWC3: .ASCII /ERROR, VECTOR NUMBER OUT OF RANGE/
.BYTE 15,12,0
DDRWC4: .ASCII /ERROR, TIME SPECIFICATION OUT OF RANGE/
.BYTE 15,12,0
.EVEN
;END OF "DRAW" ROUTINES
;"MOVE" - MOTION INSTRUCTION
;THE MOVE INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENTS:
;
; MOVE <TRANS> OR MOVE
;
;IF NO TRANSFORM IS SPECIFIED, A TRANSFORM IS ALLOCATED AND DEFINED
;TO BE THE PRESENT ARM POSITION. THE MOVE WILL THEN BE TO THIS
;NEWLY DEFINED TRANSFORM. THE INTERNAL STRUCTURE OF THE MOVE
;INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 | 002 |
; |-----------------------|
; | TRANSFORM POINTER |
; |-----------------------|
; | 000000 |
; |-----------------------|
; | 000000 |
; |-----------------------|
;DECODING ROUTINE
DMOVE: MOV #2,(R0) ;SET THE FUNCTION NUMBER AND MODE BITS
DMOV1: MOV R0,-(SP) ;SAVE POINTER TO STEP NUMBER
MOV R2,-(SP) ;SAVE REGISTER
CLR 4(R0) ;CLEAR EXTRA CELLS
CLR 6(R0)
JSR PC,GETTRN ;PICK UP TRANSFORM NUMBER
BCS DMVDNE ;EXIT IF TRANS ERROR
TST R1 ;CHECK IF TRANS SPECIFIED
BEQ DMOV3 ;BRANCH IF IT WAS
MOV R0,R2 ;ELSE,SET TRANS EQUAL TO CURRENT LOCATION
MOV #1,R0 ;READ CURRENT LOCATION STARTING WITH JOINT 1
MOV #7,R1 ;SEVEN JOINTS IN ALL
JSR PC,ANGLES ;READ CURRENT LOCATION IN "JANGLE"
BCC DMOV2 ;BRANCH IF NO ADC ERROR
MOV #ADCMES,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
SEC ;INDICATE INPUT ERROR
BR DMVDNE ;EXIT
DMOV2: MOV R2,R0
MOV #JANGLE,R1 ;HERE ARE THE CURRENT JOINT ANGLES
JSR PC,UPDATE ;CONVERT FROM JT. ANGLE TO TRANSFORM
MOV #DMOVC1,SG ;TELL OPERATOR NEW TRANS DEFINED
JSR PC,TYPSTR
SUB #TRNLST,R0 ;CONVERT TRANS PTR TO ASC NUMBER
ASH #-6,R0
MOV #OUTBUF,SG
JSR PC,CVI
MOV #OUTBUF,SG
JSR PC,TYPSTR
MOV #DMOVC2,SG ;TYPE OUT REMAINER OF MESSAGE
JSR PC,TYPSTR
MOV R2,R0 ;RESTORE TRANS POINTER
DMOV3: BIS #40000,74(R0) ;INDICATE TRANSFORM REFERENCED
MOV R0,R1 ;SHIFT TRANSFORM POINTER
DMVDNE: MOV (SP)+,R2 ;RESTORE REGISTER
MOV (SP)+,R0 ;RESTORE STEP POINTER
MOV R1,2(R0) ;SAVE TRANSFORM POINTER
RTS PC
;PRINT ROUTINE
PMOVE: MOVB #40,(SG)+ ;PUT IN TWO SPACE CHARACTERS
MOVB #40,(SG)+
MOV 2(R0),R0 ;GET THE TRANSFORM ADDRESS
SUB #TRNLST,R0 ;CONVERT TO NUMBER
ASH #-6,R0
JSR PC,CVI ;CONVERT TO ASC AND STORE
CLRB (SG)
RTS PC ;RETURN
;RUN-TIME ROUTINE
FMOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV 2(R0),R0 ;GET POINTER TO FINAL TRANSFORM
MOV R0,-(SP)
MOV #JANGLE,R1 ;COPY PRESENT POSITION INTO JANGF AND JANGM
MOV #JANGF,R2 ;ARM SOLUTIONS NEED THIS DATA
MOV #JANGM,R3
MOV #12.,R4 ;6 ANGLES IN ALL
MOV (R1),(R2)+ ;TRANSFER
MOV (R1)+,(R3)+
SOB R4,.-4
MOV #JANGF,R1 ;GET FINAL JOINT ANGLES AND STORE HERE
JSR PC,SOLVE
TST R0 ;CHECK FOR INVALID SOLUTION
BEQ FMOV1 ;BRANCH IF OK
FMVERR: MOV #FMOVC1,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
MOV #BADSOL+CANPRO,ARMSTT ;INDICATE ERROR, CAN PROCEED
BR FMVDNE ;RETURN
FMOV1: MOV (SP),R0 ;COMPUTE APPROACH POINT FROM FINAL POSITION
MOV #DEPROT,R1 ;APPROACH ← FINAL TRANSFORM x DEPROACH TRANSFORM
MOV #TMPTRN,R2 ;LEAVE RESULT IN HERE
JSR PC,TIMES
MOV #TMPTRN,R0 ;COMPUTE APPROACH POINT JOINT ANGLES
MOV #JANGM,R1 ;LEAVE IN HERE
JSR PC,SOLVE
TST R0 ;CHECK FOR VALID SOLUTION
BNE FMVERR ;BRANCH IF NO SOLUTION
MOV #TMPTRN,R0 ;PUT PRESENT TRANSFORM IN HERE
MOV #JANGLE,R1 ;HERE ARE THE CURRENT JOINT ANGLES
JSR PC,UPDATE ;GO COMPUTE PRESENT TRANSFORM
MOV #TMPTRN,R0 ;DEPARTURE PT ← PRESENT TRANS x DEPROACH TRANS
MOV #DEPROT,R1
MOV #TMPTRN,R2 ;PUT RESULTS BACK IN TMPTRN
JSR PC,TIMES
MOV #TMPTRN,R0 ;COMPUTE DEPARTURE POINT JOINT ANGLES
MOV #JANGLE,R1 ;PUT THEM IN HERE
JSR PC,SOLVE
TST R0 ;CHECK IF SOLUTION EXISTS
BNE FMVERR ;BRANCH IF NO SOLUTION
MOV #FMVLST,R0 ;NOW MOVE TO DEPARTURE POINT
JSR PC,DRIVE ;GO DRIVE THE ARM JOINTS
TST ARMSTT ;EXIT IF AN ERROR OCCURRED
BNE FMVDNE
MOV #JANGM,R0 ;ELSE GET ANGLES TO MOVE TO APPROACH POINT
MOV #JANGLE,R1 ;PUT THEM IN HERE
MOV #12.,R2
MOV (R0)+,(R1)+ ;TRANSFER 6 JOINT ANGLES
SOB R2,.-2
MOV #FMVLST,R0 ;NOW MOVE TO APPROACH POINT
JSR PC,DRIVE
TST ARMSTT ;EXIT IF ERROR
BNE FMVDNE
MOV #JANGF,R0 ;ELSE REPEAT PROCEDURE FOR FINAL POINT OF MOTION
MOV #JANGLE,R1 ;PUT THEM IN HERE
MOV #12.,R2
MOV (R0)+,(R1)+ ;TRANSFER 6 JOINT ANGLES
SOB R2,.-2
MOV #FMVLST,R0 ;MOVE TO FINAL POSITION
JSR PC,DRIVE
FMVDNE: MOV (SP)+,R0 ;CLEAR STACK
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 10 MILLISECONDS
FMVSUB: CMP #3,PTIME ;WAIT AT LEAST 30 MSEC BEFORE CHECKING DONE
BGT .+10
BIS #CHKDNE,ARMSTT ;START CHECKING FOR ARM JOINTS IN RANGE
RTS PC
;LOCAL STORAGE AREA
FMVLST: ARMDNE ;MASK BITS FOR ARM JOINTS
600. ;ALLOW 6 SECONDS FOR ARM MOTION
FMVSUB ;10MSEC SUBROUTINE
ARMBRK ;BRAKE BITS
1 ;FIRST JOINT TO BE SERVOED
6 ;TOTAL NUMBER OF JOINTS TO BE SERVOED
JANGM: .BLKW 12. ;JOINT ANGLES FOR APPROACH POINT
JANGF: .BLKW 12. ;JOINT ANGLES FOR FINAL POINT
DMOVC1: .ASCIZ /TRANSFORM # /
DMOVC2: .ASCII / DEFINED/
.BYTE 15,12,0
FMOVC1: .ASCII /REQUIRED ARM SOLUTION DOES NOT EXIST/
.BYTE 15,12,0
.EVEN
;END OF "MOVE" ROUTINES
;"OPEN" - MOTION INSTRUCTION
;THE OPEN INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENT:
;
; OPEN DISTANCE
;
;THE INTERNAL STRUCTURE OF THE OPEN INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 | 003 |
; |-----------------------|
; | HAND OPENING, IN |
; |- - - - - - - - - - - -|
; | TWO WORDS, FLT. PT. |
; |-----------------------|
; | 000000 |
; |-----------------------|
;DECODING ROUTINE
DOPEN: MOV #3,(R0) ;SET THE FUNCTION NUMBER AND MODE BITS
DOPE1: JSR PC,RELSCN ;PICK UP THE HAND OPENING
BCC DOPE2
MOV #DOPEC1,SG ;SIGNAL ERROR AND EXIT IF NOT SPECIFIED
JSR PC,TYPSTR
SEC
BR DOPDNE
DOPE2: STF AC0,2(R0) ;SAVE HAND OPENING
CLR 6(R0) ;CLEAR EXTRA WORD
DOPDNE: RTS PC
;PRINT ROUTINE
POPEN: LDF 2(R0),AC0 ;GET HAND OPENING
JSR PC,CVG ;CONVERT TO ASC AND STORE
CLRB (SG)
RTS PC ;RETURN
;RUN-TIME ROUTINE
FOPEN: LDF 2(R0),AC0 ;GET HAND OPENING
CMPF UHAND,AC0 ;MAKE SURE HAND OPENING WITHIN RANGE
CFCC
BGE FOPN1 ;BRANCH IF LESS THAN MAXIMUM
LDF UHAND,AC0 ;ELSE SET EQUAL TO MAXIMUM
BR FOPN2
FOPN1: CMPF LHAND,AC0 ;COMPARE TO MINIMUM HAND OPENING
CFCC
BLE FOPN2 ;BRANCH IF GREATER THAN MINIMUM
LDF LHAND,AC0 ;ELSE REPLACE WITH MINIMUM
FOPN2: STF AC0,HAND ;SAVE REQUESTED HAND OPENING
MOV #FOPLST,R0 ;POINT TO SERVO DATA
JSR PC,DRIVE ;GO DRIVE THE ARM JOINTS
RTS PC
;SUBROUTINE TO COMPUTE HAND OPENING EVERY 10 MILLISECONDS
FOPSUB: CMP #3,PTIME ;WAIT AT LEAST 30 MSEC BEFORE CHECKING DONE
BGT .+10
BIS #CHKDNE,ARMSTT ;START CHECKING FOR ARM JOINTS IN RANGE
RTS PC
;LOCAL STORAGE AREA
FOPLST: HNDDNE ;MASK BIT FOR HAND
300. ;ALLOW 3 SECONDS FOR HAND OPERATION
FOPSUB ;10MSEC SUBROUTINE
HNDBRK ;BRAKE BIT
7 ;FIRST JOINT TO BE SERVOED
1 ;TOTAL NUMBER OF JOINTS TO BE SERVOED
DOPEC1: .ASCII /ERROR, MUST SPECIFY HAND OPENING/
.BYTE 15,12,0
.EVEN
;END OF "OPEN" ROUTINES
;"GRASP" - MOTION INSTRUCTION
;THE GRASP INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENT:
;
; GRASP DISTANCE
;
;THE INTERNAL STRUCTURE OF THE GRASP INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 | 004 |
; |-----------------------|
; | HAND OPENING, IN |
; |- - - - - - - - - - - -|
; | TWO WORDS, FLT. PT. |
; |-----------------------|
; | 000000 |
; |-----------------------|
;DECODING ROUTINE
DGRASP: MOV #4,(R0) ;SET THE FUNCTION NUMBER AND MODE BITS
BR DOPE1 ;FROM HERE ON, JUST LIKE OPEN COMMAND
;PRINT ROUTINE
PGRASP: MOV R0,-(SP)
MOV #6,R0 ;CHANGE FORMAT
MOV #2,R1
JSR PC,FORMAT
MOV (SP)+,R0 ;RESTORE STEP POINTER
LDF 2(R0),AC0 ;GET HAND OPENING
JSR PC,CVG ;CONVERT TO ASC AND STORE
CLRB (SG)
JSR PC,RSTFOR ;RESTORE OLD FORMAT
RTS PC ;RETURN
;RUN-TIME ROUTINE
FGRASP: ADD #2,R0 ;GET POINTER TO MINIMUM HAND OPENING
MOV R0,-(SP) ;SAVE IT
MOV #FGPLST,R0 ;GET SERVO DATA POINTER
JSR PC,DRIVE ;GO DRIVE THE ARM JOINTS
TST ARMSTT ;CHECK IF ANY ERROR BITS
BNE FGPDNE ;EXIT IF ANY ERROR OCCURRED
MOV #7,R0 ;READ THE CURRENT HAND OPENING
MOV #1,R1
JSR PC,ANGLES
BCC FGRP1 ;BRANCH IF NO ADC READ ERROR OCCURRED
MOV #ADCMES,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
BR FGPDNE ;EXIT
FGRP1: LDF HAND,AC0 ;GET HAND OPENING
CMPF @(SP),AC0 ;COMPARE TO EXPECTED MINIMUM OPENING
CFCC
BLE FGPDNE ;SIGNAL NO ERROR IF GREATER THAN MINIMUM
BIS #BADHND+CANPRO,ARMSTT ;ELSE SET ERROR CONDITION
MOV #FGPC1,SG ;TELL OPERATOR
JSR PC,TYPSTR
FGPDNE: MOV (SP)+,R0 ;CLEAR STACK
RTS PC
;SUBROUTINE TO COMPUTE HAND OPENING EVERY 10 MILLISECONDS
FGPSUB: LDF HAND,AC2 ;GET CURRENT HAND OPENING
SUBF HNDINC,AC2 ;CHANGE HAND OPENING BY THIS AMOUNT
CMPF LHAND,AC2 ;COMPARE TO MINIMUM HAND OPENING
CFCC
BLT FGPS1 ;BRANCH IF STILL SOME DISTANCE TO CLOSE
BIS #CHKDNE,ARMSTT ;ELSE DONE, SET TERMINATION TEST BIT
LDF LHAND,AC2 ;SET MINIMUN HAND OPENING
FGPS1: MOV #7,R0 ;READ THE CURRENT HAND OPENING
MOV #1,R1
JSR PC,ANGLES
BCS FGPSDN ;EXIT IF ADC READ ERROR
LDF HAND,AC0 ;GET THE CURRENT HAND OPENING
SUBF AC2,AC0 ;SUBT. NEXT SET POINT FROM CURRENT OPENING
ABSF AC0 ;CHECK IF DIFFERENCE TOO GREAT
CMPF HNDHLT,AC0 ;IF TOO LARGE, HAND MUST BE GRASPING SOMETHING
CFCC
BGT FGPSDN ;BRANCH IF STILL NOTHING FOUND
BIS #CHKDNE,ARMSTT ;ELSE SET TERMINATION BIT
LDF HAND,AC2 ;USE CURRENT OPENING AS FINAL OPENING
FGPSDN: STF AC2,HAND ;SAVE NEXT SET POINT
RTS PC
;LOCAL STORAGE AREA
FGPLST: HNDDNE ;MASK BITS FOR HAND IN RANGE
500. ;ALLOW 5 SECONDS FOR HAND OPERATION
FGPSUB ;10MSEC SUBROUTINE
HNDBRK ;BRAKE BITS
7 ;FIRST JOINT TO BE SERVOED
1 ;TOTAL NUMBER OF JOINTS TO BE SERVOED
HNDINC: .WORD 36443,153412 ; .1000000@-1 RATE OF HAND CLOSING PER 10 MILSEC.
HNDHLT: .WORD 37600, 0 ; .2500000 GRASP TERMINATION ERROR DISTANCE
FGPC1: .ASCII /***ERROR***, HAND CLOSED TOO FAR/
.BYTE 15,12,0
.EVEN
;END OF "GRASP" ROUTINES
;"PAUSE" - MOTION INSTRUCTION
;THE PAUSE INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENT:
;
; PAUSE <MESSAGE>
;
;THE INTERNAL STRUCTURE OF THE PAUSE INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 or 001| 005 |
; |-----------------------|
; | STRING POINTER |
; |-----------------------|
; | 000000 |
; |-----------------------|
; | 000000 |
; |-----------------------|
;
;THE UPPER BYTE OF THE FIRST WORD IS EITHER 001 OR 000 DEPENDING UPON
;WHETHER OR NOT THE PAUSE MESSAGE STRING EXISTS. IF THERE IS AN
;ASSOCIATED STRING IT IS 001 OTHERWISE IT IS 000.
;DECODING ROUTINE
DPAUSE: MOV #5,(R0) ;SET THE FUNCTION NUMBER
JSR PC,SAVSTG ;STORE MESSAGE IN STRING AREA
BCS DPSDNE ;SIGNAL ERROR IF NO STRING SPACE LEFT
MOV R1,2(R0) ;ELSE SAVE STRING POINTER
BEQ .+6 ;TEST IF MESSAGE STRING TYPED IN
BIS #400,(R0) ;IF STRING EXISTS, PUT 001 IN UPPER BYTE
CLR 4(R0) ;CLEAR EXTRA WORDS
CLR 6(R0)
DPSDNE: RTS PC
;PRINT ROUTINE
PPAUSE: MOV 2(R0),R1 ;GET POINTER TO MESSAGE STRING
BNE PPAUS1 ;BRANCH IF MESSAGE STRING INCLUDED
CLRB (SG) ;ELSE JUST PUT IN A NULL CHARACTER
RTS PC
PPAUS1: MOVB (R1)+,(SG)+ ;TRANSFER THE MESSAGE TO THE OUTPUT STRING
BNE .-2
DEC SG ;POINT TO NULL CHARACTER
RTS PC
;RUN-TIME ROUTINE
FPAUSE: MOV 2(R0),SG ;GET POINTER TO MESSAGE STRING
BEQ .+6 ;SKIP IF NO MESSAGE STRING
JSR PC,TYPSTR ;ELSE TYPE MESSAGE
MOV #FPASC1,SG ;TYPE CRLF AND "PAUSE"
JSR PC,TYPSTR
MOV #CANPRO,ARMSTT ;INDICATE STOP EXECUTION, CAN PROCEED
RTS PC
FPASC1: .BYTE 15,12
.ASCIZ /PAUSE: /
.EVEN
;END OF "PAUSE" ROUTINES
;"STOP" - MOTION INSTRUCTION
;THE STOP INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENT:
;
; STOP <MESSAGE>
;
;THE INTERNAL STRUCTURE OF THE STOP INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 or 001| 006 |
; |-----------------------|
; | STRING POINTER |
; |-----------------------|
; | 000000 |
; |-----------------------|
; | 000000 |
; |-----------------------|
;
;THE UPPER BYTE OF THE FIRST WORD IS EITHER 001 OR 000 DEPENDING UPON
;WHETHER OR NOT THE PAUSE MESSAGE STRING EXISTS. IF THERE IS AN
;ASSOCIATED STRING IT IS 001 OTHERWISE IT IS 000.
;DECODING ROUTINE
DSTOP: MOV #6,(R0) ;SET THE FUNCTION NUMBER
JSR PC,SAVSTG ;STORE MESSAGE IN STRING AREA
BCS DSPDNE ;SIGNAL ERROR IF NO STRING SPACE LEFT
MOV R1,2(R0) ;ELSE SAVE STRING POINTER
BEQ .+6 ;TEST IF MESSAGE STRING TYPED IN
BIS #400,(R0) ;IF STRING EXISTS, PUT 001 IN UPPER BYTE
CLR 4(R0) ;CLEAR EXTRA WORDS
CLR 6(R0)
DSPDNE: RTS PC
;PRINT ROUTINE: SAME AS "PAUSE" PRINT ROUTINE
;RUN-TIME ROUTINE
FSTOP: MOV 2(R0),SG ;GET POINTER TO MESSAGE STRING
BEQ .+6 ;SKIP IF NO MESSAGE STRING
JSR PC,TYPSTR ;ELSE TYPE MESSAGE
MOV #FSTPC1,SG ;TYPE CRLF AND "STOP"
JSR PC,TYPSTR
MOV #STPPRG,ARMSTT ;INDICATE STOP EXECUTION, CAN'T PROCEED
RTS PC
FSTPC1: .BYTE 15,12
.ASCIZ /STOP: /
.EVEN
;END OF "STOP" ROUTINES
;"GO" - MOTION INSTRUCTION
;THE GO INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENTS:
;
; GO <TRANS> OR GO
;
;IF NO TRANSFORM IS SPECIFIED, A TRANSFORM IS ALLOCATED AND DEFINED
;TO BE THE PRESENT ARM POSITION. THE GO WILL THEN BE TO THIS
;NEWLY DEFINED TRANSFORM. THE INTERNAL STRUCTURE OF THE GO
;INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 | 007 |
; |-----------------------|
; | TRANSFORM POINTER |
; |-----------------------|
; | 000000 |
; |-----------------------|
; | 000000 |
; |-----------------------|
;DECODING ROUTINE
DGO: MOV #7,(R0) ;SET THE FUNCTION NUMBER AND MODE BITS
JMP DMOV1 ;FROM HERE ITS THE SAME AS THE DECODE MOVE
; SEQUENCE
;PRINT ROUTINE: SAME "MOVE" PRINT ROUTINE
;RUN-TIME ROUTINE
FGO: MOV 2(R0),R0 ;GET POINTER TO REQUIRED TRANSFORM
MOV #JANGLE,R1 ;CONVERT TO JOINT ANGLES AND STORE HERE
JSR PC,SOLVE
TST R0 ;CHECK FOR INVALID SOLUTION
BEQ FGO1 ;BRANCH IF OK
MOV #FGOC1,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
MOV #BADSOL+CANPRO,ARMSTT ;INDICATE ERROR, CAN PROCEED
BR FGODNE ;RETURN
FGO1: MOV #FGOLST,R0 ;POINT TO SERVO DATA
JSR PC,DRIVE ;GO DRIVE THE ARM JOINTS
FGODNE: RTS PC
;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 10 MILLISECONDS
FGOSUB: CMP #3,PTIME ;WAIT AT LEAST 30 MSEC BEFORE CHECKING DONE
BGT .+10
BIS #CHKDNE,ARMSTT ;START CHECKING FOR ARM JOINTS IN RANGE
RTS PC
;LOCAL STORAGE AREA
FGOLST: ARMDNE ;MASK BITS FOR ARM JOINTS
600. ;ALLOW 6 SECONDS FOR ARM MOTION
FGOSUB ;10MSEC SUBROUTINE
ARMBRK ;BRAKE BITS
1 ;FIRST JOINT TO BE SERVOED
6 ;TOTAL NUMBER OF JOINTS TO BE SERVOED
FGOC1: .ASCII /REQUIRED ARM SOLUTION DOES NOT EXIST/
.BYTE 15,12,0
.EVEN
;END OF "GO" ROUTINES
;"DRIVE" - MOTION INSTRUCTION
;THE DRIVE INSTRUCTION IS TYPED WITH THE FOLLOWING ARGUMENTS:
;
; DRIVE JT. NUMBER, CHANGE IN JT. ANGLE, DURATION
;
;ALL THREE ARGUMENTS MUST BE GIVEN. THE JOINT NUMBERS RANGE FROM 1 TO 7
;WITH 7 BEING THE HAND JOINT. THE CHANGE IN JOINT ANGLE IS STORED AS
;A INTEGER NUMBER AND CAN ASSUME VALUES FROM 320.00 TO -320.00. THE
;DURATION MUST BE IN SECONDS. THE INTERNAL STRUCTURE OF THE DRIVE
;INSTRUCTION BLOCK IS AS FOLLOWS:
;
; |-----------------------|
; STEP→ | 000 | 010 |
; |-----------------------|
; | JOINT NUMBER (1-7) |
; |-----------------------|
; | CHANGE IN JT ANG X 100|
; |-----------------------|
; | TIME X 100 |
; |-----------------------|
;DECODING ROUTINE
DDRIVE: MOV R0,R1 ;SAVE POINTER TO STEP NUMBER
MOV #10,(R1) ;SET THE FUNCTION NUMBER AND MODE BITS
JSR PC,INTSCN ;PICK UP THE JOINT NUMBER
BCC DDRV2 ;BRANCH IF NO ERROR SIGNALED
DDRV1: MOV #DDRVC1,SG ;ELSE TYPE ERROR MESSAGE
BR DDRV6
DDRV2: TST R0 ;CHECK FOR JOINT NUMBER BETWEEN 1 AND 7
BGT DDRV4 ;BRANCH IF GREATER THAN 0
DDRV3: MOV #DDRVC2,SG ;ELSE TYPE ERROR MESSAGE
BR DDRV6
DDRV4: CMP #7,R0 ;CHECK IF LESS THAN 7
BLT DDRV3 ;SIGNAL ERROR IF NOT
MOV R0,2(R1) ;SAVE JOINT NUMBER
JSR PC,CLRCMA ;GET CHANGE IN JOINT ANGLE
JSR PC,RELSCN
BCS DDRV1 ;BRANCH IF ERROR INDICATED
MULF #41710,AC0 ;SCALE UP BY A FACTOR OF 100
STCFI AC0,4(R1) ;SAVE CHANGE IN JOINT ANGLE
BCC DDRV5 ;BRANCH IF NOT TOO LARGE
MOV #DDRVC3,SG ;ELSE INDICATE NUMBER OUT OF RANGE
BR DDRV6
DDRV5: JSR PC,CLRCMA ;PICK UP TIME FOR MOTION
JSR PC,RELSCN
BCS DDRV1 ;BRANCH IF ERROR INDICATED
MULF #41710,AC0 ;ELSE SCALE BY A FACTOR OF 100
STCFI AC0,6(R1) ;SAVE DURATION OF MOTION
BCS .+4 ;SKIP AND SIGNAL ERROR IF DURATION OUT OF RANGE
BGT DDVDNE ;BRANCH IF DURATION GREATER THAN ZERO
MOV #DDRVC4,SG ;ELSE INDICATE NUMBER OUT OF RANGE
DDRV6: JSR PC,TYPSTR ;TYPE ERROR MESSAGE
SEC
DDVDNE: MOV R1,R0 ;RESTORE STEP POINTER
RTS PC
;PRINT ROUTINE
PDRIVE: MOV R0,R1 ;SAVE STEP POINTER
MOVB #40,(SG)+ ;PUT IN TWO SPACE CHARACTERS
MOVB #40,(SG)+
MOV 2(R1),R0 ;GET JOINT NUMBER
JSR PC,CVI ;CONVERT TO ASC AND STORE
MOVB #54,(SG)+ ;PUT IN A COMMA
LDCIF 4(R1),AC0 ;FLOAT THE CHANGE IN JOINT ANGLE
DIVF #41710,AC0 ;DIVIDE ANGLE BY 100
JSR PC,CVG ;CONVERT TO ASC AND STORE
MOVB #54,(SG)+ ;PUT IN A COMMA
LDCIF 6(R1),AC0 ;FLOAT TIME
DIVF #41710,AC0 ;DIVID TIME BY 100
JSR PC,CVG ;CONVERT TO ASC
CLRB (SG)
RTS PC ;RETURN
;RUN-TIME ROUTINE
FDRIVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV 2(R0),R1 ;SET JOINT NUMBER
MOV R1,FDRVJT ;SAVE IN DATA BLOCK
DEC R1 ;USE THIS FOR A SHIFT COUNT
MOV #1,R2 ;COMPUTE THE BRAKE BIT MASK
ASH R1,R2
MOV R2,FDRVBK ;SAVE THE BRAKE BIT
NEG R1 ;USE THIS AS A SHIFT COUNT FOR THE IN RANGE BIT
MOV #100,R2 ;GET IN RANGE BIT
ASH R1,R2
MOV R2,FDRVLT ;SAVE IN RANGE DONE BIT
MOV 2(R0),R1 ;CONVERT JOINT NUMBER TO F.P. NUMBER INDEX
DEC R1
ASH #2,R1
MOV R1,FPINDX ;SAVE INDEX
LDCIF 4(R0),AC0 ;GET THE CHANGE IN JOINT ANGLE
DIVF #41710,AC0 ;SCALE BACK TO DEGREES OR INCHES
STF AC0,DTH ;SAVE CHANGE IN JOINT ANGLE FOR LATER
ADDF JANGLE(R1),AC0 ;ADD CURRENT JOINT ANGLE OF REQUESTED JOINT
CMPF LSTOP(R1),AC0 ;COMPARE TO JOINT STOP LIMITS
CFCC
BGT FDRV1 ;BRANCH AND SIGNAL ERROR IF TOO LOW
CMPF USTOP(R1),AC0 ;COMPARE TO UPPER STOP LIMIT
CFCC
BGE FDRV2 ;BRANCH IF IN RANGE
FDRV1: MOV #FDRVC1,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPSTR
MOV #BADSOL+CANPRO,ARMSTT ;INDICATE ERROR, BUT CAN PROCEED
BR FDRVDN ;EXIT
FDRV2: STF AC0,FTH ;SAVE FINAL JOINT ANGLE
MOV 6(R0),R2 ;GET DURATION FOR MOTION
MOV R2,TIME1 ;SAVE WITH JOINT ANGLE DATA
ADD #300.,R2 ;ALLOW 3 SECONDS TO NULL ERROR
BVC .+6 ;CHECK FOR OVERFLOW
MOV #77777,R2 ;REPLACE WITH MAXIMUM PERMITED TIME IF SO
MOV R2,FDRVTM ;SAVE TOTAL TIME FOR MOTION
MOV #FDRVLT,R0 ;PERFORM MOTION
JSR PC,DRIVE
FDRVDN: MOV (SP)+,R2 ;CLEAR STACK
RTS PC
;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 10 MILLISECONDS
FDRVSB: LDCIF PTIME,AC0 ;GET CURRENT TIME
LDCIF TIME1,AC1 ;NORMALIZE BETWEEN 0 AND 1 FOR ENTIRE MOTION
DIVF AC1,AC0
LDF #40700,AC1 ;EVALUATE 5TH ORDER POLY TO GET SMOOTH MOTION
MULF AC0,AC1 ;6.0 x T
ADDF #141160,AC1 ;-15.0
MULF AC0,AC1 ; x T
ADDF #41040,AC1 ;+10.0
MULF AC0,AC1 ;x T
MULF AC0,AC1 ;x T
MULF AC0,AC1 ;x T
SUBF #40200,AC1 ;-1.0
MULF DTH,AC1 ;TIMES CHANGE IN JOINT ANGLE
ADDF FTH,AC1 ;ADD TO FINAL SET POINT, THIS IS CURRENT ANGLE
MOV FPINDX,R0 ;GET INDEX TO JOINT SET POINTS
STF AC1,JANGLE(R0) ;SAVE NEW SET POINT
CMP PTIME,TIME1 ;CHECK IF END OF MOTION
BLT .+10 ;SKIP IF STILL MORE TO DO
BIS #CHKDNE,ARMSTT ;ELSE INDICATE CHECK FOR JOINTS IN RANGE
RTS PC
;LOCAL STORAGE AREA
FDRVLT: 0 ;MASK BITS FOR ARM JOINT DONE BIT
FDRVTM: 0 ;TIME ALLOWED FOR MOTION
FDRVSB ;SUBROUTINE TO EVALUATE POLYNOMIAL EACH 10 MSEC.
FDRVBK: 0 ;BRAKE BIT
FDRVJT: 0 ;NUMBER OF JOINT TO BE SERVOED
1 ;TOTAL NUMBER OF JOINTS TO BE SERVOED
FPINDX: 0 ;FLOATING POINT INDEX INTO JANGLE TABLE
TIME1: 0 ;DURATION OF MOTION
DTH: .BLKW 2 ;F.P. CHANGE IN JOINT ANGLE
FTH: .BLKW 2 ;F.P. FINAL JOINT ANGLE
DDRVC1: .ASCII /ERROR, INSTRUCTION FORM IS "DRIVE JT,ANGLE,TIME"/
.BYTE 15,12,0
DDRVC2: .ASCII /JOINT NUMBER OUT OF RANGE/
.BYTE 15,12,0
DDRVC3: .ASCII /CHANGE IN JOINT ANGLE OUT OF RANGE/
.BYTE 15,12,0
DDRVC4: .ASCII /DURATION OF MOTION OUT OF RANGE/
.BYTE 15,12,0
FDRVC1: .ASCII /REQUESTED JOINT MOTION OUT OF RANGE/
.BYTE 15,12,0
.EVEN
;END OF "DRIVE" ROUTINES
;"LIST" - SUBR. USED TO PRINT A LIST OF ITEMS
;THIS SUBROUTINE NEEDS A POINTER TO A LIST OF CONSTANTS IN R3 AND
;A POINTER TO THE STRING CONTAINING THE LIMITS OF THE ITEMS TO
;BE PRINTED IN SG. THE LIST IN R3 MUST BE ARRANGED AS FOLLOWS:
;
; LSTPT: MAXNUM ;MAXIMUM ITEM NUMBER FOR LIST
; FIRSTN ;FIRST PERMITTED ITEM NUMBER
; LENGTH ;BYTE LENGTH OF EACH ITEM
; STRADD ;STARTING ADDRESS OF ITEM LIST
; HEADER ;PT. TO A COLUMN HEADER STRING
; PRTSUB ;SUBROUTINE TO PRINT ITEM
;
;REGISTERS USED:
;
; SG, R3 PASS ARGUMENTS AND SG IS MODIFIED
; R0,R1,AC0,AC1 ARE GARBAGED
LIST: MOV R2,-(SP) ;SAVE REGISTERS
JSR PC,INTSCN ;PICK UP STARTING NUMBER
BCC LIST2 ;BRANCH IF NUMBER SPECIFIED
LIST1: MOV (R3),R0 ;PRINT ALL ITEMS IF NO ARGUMENTS GIVEN
MOV 2(R3),R1
BR LIST7
LIST2: MOV R0,R1 ;CHECK FOR NUMBER IN RANGE
CMP 2(R3),R0
BLE LIST4
LIST3: MOV #LSTER1,SG ;INDICATE NUMBER OUT OF RANGE
JSR PC,TYPSTR
BR LSTDNE ;EXIT
LIST4: JSR PC,CLRCMA
JSR PC,INTSCN ;PICK UP FINAL NUMBER
BCC LIST5 ;BRANCH IF SECOND ARGUMENT GIVEN
MOV R1,R0 ;ONE ARGUMENT→ PRINT ONLY ONE NUMBER
BR LIST6
LIST5: CMP R0,R1 ;CHECK #FINAL NUM≥#INITIAL NUM
BLT LIST3
LIST6: CMP (R3),R0 ;CHECK FOR FINAL NUMBER IN RANGE
BLT LIST3
LIST7: MOV R0,R2 ;COMPUTE THE NUMBER OF ITEMS TO PRINT
SUB R1,R2
INC R2
SUB 2(R3),R1 ;ADJUST FOR FIRST ITEM NUMBER
MUL 4(R3),R1 ;COMPUTE THE ABS. ADDR OF THE FIRST ITEM
MOV R1,R0 ;GET POINTER TO FIRST ITEM
ADD 6(R3),R0
MOV 10(R3),SG ;TYPE COLUMN HEADINGS
JSR PC,TYPSTR
LIST8: JSR PC,@12(R3) ;EXECUTE PRINT ROUTINE
JSR PC,HOLD ;CHECK IF TEMP. STOP OF PRINT REQUESTED
ADD 4(R3),R0 ;POINT TO NEXT ITEM
SOB R2,LIST8 ;REPEAT UNTIL DONE
LSTDNE: MOV (SP)+,R2
RTS PC
LSTER1: .ASCII /ARGUMENT OUT OF RANGE/
.BYTE 15,12,0
.EVEN
;END OF "LIST"
;"DECODE" - SUBR. TO DECODE STRING INPUT INTO INTERNAL MOTION INSTR. FORMAT
;THE STRING CONTAINING THE MOTION INSTRUCTION MUST BE POINTED TO BY
;THE SG REGISTER. IF THE STRING DECODES PROPERLY INTO A MOTION
;INSTRUCTION, REGISTER R1 IS LEFT POINTING TO THE TEMPORARY STORAGE
;LOCATION OF THE MOTION INSTRUCTION BLOCK. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #STRING,SG ;POINT TO STRING INPUT
; JSR PC,DECODE
; BCS ERROR ;TEST FOR IMPROPER INSTRUCTION
; MOV R1,STEP ;SAVE POINTER TO INSTRUCTION STEP
;
;IF THE INSTRUCTION DOES NOT DECODE PROPERLY, THE C BIT IS SET WHEN THIS
;ROUTINE RETURNS TO THE CALLING PROGRAM. OTHERWISE THE C BIT IS CLEARED.
;REGISTERS USED:
;
; R1 AND SG PASS ARGUMENTS AND ARE MODIFIED
; AC0,AC1 ARE GARBAGED
DECODE: MOV R0,-(SP) ;SAVE INSTRUCTION POINTER
MOV #IHASH,R0 ;DETERMINE INSTRUCTION NUMBER
MOV #INAMES,R1 ;POINT TO HASH TABLE AND STRING NAMES
JSR PC,IDENT
BCS DCDDNE ;EXIT IF NOT MOTION INSTRUCTION
ASL R0 ;CONVERT FUNCTION NUMBER TO WORD INDEX
MOV R0,R1
MOV #ISTEP,R0 ;CONSTRUCT NEW INSTRUCTION IN ISTEP ARRAY
JSR PC,@IDECDE(R1) ;GO DECODE INSTRUCTION, RETURNS C BIT SET IF
; COULDN'T DECODE INSTRUCTION PROPERLY
MOV #ISTEP,R1 ;RETURN ADDR. OF MOTION INSTR. STORAGE LOCATION
DCDDNE: MOV (SP)+,R0 ;RESTORE STEP POINTER
RTS PC
ISTEP: .BLKW 4 ;STORAGE AREA FOR NEW INSTRUCTION
;END OF "DECODE"
;"DRIVE" - SUBR. TO GENERATE AND CONTROL JOINT MOTIONS
;THIS ROUTINE RELEASES THE JOINT BRAKES AND OUTPUTS THE DAC VALUES FOR
;A GIVEN JOINT MOTION. "DRIVE" ALWAYS INITIATES A CLOCK INTERRUPT
;ROUTINE WHICH CAN BE USED TO MODIFY THE REQUIRED JOINT ANGLES EVERY
;10 MILLISECONDS. "DRIVE" REQUIRES AS ITS ONLY ARGUMENT A POINTER
;TO A TABLE OF MOTION VARIABLES. A DESCRIPTION OF THE MOTION
;VARIABLE TABLE FOLLOWS:
;
; DRVLST→ JTMASK ;BIT MASK TO CHECK IF ALL REQUIRED JOINTS ARE
; ; WITHIN FINAL ERROR TOLERANCE. THIS TEST IS
; ; USED TO TERMINATE A NORMAL MOTION.
; TTIME ;TOTAL PERMITTED JOINT MOTION TIME IN 10 MILLI-
; ; SECOND COUNTS. A TIME OUT ERROR IS GENERATED
; ; AFTER TTIME*10 MILLISECONDS
; MILSUB ;ADDR. OF SUBROUTINE TO BE CALLED EVERY 10 MSEC TO
; ; UPDATE THE DESIRED JOIN ANGLES.
; BRAKES ;BRAKE BITS TO BE RELEASED AND LATER SET
; FSTJT ;LOGICAL NUMBER OF FIRST JOINT TO BE SERVOED
; NUMJT ;TOTAL NUMBER OF JOINTS TO BE SERVOED
;
;ALL JOINTS STARTING WITH FSTJT AND ENDING WITH FSTJT+NUMJT-1 ARE
;SERVOED EACH 10 MILLISECONDS UNTIL EITHER THE MOTION IS COMPLETED,
;THE PANIC BUTTON IS HIT, OR A TIME OUT ERROR OCCURS. THE IN RANGE
;BITS FOR THE JOINTS THAT ARE BEING SERVICED ARE NOT CHECKED UNTIL
;THE "CHKDNE" BIT IS SET IN THE ARM STATUS WORD "ARMSTT". IT IS THE
;RESPONSIBILITY OF THE "MILSUB" SUBROUTINE TO SET THIS BIT. FOR
;MOTIONS THAT COMPUTE INCREMENTAL SET POINTS, THE "CHKDNE" BIT SHOULD
;NOT BE SET UNTIL THE LAST SET POINT IS COMPUTED.
;
;A SAMPLE CALLING SEQUENCE TO THIS ROUTINE FOLLOWS:
;
; MOV #DRVLST,R0
; JSR PC,DRIVE
;
;THIS ROUTINE DOES NOT RETURN AN ERROR CODE IN A REGISTER, BUT ANY
;MOTION ERROR CONDITIONS GENERATED DURING THE MOTION ARE INDICATED BY
;SETTING BITS IN THE ARM STATUS WORD "ARMSTT".
;REGISTERS USED:
;
; R0 PASSES ARGUMENTS AND IS ALTERED
; R1, SG AND ALL FLOATING AC'S MAY BE GARBAGED
;DEFINITIONS:
JTMASK ==0
TTIME ==2
MILSUB ==4
BRAKES ==6
FSTJT ==10
NUMJT ==12
DRIVE: MOV R2,-(SP)
MOV R0,R2 ;SAVE PTR TO DRIVE DATA
MOV R0,DRVLST
MOV #1,PTIME ;KEEP TRACK OF PRESENT TIME IN 10 MSEC COUNTS
MOV FSTJT(R2),R0 ;INITIALIZE THE DACS TO THE CURRENT POSITION
MOV NUMJT(R2),R1
JSR PC,DACOUT
MOV BRAKES(R2),R0 ;GET BRAKE BITS
JSR PC,RELBRK ;RELEASE JOINT BRAKES AND ENABLE INTERFACE
BCS DRIV1 ;BRANCH IF COULDN'T ENABLE INTERFACE
MOV #RUN,ARMSTT ;INDICATE ARM IN MOTION
CLR DNECNT ;ZERO NUMBER OF SEQUENTIAL TIME DONE BITS ON
MOV #3,CLKSET ;START CLOCK
MOV #111,CLKS
MOV #1000.,CLKSET ;SET CLOCK TO 10 MILLISECOND PERIOD
BIT #RUN,ARMSTT ;SEE IF ARM MOTION STOPPED
BNE .-6 ;WAIT TILL ARM MOTION COMPLETED ( BUSY WAIT )
DRIV1: MOV #ERRTST,R0 ;GET POINTER TO ERROR BITS AND ERROR MESSAGES
MOV NUMERR,R1 ;GET THE NUMBER OF ERRORS TO CHECK
DRIV2: BIT (R0)+,ARMSTT ;CHECK IF THIS ERROR BIT TURNED ON
BNE DRIV3 ;BRANCH IF IT IS
ADD #2,R0 ;ELSE POINT TO THE NEXT ERROR BIT
SOB R1,DRIV2 ;REPEAT FOR ALL ERROR BITS
BR DRVDNE ;EXIT IF NOT ERROR BITS
DRIV3: MOV (R0),SG ;GET ERROR MESSAGE
JSR PC,TYPSTR ;AND TYPE IT
DRVDNE: MOV (SP)+,R2
RTS PC
; [CONT. OF "DRIVE"]
;CLOCK INTERRUPT ROUTINE
CKINTR: MOV R0,-(SP) ;SAVE REGISTERS
MOV R1,-(SP)
MOV R2,-(SP)
MOV DR11S,-(SP) ;SAVE STATUS OF DR11
MOV #STTMDE,DR11S ;SET MODE TO FETCH SUBDEVICE BITS
MOV DR11I,R0 ;GET SUBDEVICE STATUS BITS
MOV (SP)+,DR11S
MOV DRVLST,R2 ;GET POINTER TO DRIVE DATA
BIT #PANICB,R0 ;CHECK IF PANIC BUTTON HIT
BNE CHKENB ;BRANCH IF PANIC BUTTON NOT ON
BIS #PANIC+CANPRO,ARMSTT ;ELSE INDICATE ERROR CONDITION
BR STPJT ;GO STOP RUNNING
CHKENB: BIT #ISON,R0 ;CHECK IF HARDWARE SERVO STILL ENABLED
BNE JTIN1 ;BRANCH IF STILL ON
BIS #NOTENB+CANPRO,ARMSTT ;ELSE INDICATE ERROR CONDITION
BR STPJT
JTIN1: BIT #CHKDNE,ARMSTT ;CHECK IF JUST WAITING FOR JOINT IN RANGE BITS
BEQ NEWSET ;BRANCH IF STILL MOVING SET POINTS
INC DNECNT ;ELSE ASSUME ALL JOINTS WITHIN FINAL RANGE
COM R0
BIT JTMASK(R2),R0 ;CHECK IF ACTUALLY IN RANGE
BEQ .+6 ;SKIP IF IN RANGE
CLR DNECNT ;ELSE ZERO IN RANGE COUNT
COM R0
CMP #3,DNECNT ;DON'T STOP TILL TEST POSITIVE 3 TIMES IN A ROW
BLE STPJT ;STOP MOTION IF IN RANGE TEST SUCCEEDS 3 TIMES
BR SETDAC ;NOT DONE YET, REFRESH THE DAC VALUES
NEWSET: JSR PC,@MILSUB(R2) ;GO UPDATE JOINT ANGLES (NEW SET POINT)
BIT #BADSOL+ADERR,ARMSTT ;CHECK IF RUN TIME ERROR OCCURRED
BNE STPJT ;BRANCH IF ERROR
SETDAC: MOV FSTJT(R2),R0 ;STILL RUNNING, OUTPUT DAC VALUES
MOV NUMJT(R2),R1
JSR PC,DACOUT
INC PTIME ;INCREMENT TIME COUNT
CMP PTIME,TTIME(R2) ;COMPARE TO TOTAL TIME PERMITTED
BLE CKCONT ;BRANCH IF STILL MORE TIME
BIS #TMEOUT+CANPRO,ARMSTT ;ELSE INDICATE TIME OUT ERROR
STPJT: JSR PC,SETBRK ;SET JOINT BRAKES AND DISABLE THE INTERFACE
CLR CLKS ;STOP SCHEDULING CLOCK ROUTINE
BIC #RUN+CHKDNE,ARMSTT ;INDICATE ARM MOTION STOPPED
CKCONT: MOV (SP)+,R2 ;RESTORE REGISTERS
MOV (SP)+,R1
MOV (SP)+,R0
RTI ;RETURN FROM INTERRUPT
;LOCAL STORAGE AREA
PTIME: 0 ;TIME INTO CURRENT JOINT MOTION ( IN 10 MILLSEC. COUNTS )
DRVLST: 0 ;POINTER TO TABLE CONTAINING MOTION VARIABLE
DNECNT: 0 ;COUNTER FOR NUMBER OF SEQUENTIAL TIMES IN RANGE TRUE
ERRTST: BADSOL ;NO ARM SOLUTION ERROR BIT AND ERROR MESSAGE
DRVC1
PANIC ;PANIC BUTTON ERROR BIT AND ERROR MESSAGE
DRVC2
TMEOUT ;TIME OUT ERROR BIT AND MESSAGE
DRVC3
ADERR ;ADC ERROR
ADCMES
NOTENB ;INTERFACE NOT ENABLED
DRVC4
NUMERR: .-ERRTST/4 ;NUMBER OF ERROR BITS TO CHECK
DRVC1: .ASCII /***ERROR***, NO ARM SOLUTION EXISTS FOR REQUESTED TRANSFORM/
.BYTE 15,12,0
DRVC2: .ASCII /***ERROR***, SOMEONE HIT THE PANIC BUTTON/
.BYTE 15,12,0
DRVC3: .ASCII /***ERROR***, FUNCTION TOOK TOO LONG TO EXECUTE/
.BYTE 15,12,0
DRVC4: .ASCII /***ERROR***, HARDWARE SERVO NOT ENABLED/
.BYTE 15,12,0
.EVEN
;END OF "DRIVE" ROUTINE
;"PVECT" - SUBR. TO PRINT VECTOR OUT ON TTY
;THE VECTOR ADDRESS MUST BE LOADED INTO R0. IF THE VECTOR IS NOT
;INITIALIZED, I.E. THAT THE FOURTH ELEMENT IS ZERO, NO PRINTING
;IS DONE. A SAMPLE CALL TO THIS ROUTINE FOLLOWS:
;
; MOV #VECT,R0 ;LOAD VECTOR ADDRESS
; JSR PC,PVECT
;
;THERE IS NO ERROR MESSAGE RETURNED
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,SG,AC0,AC1 ARE GARBAGED
PVECT: MOV R0,-(SP) ;SAVE VECTOR POINTER
TST 14(R0) ;CHECK IF VECTOR INITIALIZED
BEQ PVCDNE ;DON'T DO ANYTHING IF ITS NOT
MOV #OUTBUF,SG ;SET POINTER TO OUTPUT STRING
MOVB #126,(SG)+ ;"V"
SUB #VECLST,R0
ASH #-4,R0
MOV R0,R1
JSR PC,CVI ;CONVERT TO ASCII
MOVB #72,(SG)+ ;PUT IN A COLON
CMP #9.,R1 ;IF LESS OR EQ TO 9 NEED AN EXTRA SP CHAR.
BLT .+6
MOVB #40,(SG)+
MOV #4,R1 ;SET LOOP COUNT TO OUTPUT 4 NUMBERS
MOV (SP),R0
PVEC1: MOVB #40,(SG)+ ;PUT IN ONE SPACE CHARACTER
LDF (R0)+,AC0 ;GET FLOATING POINT NUMBER
JSR PC,CVG ;CONVERT TO ASCII
SOB R1,PVEC1 ;DO ALL FOUR NUMBERS
MOV #OUTBUF,SG ;POINT TO START OF STRING
JSR PC,TYPSTR ;OUTPUT STRING
JSR PC,CRLF
PVCDNE: MOV (SP)+,R0
RTS PC
HVECT: .ASCII / X Y Z/
.BYTE 15,12,0
.EVEN
;END OF "PVECT"
;"PTRANS" - SUBR. TO PRINT TRANSFORM OUT ON TTY
;THE TRANS ADDRESS MUST BE LOADED INTO R0. IF THE TRANSFORM IS NOT
;INITIALIZED, I.E. THAT THE 16TH ELEMENT IS ZERO, NO PRINTING IS
;DONE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #TRANS,R0 ;LOAD TRANSFORM ADDRESS
; JSR PC,PTRANS
;
;AFTER EXECUTION OF PTRANS, THE COMPUTED EULER ANGLES ARE LEFT IN
;THE ARRAY "EANGLE". THERE IS NO ERROR MESSAGE RETURNED.
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,SG,AC0,AC1,AC2,AC3,AC4 ARE GARBAGED
PTRANS: MOV R0,-(SP) ;SAVE TRANSFORM POINTER
TST 74(R0) ;CHECK FOR TRANS INITIALIZED
BEQ PTNDNE ;EXIT IF ITS NOT
MOV #EANGLE,R1 ;CONVERT TRANS TO EULER ANGLES
JSR PC,EULER
MOV #OUTBUF,SG ;SET POINTER TO OUTPUT STRING
SUB #TRNLST,R0 ;CONVERT TRANS POINTER TO NUMBER
ASH #-6,R0
CMP #MAXTRN,R0 ;CHECK IF NOT PRINTING REG. TRANS
BLT PTRA1 ;SKIP IF TRANS NOT IN TRNLST AREA
MOV R0,R1
MOVB #124,(SG)+ ;"T"
JSR PC,CVI ;CONVERT TRANS NUMBER TO ASCII
MOVB #72,(SG)+ ;PUT IN A COLON
CMP #9.,R1 ;PUT IN A EXTRA SPACE CHAR IF NOT TWO DIGITS
BLT .+6
MOVB #40,(SG)+
PTRA1: MOV #6,R1 ;SET LOOP COUNT TO OUTPUT 6 NUMBERS
MOV #EANGLE,R0 ;CONVERT EULER ANGLES
PTRA2: MOVB #40,(SG)+ ;PUT IN ONE SPACE CHARACTER
LDF (R0)+,AC0 ;GET FLOATING POINT NUMBER
JSR PC,CVF ;CONVERT TO ASCII
SOB R1,PTRA2 ;DO ALL SIX NUMBERS
MOV #OUTBUF,SG ;POINT TO START OF OUTPUT STRING
JSR PC,TYPSTR ;OUTPUT STRING
JSR PC,CRLF
PTNDNE: MOV (SP)+,R0 ;RESTORE TRANSFORM POINTER
RTS PC
HTRANS: .ASCII / X Y Z O A T/
.BYTE 15,12,0
.EVEN
;END OF "PTRANS"
;"PSTEP" - SUBR. TO PRINT MOTION INSTRUCTION OUT ON TTY
;THE INSTRUCTION STEP ADDRESS MUST BE LOADED INTO R0. IF A
;INSTRUCTION CELL IS NOT INITIALIZED, I.E. THAT THE FIRST WORD
;OF THE 4 WORD BLOCK IS ZERO, NO PRINTING IS DONE. A SAMPLE
;CALLING SEQUENCE TO THIS ROUTINE FOLLOWS:
;
; MOV #STEP,R0 ;LOAD VECTOR ADDRESS
; JSR PC,PSTEP
;
;AT THE END OF EXECUTION, "OUTBUF" IS ALWAYS LEFT WITH AT LEAST
;THE STEP NUMBER CODED IN ASC. THERE IS NO ERROR MESSAGE
;RETURNED FROM THIS ROUTINE.
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,SG,AC0,AC1 ARE GARBAGED
PSTEP: MOV R2,-(SP)
MOV R0,-(SP) ;SAVE STEP POINTER
JSR PC,LINNUM ;CONVERT STEP ADDRESS TO ASC NUMBER
MOV (R0),R2 ;GET FUNCTION NUMBER
BIC #177400,R2
DEC R2 ;CONVERT FUNCTION NUMBER TO WORD INDEX
BMI PSPDNE ;DON'T DO ANYTHING IF NO FUNCTION
ASL R2
MOV INAMES(R2),R1 ;GET POINTER TO FUNCTION NAME
MOVB (R1)+,(SG)+ ;PUT FUNCTION NAME IN OUTPUT STRING
BNE .-2
MOVB #40,-1(SG) ;PUT IN TWO SPACE CHARACTERS
MOVB #40,(SG)+
JSR PC,@IPRINT(R2) ;ADD IN THE ARGUMENTS
MOV #OUTBUF,SG ;TYPE OUT THE MOTION INSTRUCTION
JSR PC,TYPSTR
JSR PC,CRLF
PSPDNE: MOV (SP)+,R0
MOV (SP)+,R2
RTS PC
HSTEP: .BYTE 15,12,0,0
;END OF "PSTEP"
;"LINNUM" - SUBR. TO CONVERT ADDR. OF MOTION INSTRUCTION TO ASC LINE #
;THE INSTRUCTION STEP ADDRESS MUST BE LOADED INTO R0. THE ASCII
;STRING CONTAINING THE LINE NUMBER IS PUT IN OUTBUF AND SG IS LEFT
;POINTING AT A NULL CHARACTER. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STEP,R0 ;LOAD INSTRUCTION ADDRESS
; JSR PC,LINNUM
;
;THERE IS NO ERROR MESSAGE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
LINNUM: MOV R0,-(SP) ;SAVE STEP POINTER
MOV #OUTBUF,SG ;SET POINTER TO OUTPUT STRING
SUB #STPLST-10,R0 ;CONVERT STEP ADDRESS TO REL NUMBER
ASH #-3,R0
MOV R0,-(SP)
JSR PC,CVI ;CONVERT TO ASCII
MOV (SP)+,R0
MOVB #56,(SG)+ ;PUT IN A PERIOD
MOVB #40,(SG)+ ;FOLLOWED BY A SPACE CHARACTER
CMP #99.,R0 ;IF LESS OR EQ TO 99 NEED AN EXTRA SP CHAR.
BLT LINDNE
MOVB #40,(SG)+
CMP #9.,R0 ;IF LESS OR EQ TO 9 NEED AN EXTRA SP CHAR.
BLT LINDNE
MOVB #40,(SG)+
LINDNE: CLRB (SG) ;PUT IN NULL CHARACTER
MOV (SP)+,R0
RTS PC
;END OF "LINNUM"
;"GETVEC" - SUBR. TO SELECT FIRST FREE VECTOR
;SELECTS FIRST VECTOR WITH 0 FOR ITS FOURTH ELEMENT AND RETURNS THE
;VECTOR ADDRESS IN R0. A SAMPLE CALL FOLLOWS:
;
; JSR PC,GETVEC ;NO ARGUMENTS REQUIRED
; BCS ERROR ;CHECK FOR ERROR RETURN
;
;IF NO VECTORS ARE CURRENTLY FREE, THE C BIT IS SET WHEN THIS ROUTINE
;EXITS, OTHERWISE IS IT CLEARED.
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS ALTERED
; R1,SG ARE GARBAGED
GETVEC: MOV #MAXVEC,R1 ;TOTAL NUMBER OF VECTORS TO CHECK
MOV LSTVEC,R0 ;LAST VECTOR ASSIGNED
GETV1: ADD #20,R0 ;POINT TO NEXT VECTOR
CMP #VECEND,R0 ;CHECK IF BEYOND END OF LIST
BGT .+6
MOV #VECLST,R0 ;BACK TO BEGINNING IF AT END
TST 14(R0) ;CHECK IF VECTOR ASSIGNED YET
BEQ GETV2 ;BRANCH IF FOUND A FREE ONE
SOB R1,GETV1 ;CHECK ALL VECTORS
MOV #GETVER,SG ;NO FREE ONES LEFT, TYPE ERROR MESSAGE
JSR PC,TYPSTR
SEC ;SET ERROR CODE
RTS PC ;EXIT
GETV2: MOV R0,LSTVEC ;SAVE POINTER FREE VECTOR
RTS PC ;RETURN
GETVER: .ASCII /NO FREE VECTORS LEFT/
.BYTE 15,12,0
.EVEN
;END OF "GETVEC"
;"GETTRN" - SUBR. TO DECODE TRANSFORM NUMBER/SELECT FREE TRANSFORM
;TAKES THE STRING POINTERED TO BY THE SG REGISTER AND SEARCHES FOR
;A TRANSFORM NUMBER SPECIFICATION. IF A TRANSFORM NUMBER IS SPECIFIED
;A CHECK IS MADE TO INSURE THAT IT IS WITHIN RANGE AND THEN THE NUMBER
;IS CONVERTED INTO AN ABSOLUTE TRANSFORM ADDRESS. IF NO NUMBER IS
;SPECIFIED, THE FIRST FREE TRANSFORM ( I.E. THE FIRST TRANS WITH 0 FOR
;IT'S 16TH ELEMENT ) IS SELECTED AND IT'S ABSOLUTE ADDRESS IS RETURNED.
;THE TRANFORM ADDRESS IS LEFT IN R0 AND R1 IS 0 IF THE TRANS NUMBER
;WAS SPECIFIED AND NON-ZERO OTHERWISE. A SAMPLE CALL FOLLOWS:
;
; MOV #STRING,SG ;POINT TO INPUT STRING
; JSR PC,GETTRN ;NO ARGUMENTS REQUIRED
; BCS ERROR ;CHECK FOR ERROR RETURN
; TST R1 ;CHECK IF TRANS SPECIFIED
;
;IF A INVALID TRANFORM NUMBER IS SPECIFIED OR IF NO TRANSFORM IS SPECIFIED
;AND THERE ARE NO FREE TRANSFORMS LEFT, THE C BIT IS SET WHEN THIS
;ROUTINE EXITS, OTHERWISE IS IT CLEARED.
;REGISTERS USED:
;
; R0,R1,SG PASSES ARGUMENTS AND ARE ALTERED
GETTRN: JSR PC,INTSCN ;PICK UP TRANS NUMBER SPECIFIED
BCC GETT3 ;BRANCH IF TRANS NUMBER SPECIFIED
MOV #MAXTRN,R1 ;ELSE, SELECT FIRST FREE TRANS
MOV LSTTRN,R0 ;LAST TRANS ASSIGNED
GETT1: ADD #100,R0 ;POINT TO NEXT TRANS
CMP #TRNEND,R0 ;CHECK IF BEYOND END OF LIST
BGT .+6
MOV #TRNLST,R0 ;BACK TO BEGINNING IF AT END
TST 74(R0) ;CHECK IF TRANS ASSIGNED YET
BNE GETT2 ;BRANCH IF NOT FREE TRANSFORM
MOV R0,LSTTRN ;ELSE, SAVE POINTER CURRENT TRANSFORM
RTS PC ;EXIT
GETT2: SOB R1,GETT1 ;CHECK ALL TRANSFORMS
MOV #GETTC1,SG ;NO FREE ONES LEFT, TYPE ERROR MESSAGE
BR GETT5
GETT3: TST R0 ;CHECK FOR TRANS NUM>0
BLT GETT4
CMP #MAXTRN,R0 ;CHECK FOR TRANS NUM TOO LARGE
BGT GETT6
GETT4: MOV #GETTC2,SG ;OUT OF RANGE, TYPE ERROR MESSAGE
GETT5: JSR PC,TYPSTR
SEC ;SET ERROR CODE
RTS PC ;EXIT
GETT6: ASH #6,R0 ;CONVERT TO ABSOLUTE ADDRESS
ADD #TRNLST,R0
CLR R1 ;INDICATE TRANS NUMBER SPECIFIED
RTS PC ;RETURN
GETTC1: .ASCII /NO FREE TRANSFORMS LEFT/
.BYTE 15,12,0
GETTC2: .ASCII /TRANSFORM NUMBER SPECIFIED OUT OF RANGE/
.BYTE 15,12,0
.EVEN
;END OF "GETTRN"
;"SAVSTG" - SUBR. TO SAVE STRING MESSAGES IN STRING ARRAY
;THIS ROUTINE IS CALLED WHEN AN INPUT STRING MUST BE SAVED IN THE
;STRING ARRAY. A POINTER TO THE INPUT STRING IS ASSUMED TO BE IN
;REGISTER SG. THIS ROUTINE STORES THE STRING IN THE FIRST STRING
;ARRAY ELEMENT THAT BEGINS WITH A NULL CHARACTER. IF ONLY AN EMPTY
;LINE WAS TYPED IN, A ZERO IS RETURNED IN REGISTER R1 OTHERWISE, ON
;EXITING, THIS ROUTINE LEAVES THE ADDRESSES OF THE STORAGE LOCATION
;OF THE STRING IN R1. THE STRING CAN CONTAIN AT MOST 71 CHARACTERS
;AND IS STORED WITHOUT IT'S FINAL CARRIAGE RETURN. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #STRING,SG ;INPUT STRING PTR
; JSR PC,SAVSTG
; BCS ERROR ;BRANCH IF ERROR OCCURRED
; MOV R1,STGADD ;OTHERWISE SAVE STRING ADDR.
;
;THIS ROUTINE LEAVES THE C BIT SET ON EXITING IF NO FREE ELEMENTS
;IN THE STRING ARRAY WERE FOUND, ELSE THE C BIT IS CLEARED.
;REGISTERS USED:
;
; R1 AND SG PASS ARGUMENTS AND ARE ALTERED
; NO OTHER REGISTERS AFFECTED
SAVSTG: MOV R2,-(SP) ;SAVE REGISTER
CLR R1
CMPB #40,(SG)+ ;IGNOR LEADING SPACE CHARACTERS
BEQ .-4
CMPB #15,-(SG) ;CHECK IF EMPTY LINE
BEQ SAVDNE ;RETURN ZERO IF EMPTY LINE
MOV #STGLST,R1 ;ELSE CHECK THRU STRG LIST FOR 1ST EMPTY CELL
MOV #MAXSTG,R2
SAVS1: TSTB (R1) ;CHECK IF THIS IS A FREE ARRAY ELEMENT
BEQ SAVS2 ;BRANCH IF FREE
ADD #72.,R1 ;POINT TO NEXT ARRAY ELEMENT
SOB R2,SAVS1 ;CHECK ALL CELLS
MOV #SAVC1,SG ;IF NO EMPTY CELLS, INDICATE ERROR
JSR PC,TYPSTR
SEC
BR SAVDNE ;RETURN
SAVS2: MOV R1,-(SP) ;SAVE ELEMENT ADDRESS
MOV #71.,R2 ;TRANSFER AT MOST 71 CHARACTERS
SAVS3: MOVB (SG)+,(R1)+ ;TRANSFER CHARACTERS
CMPB #15,(SG) ;CHECK FOR CARRIAGE RETURN
BEQ .+4 ;STOP IF C.R. FOUND
SOB R2,SAVS3
CLRB (R1) ;PUT IN A NULL CHARACTER
MOV (SP)+,R1 ;RESTORE POINTER TO START OF SAVED STRING
SAVDNE: MOV (SP)+,R2
RTS PC
SAVC1: .ASCII /NO MORE STRING SPACE LEFT/
.BYTE 15,12,0
.EVEN
;END OF "SAVSTG"
;"MODTRN" - SUBR. TO PERMIT MODIFICATION OF EXISTING TRANSFORMS
;THIS SUBROUTINE IS CALLED TO ALLOW THE USER TO EDIT EXISTING TRANSFORMS.
;THE ONLY REQUIRED ARGUMENT TO THIS ROUTINE IS A TRANS POINTER LOADED INTO
;REGISTER R0. EDITING IS CONTINUED INDEFINITLY UNTIL THE USER RESPONSES TO
;THE QUERY "CHANGES" WITH A NULL LINE ( I.E. NO REQUESTED CHANGES ). A
;SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #TRANS,R0
; JSR PC,MODTRN
;
;THERE IS NO ERROR RETURN FROM THIS ROUTINE
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,R2,R3,SG,AC0,AC1,AC2,AC3,AC4 ARE GARBAGED
MODTRN: MOV #HTRANS,SG ;TYPE OUT THE COLUMN HEADER
JSR PC,TYPSTR
MODT1: JSR PC,PTRANS ;TYPE OUT THIS TRANSFORM
CMP #TRN1,R0 ;CHECK IF TYPING OUT RESERVED TRANSFORM
BGT MDTDNE ;DON'T PERMIT EDITING OF RESERVED TRANS
MOV #VECC1,SG ;ASK FOR CHANGES
JSR PC,TYPSTR
MOV #INBUF,SG
JSR PC,INSTR
MOV #INBUF,SG ;POINT TO START OF INPUT STRING
MOV #6,R3 ;GET LOOP COUNTER
MOV #EANGLE,R1 ;EULER ANGLES ARE STORED IN HERE
MOV #1,R2 ;KEEP TRACK OF ANY CHANGES
MODT2: JSR PC,RELSCN ;CONVERT FROM ASCII TO F.P.
BCS NOCORR ;BRANCH IF NO CORRECTION TYPED IN
STF AC0,(R1) ;CHANGE EULER ANGLE
CLR R2 ;INDICATE CHANGE MADE
NOCORR: ADD #4,R1 ;POINT TO NEXT EULER ELEMENT
JSR PC,CLRCMA ;SKIP OVER COMMA
SOB R3,MODT2 ;REPEAT FOR ALL NUMBERS
MOV #EANGLE,R1 ;CONVERT EULER ANGLES BACK TO TRANS
JSR PC,UNEUL
SOB R2,MODT1 ;REPEAT IF CORRECTIONS MADE
MDTDNE: RTS PC
;END OF "MODTRN"
;"TIMES" - SUBR. TO MULTIPLY TO TRANSFORMS TOGETHER
;THE MATRIX "T1" IS MULTIPLIED BY THE COLUMN MATRIX "T2" AND THE RESULTS ARE
;STORED IN THE COLUMN MATRIX "T3". THIS IS THE FOLLOWING OPERATION:
;
; | T3 | ← | T1 | X | T2 |
;
;IT IS ASSUMED THAT THE TRANSFORMS ARE STORED BY COLUMNS AND THAT THE LAST
;ROW OF EACH TRANSFORM IS ( 0,0,0,1 ). THE SAME TRANSFORM CAN BE GIVEN AS
;ANY TWO OF THE REQUIRED ARGUMENTS. IN FACT THE SAME TRANSFORM CAN BE GIVEN
;FOR ALL THREE ARGUMENTS ( I.E. T1 ← T1 x T1 ). A SAMPLE CALLING SEQUENCE
;FOLLOWS:
;
; MOV #T1,R0 ;LOAD ADDRESS OF TRANSFORMS
; MOV #T2,R1
; MOV #T3,R2 ;T3 ← T1 x T2
; JSR PC,TIMES
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; R0,R1,R2 PASS ARGUMENTS AND ARE MODIFIED
; AC0,AC1,AC2,AC3,AC4 ARE GARBAGED
TIMES: MOV R4,-(SP) ;SAVE REGISTERS
MOV R5,-(SP)
MOV #TIMTRN,R4 ;SAVE T1 TRANSFORM IN HERE
MOV #32.,R5 ;16. FLOATING POINT WORDS IN ALL
MOV (R0)+,(R4)+
SOB R5,.-2
MOV #10,R4 ;SET FLAG BITS TO INDICATE WHEN MULT. BY LAST COL.
COLLP: MOV #TIMTRN,R0 ;GET POINTER TO T1 TRANSFORM
LDF (R1)+,AC2 ;LOAD A COLUMN OF THE T2 MATRIX
LDF (R1)+,AC3
LDF (R1)+,AC0
STF AC0,AC4
MOV #3,R5 ;SET COUNT TO MULT. COLM OF T2 TIMES 3 ROWS OF T1
ASR R4 ;SET C BIT, 0→ COL. 1,2,OR 3 , 1→ COL. 4 OF T2
ROWM: LDF (R0)+,AC0 ;LOAD ELEMENT OF T1 ROW, FIRST COLUMN
MULF AC2,AC0 ;MULT BY T2 ELEMENT IN FIRST ROW
LDF 14(R0),AC1 ;LOAD ELEM. OF SAME T1 ROW, SECOND COLUMN
MULF AC3,AC1 ;MULT BY T2 ELEMENT, SECOND ROW
ADDF AC1,AC0 ;ADD TO PREVIOUS PRODUCT
LDF 34(R0),AC1 ;LOAD ELEM. OF SAME T1 ROW, THIRD COLUMN
MULF AC4,AC1 ;MULT BY T2 ELEMENT, THIRD ROW
ADDF AC1,AC0 ;ADD TO PREVIOUS SUMS
BCC .+6 ;SKIP IF NOT LAST COLUMN OF T2
ADDF 54(R0),AC0 ;ELSE ADD LAST ROW ELEMENT OF T1 TO PRODUCT SUM
STF AC0,(R2)+ ;SAVE RESULT IN T3 MATRIX
SOB R5,ROWM ;DO ALL 3 ROWS OF T1
CLR (R2)+ ;PUT A ZERO AS THE LAST COLUMN ELEMENT OF T3
CLR (R2)+
ADD #4,R1 ;POINT TO NEXT COLUMN OF T2
TST R4 ;CHECK IF ALL FOUR COLUMNS COMPLETED
BNE COLLP ;BRANCH IF STILL SOME MULTIPLICATION TO DO
MOV #40200,-4(R2) ;PUT 1.0 AS FINAL ELEMENT OF T3
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
RTS PC
TIMTRN: .BLKW 32. ;TEMP. STORAGE AREA FOR T1 TRANSFORM
;END OF "TIMES"
;"ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING
;JOINT ANGLES ARE READ FROM THE A/D AND CONVERTED INTO DEGREES AND
;INCHES. THE FIRST JOINT TO BE READ MUST BE SPECIFIED BY REGISTER
;R0 ( I.E. 1-6 FOR THE ARM JOINTS AND 7 FOR THE HAND ) AND THE NUMBER
;OF JOINTS TO BE READ MUST BE SPECIFIED IN R1. THE CONVERTED READINGS
;ARE LEFT IN THE ARRAY "JANGLES" IN THERE APPROPRIATE ELEMENTS ( I.E.
;JOINT #1 IN JANGLE, JOINT #2 IN JANGLE+4, ETC. ). A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #FRSTJT,R0 ;NUMBER OF FIRST JOINT TO BE READ
; MOV #JTS,R1 ;NUMBER OF JOINTS TO BE READ
; JSR PC,ANGLES ;NO ARGUMENTS REQUIRED
;
;IF THE ADC READ TAKES TOO LONG TO EXECUTE, THE C BIT IS SET AND
;"ADERR" IS INDICATED IN THE ARM STATUS WORD WHEN THIS ROUTINE EXITS,
;OTHERWISE THE C BIT IS CLEARED ON EXITING.
;REGISTERS USED:
;
; R0 AND R1 PASS ARGUMENTS AND ARE GARBAGED
; AC0,AC1 ARE GARBAGED
ANGLES: MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV DR11S,-(SP) ;SAVE DR11 STATUS REGISTER
DEC R0 ;CONVERT JOINT NUMBER TO WORD POINTER
ASL R0
MOV R0,R2 ;GET INDEXED PTR TO "JANGLE" ARRAY
ASL R2
ADD #JANGLE,R2
BIC #3+ADCENB,DR11S ;SET DR11 TO ADC READ MODE, ALLOW ARM INTERRUPTS
BIS #ADCMDE,DR11S
ADD #ACHAN,R0 ;GET INDEXED PTR INTO POT CHANNEL ARRAY
ANGL1: MOV (R0)+,DR11O ;SEND OFF A POT CHANNEL NUMBER
MOV #100.,R3 ;SET A/D CONVERSION TIME OUT COUNT
ANGL2: BIT #ADCDNE,DR11S ;CHECK IF A/D CONVERSION COMPLETED
BNE ANGL3 ;BRANCH IF COMPLETED
SOB R3,ANGL2 ;LOOP TILL TIME RUNS OUT
MOV #ADERR,ARMSTT ;INDICATE A/D READ ERROR, CAN'T PROCEED
SEC ;SIGNAL ERROR AND EXIT
BR ANGDNE
ANGL3: MOV DR11I,R4 ;GET ADC READING
LDCIF R4,AC0 ;FLOATING IT
MULF SCALE-JANGLE(R2),AC0 ;CONVERT TO DEGREES OR INCHES
ADDF OFFSET-JANGLE(R2),AC0 ;APPLY POT OFFSET
STF AC0,(R2)+ ;SAVE NEW POT READING IN "JANGLE" ARRAY
SOB R1,ANGL1 ;REPEAT FOR ALL REQUESTED JOINTS
CLC
ANGDNE: MOV (SP)+,DR11S ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
ADCMES: .ASCII /ANALOG TO DIGITAL CONVERTED NOT WORKING/
.BYTE 15,12,0
.EVEN
;END OF "ANGLES"
;"DACOUT" - SUBR. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES
;THIS ROUTINE REQUIRES TWO ARGUMENTS, THE LOGICAL NUMBER OF THE FIRST
;JOINT TO HAVE IT'S DAC OUTPUT SET AND THE NUMBER OF JOINTS TO BE
;SERVICED. ALL JOINTS FROM THE 1ST. JOINT TO (1ST JOINT -1 + NUMBER
;OF JOINTS) HAVE THEIR DAC VALUES SET. THE DAC VALUES ARE
;TAKEN FROM THE ARRAY "JANGLE" AND ARE CONVERTED TO DAC UNITS VIA
;THE CONSTANTS BOFFST AND BSCALE.
;
; MOV #FRSTJT,R0 ;NUMBER OF FIRST JOINT TO BE SERVICED
; MOV #JTS,R1 ;NUMBER OF JOINTS TO BE SERVICED
; JSR PC,DACOUT ;NO ARGUMENTS REQUIRED
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.
;REGISTERS USED:
;
; R0 AND R1 PASS ARGUMENTS AND ARE GARBAGED
; AC0,AC1 ARE GARBAGED
DACOUT: MOV R2,-(SP)
MOV R3,-(SP)
MOV DR11S,-(SP) ;SAVE DR11 STATUS REGISTER
DEC R0 ;CONVERT JOINT NUMBER TO WORD POINTER
ASL R0
MOV R0,R2 ;GET INDEXED PTR TO "JANGLE" ARRAY
ASL R2
ADD #JANGLE,R2
BIC #3,DR11S ;SET DR11 TO DAC MODE
ADD #DCHAN,R0 ;GET INDEXED PTR INTO DAC CHANNEL ARRAY
DACO1: LDF (R2)+,AC0 ;GET DESIRED JOINT ANGLE
SUBF BOFFST-JANGLE-4(R2),AC0 ;SUBT. POT OFFSET
MULF BSCALE-JANGLE-4(R2),AC0 ;CONVERT FROM DEGREES TO DAC UNITS
STCFI AC0,R3 ;CONVERT TO INTEGER
BIC #170000,R3 ;CLEAR ROOM FOR DAC CHANNEL, POS. MODE
BIS (R0)+,R3 ;ADD DAC CHANNEL
MOV R3,DR11O ;OUTPUT DAC READING AND CHANNEL NUMBER
SOB R1,DACO1 ;REPEAT FOR ALL REQUESTED JOINTS
MOV (SP)+,DR11S ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;END OF "DACOUT"
;"RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS
;THE BRAKE BITS TO BE MODIFIED MUST BE LOADED INTO REGISTER R0 BEFORE
;CALLING "RELBRK". "SETBRK" DOES NOT REQUIRE AN ARGUMENT. A SAMPLE
;SERIES OF CALLS TO THESE ROUTINES FOLLOWS:
;
; MOV #BRAKES,R0 ;SEND BRAKE BITS
; JSR PC,RELBRK
; or
; JSR PC,SETBRK
;
;THE ARM INTERFACE IS ENABLED AND DISABLED BY THESE ROUTINES AS
;NECESSARY. IF "RELBRK" IS UNABLE TO ENABLE THE HARDWARE SERVO,
;THE ERROR BIT "NOTENB" IS SET IN "ARMSTT" AND THE C BIT IS SET
;WHEN THIS ROUTINE EXITS.
;REGISTERS USED:
;
; R0 PASSES ARGUMENTS AND IS MODIFIED
; NO OTHER REGISTERS AFFECTED
RELBRK: MOV DR11S,-(SP) ;SAVE DR11 STATUS BITS
MOV #BRKMDE,DR11S ;SET BRAKE BIT MODE
BIS #ENABLE,R0 ;ENABLE INTERFACE TO COMPUTER MODE
MOV R0,DR11O ;RELEASE APPROPRIATE BRAKES
MOV #STTMDE,DR11S ;CHECK THE IF HARDWARE SERVO NOW ON
MOV DR11I,R0
CLC ;ASSUME SERVO ENABLED
BIT #ISON,R0 ;TEST IF ENABLED
BNE RELDNE ;EXIT IF EVERYTHING OK
BIS #NOTENB+CANPRO,ARMSTT ;ELSE INDICATE ERROR
SEC
RELDNE: MOV (SP)+,DR11S
RTS PC
SETBRK: MOV DR11S,-(SP) ;SAVE DR11 STATUS BITS
MOV #BRKMDE,DR11S ;SET BRAKE BIT MODE
CLR DR11O ;DISABLE ARM INTERFACE AND SET ALL BRAKES
MOV (SP)+,DR11S
RTS PC
;END OF BRAKE ROUTINES
;COMMAND HASH TABLE, CONTAINS FUNCTIONS NUMBERS OR 377 IF NOT USED
CHASH: .BYTE 377,377, 6,377, 3, 5, 7, 15
.BYTE 1,377,377,377, 10,377,377, 13
.BYTE 12, 16, 0, 11,377,377,377, 2
.BYTE 377,377,377, 4,377, 14,377,377
;COMMAND FUNCTION ADDRESS POINTER TABLE
CFUNCT: .WORD VECT, TRANS,RELTRN, HERE
.WORD WHERE, EXEC,PROCED, DONE
.WORD EDIT, LISTV, LISTT, LISTS
.WORD LISTA, CLEAR,PROCED
;COMMAND NAME STRING POINTER TABLE
CNAMES: .WORD NVECT,NTRANS,NRELTR, NHERE
.WORD NWHERE, NEXEC, NPROC, NDONE
.WORD NEDIT,NLISTV,NLISTT,NLISTS
.WORD NLISTA,NCLEAR, NP
;COMMAND NAME STRINGS
NVECT: .ASCIZ /VECT/
NTRANS: .ASCIZ /TRANS/
NRELTR: .ASCIZ /RELTRN/
NHERE: .ASCIZ /HERE/
NWHERE: .ASCIZ /WHERE/
NEXEC: .ASCIZ /EXEC/
NPROC: .ASCIZ /PROCEED/
NDONE: .ASCIZ /DONE/
NEDIT: .ASCIZ /EDIT/
NLISTV: .ASCIZ /LISTV/
NLISTT: .ASCIZ /LISTT/
NLISTS: .ASCIZ /LISTS/
NLISTA: .ASCIZ /LISTA/
NCLEAR: .ASCIZ /CLEAR/
NP: .ASCIZ /P/
.EVEN
;MOTION INSTRUCTION HASH TABLE, CONTAINS FUNCTION NUMBERS OR 377 IF NOT USED
IHASH: .BYTE 377, 377, 377, 377, 377, 377, 5, 377
.BYTE 377, 377, 377, 377, 377, 377, 0, 377
.BYTE 377, 377, 2, 377, 377, 377, 6, 1
.BYTE 377, 377, 7, 377, 377, 3, 4, 377
;ADDRESSES OF MOTION INSTRUCTION RUN-TIME ROUTINES
IFUNCT: .WORD FDRAW, FMOVE, FOPEN,FGRASP
.WORD FPAUSE, FSTOP, FGO,FDRIVE
;ADDRESSES OF ROUTINES TO DECODE MOTION INSTRUCTION STRING INPUT
IDECDE: .WORD DDRAW, DMOVE, DOPEN,DGRASP
.WORD DPAUSE, DSTOP, DGO,DDRIVE
;ADDRESSES OF SUBROUTINES TO PRINT STORED MOTION INSTRUCTIONS
IPRINT: .WORD PDRAW, PMOVE, POPEN, POPEN
.WORD PPAUSE,PPAUSE, PMOVE,PDRIVE
;MOTION INSTRUCTION STRING NAME POINTER TABLE
INAMES: .WORD NDRAW, NMOVE, NOPEN,NGRASP
.WORD NPAUSE, NSTOP, NGO,NDRIVE
;COMMAND NAME STRINGS
NDRAW: .ASCIZ /DRAW/
NMOVE: .ASCIZ /MOVE/
NOPEN: .ASCIZ /OPEN/
NGRASP: .ASCIZ /GRASP/
NPAUSE: .ASCIZ /PAUSE/
NSTOP: .ASCIZ /STOP/
NGO: .ASCIZ /GO/
NDRIVE: .ASCIZ /DRIVE/
.EVEN
;EDIT FUNCTION HASH TABLE, CONTAINS FUNCTIONS NUMBERS OR 377 IF NOT USED
EHASH: .BYTE 377, 10, 377, 377, 3, 377, 11, 377
.BYTE 377, 1, 12, 377, 7, 377, 377, 377
.BYTE 13,377, 377, 2, 5, 0, 377, 4
.BYTE 377,377, 6, 377,377, 377, 377, 377
;EDIT COMMAND FUNCTION ADDRESS POINTER TABLE
EFUNCT: .WORD INSERT,INSERT,DELETE,DELETE
.WORD SKIP, SKIP, LAST, LAST
.WORD FIND, FIND,TERMIN,TERMIN
;EDIT COMMAND NAME STRING POINTER TABLE
ENAMES: .WORD NINSER, NI,NDELET, ND
.WORD NSKIP, NS, NLAST, NL
.WORD NFIND, NF,NTERMI, NT
;EDIT COMMAND NAME STRINGS
NINSER: .ASCIZ /INSERT/
NI: .ASCIZ /I/
NDELET: .ASCIZ /DELETE/
ND: .ASCIZ /D/
NSKIP: .ASCIZ /SKIP/
NS: .ASCIZ /S/
NLAST: .ASCIZ /LAST/
NL: .ASCIZ /L/
NFIND: .ASCIZ /FIND/
NF: .ASCIZ /F/
NTERMI: .ASCIZ /TERMINATE/
NT: .ASCIZ /T/
.EVEN
;PROGRAM,TRANSFORM, AND VECTOR STORAGE AREA ( MUST BE SAVED )
LSTSTP: STPLST ;LAST PROGRAM STEP MODIFIED
STPLST: .BLKW MAXSTP*4 ;STORAGE AREA FOR PROGRAM, FOUR WORDS PER
STPEND: ; STEP
LSTTRN: TRNLST ;LAST TRANFORMED ASSIGNED
TRNLST: .WORD -37600,0 ;TRANS 0 IS THE PARK POSITION
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 40200,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD -37600,0
.WORD 0,0
.WORD 41201, 31463 ; 16.15000
.WORD 140717, 5076 ;-6.470000
.WORD 41034,121727 ; 9.790000
.WORD 40200,0
TRN1: .BLKW MAXTRN-1*40 ;FREE TRANSFORM AREA
TRNEND:
LSTVEC: VEC3 ;LAST VECTOR ASSIGNED
VECLST: .WORD 0,0,0,0,0,0 ;NULL VECTOR
.WORD 40200,0
.WORD 40200,0,0,0 ;X UNIT VECTOR
.WORD 0,0,40200,0
.WORD 0,0,40200,0 ;Y UNIT VECTOR
.WORD 0,0,40200,0
VEC3: .WORD 0,0,0,0 ;Z UNIT VECTOR
.WORD 40200,0,40200,0
VEC4: .BLKW MAXVEC-4*10 ;FREE VECTOR AREA
VECEND:
STGLST: .BLKW MAXSTG*36. ;MESSAGE STRING SPACE
STGEND:
;VARIABLE STORAGE AREA ( MUST BE SAVED )
;ARM STATUS BITS
TMEOUT ==1 ;MOTION TIME OUT, FUNCTION TOOK TOO LONG TO EXECUTE
PANIC ==2 ;PANIC BUTTON HIT
BADSOL ==4 ;INVALID ARM SOLUTION REQUESTED
BADHND ==10 ;HAND CLOSED FURTHER THAT SPECIFIED
CANPRO ==20 ;PROCEEDING PERMITTED FROM THIS ERROR CONDITION
RUN ==40 ;JOINT MOTION STILL IN PROGRESS
STPPRG ==100 ;STOP EXECUTION OF MOTION PROGRAM
ADERR ==200 ;A/D READ ERROR OCCURRED, TOOK TOO LONG TO READ
CHKDNE ==400 ;SET WHEN IN RANGE BITS ARE TO BE CHECKED DURING A MOTION
NOTENB ==1000 ;HARDWARE SERVO NOT ENABLED
ARMSTT: 0 ;ARM STATUS WORD FOR ARM STATUS BITS
INISTP: 0 ;ABS. ADDR. OF FIRST MOTION INSTRUCTION TO BE EXECUTED
FINSTP: 0 ; " " " LAST " " " " "
NXTSTP: 0 ; " " " NEXT " " " " "
LPSTP: 0 ;NUMBER OF TIMES MOTION INSTRUCTION SEQUENCE IS TO BE REPEATED
;PROGRAM CONSTANTS AND TEMPORARY STORAGE AREA ( NEED NOT BE SAVED )
FSTAT: 0 ;FLOATING POINT PROCESSOR STATUS WORD
EANGLE: .BLKW 12. ;TEMP. STORAGE FOR EULER ANGLES
JANGLE: .WORD -35714,0 ;CURRENT JOINT ANGLES
.WORD -36114,0
.WORD 41200,0
.WORD 42207,0
.WORD 41664,0
.WORD 0,0
HAND: .WORD 40300,0 ;CURRENT HAND OPENING
TMPTRN: .BLKW 40 ;TEMPORARY TRANFORM STORAGE
SMPTRN: .WORD 0,0 ;REASONABLE INITIAL ARM POSITION
.WORD 140200,0
.WORD 0,0
.WORD 0,0
.WORD 140200,0
.WORD 0,0
.WORD 0,0
.WORD 40200,0
.WORD 0,0
.WORD 0,0
.WORD 140200,0
.WORD 0,0
.WORD 41240,0
.WORD 41360,0
.WORD 40200,0
.WORD 40200,0
DEPROT: .WORD 40200,0 ;DEPROACH TRANSFORMS ( DEPART/APPROACH )
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 40200,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 40200,0
.WORD 0,0
.WORD 0,0
.WORD 0,0
.WORD 140440, 0 ;-2.500000 x A VECTOR
.WORD 40200,0
;JOINT CALIBATION DATA ( NEED NOT BE SAVED )
SCALE: .FLT2 .08466 ;SCALE FACTOR FROM ADC TO ANGLES
.FLT2 -.08547
.FLT2 -.0086289
.FLT2 -.0852541
.FLT2 .06659267
.FLT2 .0856211
.FLT2 -.001266
OFFSET: .FLT2 -228.158 ;JOINT OFFSETS
.FLT2 81.453
.FLT2 37.4381
.FLT2 56.7903
.FLT2 -131.765
.FLT2 -93.0176
.FLT2 4.4778
BSCALE: .FLT2 11.806 ;S.F. FROM JT. ANG. TO DAC UNITS
.FLT2 -11.694
.FLT2 -115.8896
.FLT2 -11.7185
.FLT2 15.0091
.FLT2 11.6619
.FLT2 -789.8894
BOFFST: .FLT2 -54.690 ;DAC OFFSET
.FLT2 -93.675
.FLT2 19.7575
.FLT2 -117.810
.FLT2 4.6167
.FLT2 82.2488
.FLT2 1.88116
LSTOP: .FLT2 -203.0 ;LOWER JOINT STOP LIMIT
.FLT2 -180.0
.FLT2 7.63
.FLT2 -265.0
.FLT2 -109.0
.FLT2 -84.0
LHAND: .FLT2 0.00
USTOP: .FLT2 90.0 ;UPPER JOINT STOP LIMIT
.FLT2 0.00
.FLT2 35.7
.FLT2 36.0
.FLT2 109.0
.FLT2 234.0
UHAND: .FLT2 3.78
ACHAN: 0 ;JOINT A/D CHANNELS
1
2
3
4
5
6
DCHAN: 00000 ;JOINT DAC CHANNELS
20000
40000
60000
100000
120000
140000
DBUF: .BLKW 200. ;DIAGNOSTIC DATA BUFFER(USED FOR DEBUGGING)
.END START