COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.TITLE DRIVE
C00006 00003	"CLKSER" - CLOCK INTERRUPT ROUTINE
C00012 00004	"ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING
C00015 00005	"TODAC"&"REFRESH" - SUBRS. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES       
C00018 00006	"RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS        
C00020 ENDMK
C⊗;
.TITLE DRIVE

;THIS ROUTINE OUTPUTS THE DAC VALUES FOR A GIVEN JOINT MOTION.  "DRIVE"
;ASSUMES THAT THE FOLLOWING THREE WORDS IN MEMORY HAVE BEEN PROPERLY
;INITIALIZED:
;
;	JTBITS	;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 5
;		;  MILLISECOND COUNTS.  A TIME OUT ERROR IS
;		;  AFTER TTIME*5 MILLISECONDS
;	RUNSUB	;ADDR. OF SUBR TO BE CALLED EVERY 20 MSEC TO
;		;  UPDATE THE JOINT SET POINTS. ALL REGISTERS
;		;  ARE AVAILABLE FOR USE, R0 ← SET PT. TIME,
;		;  R1 ← PTR TO JOINT ANGLE ARRAY. IF "STPMVE"
;		;  IS SET BY THIS ROUTINE R0 MUST CONTAIN AN
;		;  ERROR MESSAGE CODE ON EXIT.
;
;ALL JOINTS OF THE ARM JOINTS ARE SERVOED EVERY 5 MSEC. 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 CHECKED ARE NOT USED UNTIL
;AFTER THE "FINAL" BIT IS SET IN THE "ARMS".  THE "RUNSUB" ROUTINE SHOULD SET
;THE "FINAL" BIT AFTER ALL INCREMENTAL SET POINTS HAVE BEEN COMPUTED.  IN
;BETWEEN CALLS TO "RUNSUB", LINEAR INTERPOLATION IS USED TO GENERATE
;INTERMEDIATE SET POINTS. A SAMPLE CALLING SEQUENCE TO "DRIVE" FOLLOWS:
;
;		JSR	PC,DRIVE
;
;"DRIVE" INDICATES ANY ERROR CONDITIONS GENERATED DURING THE MOTION
;BY LEAVING BITS SET IN THE ARM STATUS WORD "ARMS".

;REGISTERS USED:
;	R0,R1,R2,R3 ARE GARBAGED

DRIVE:	MOV	@#MODES,R0	;SET ANY SPECIAL MODES REQUIRED
	JSR	PC,RELBRK
	MOV	#-1,@#PTIME	;PRESENT TIME IN 5 MSEC COUNTS
	CLR	@#DNECNT	;# OF SEQUENTIAL TIMES IN RANGE
	CLR	@#DRVERR	;ERROR MESSAGE CODE
	CLR	@#BADBIT	;JOINT OUT OF RANGE BITS
	BIS	#RUN,@#ARMS	;INDICATE ARM IN MOTION
AWAIT:	WAIT			;WAIT TILL MOTION COMPLETED
	BIT	#RUN,@#ARMS
	BNE	AWAIT
	MOV	@#DRVERR,R1	;ERROR MESSAGE INDEX
	BEQ	.+6
DRVMES:	JSR	PC,TYPERR	;TYPE OUT ANY NECESSARY ERROR MESS.
	MOV	@#MODES,R0	;TURN OFF SPECIAL MODES
	JSR	PC,SETBRK
	MOV	@#PMODES,@#MODES;CLEAR ALL TEMPORARY MODES
	CLR	@#JTBITS	;CLEAR TOLERANCE BITS
	RTS	PC

;END OF "DRIVE"
;"CLKSER" - CLOCK INTERRUPT ROUTINE

CLKSER:	MOV	R0,-(SP)	;SAVE ALL REGISTERS
	MOV	R1,-(SP)
	MOV	R2,-(SP)
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	@#DR11S,-(SP)

	TST	@#WAITNG	;START-UP WAIT?
	BLE	1$
	DEC	@#WAITNG	;YES, TIME TO INITIALIZE RUNNING?
	BLE	.+6
	JMP	SETDAC		;NO
	MOV	#ENABLE+ARMBRK,R0;YES, RELEASE ARM BRAKES
	JSR	PC,RELBRK

1$:	BIT	#RUN,@#ARMS	;ARM IN MOTION?
	BNE	.+6
	JMP	SETDAC		;NO JUST RE-FRESH DACS

   .IFNZ LSI
	MOV	#STTMDE,@#DR11S	;FETCH SUBDEVICE BITS
	MOV	@#DR11I,R0
   .IFF
	MOV	#-1,R0
   .ENDC

	MOV	#ABORT,R1	;HAS SOMEONE TYPED ON THE TTY?
	JSR	PC,TICKLE
	BCC	.+6
	JMP	SIGERR

	BIT	#PANIC,R0	;PANIC BUTTON HIT?
	BNE	.+6
	JMP	SIGER1

	BIT	#ISON,R0	;HARDWARE SERVO STILL ENABLED?
	BNE	.+6
	JMP	SIGER2

	INC	@#PTIME		;INCREMENT TIME COUNT
	CMP	@#PTIME,@#TTIME ;OUT OF TIME?
	BLO	2$		;NO
	MOV	@#BADBIT,R1	;GET OUT OF RANGE BITS
	SWAB	R1
	BIS	#NOTIME,R1
	JMP	SIGERR

2$:	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	4$		;NO
	CMP	@#PULTME,@#PTIME;YES, TIME TO STOP PULSING?
	BGE	3$		;NO
	MOV	#HITHND,R0	;YES
	BIC	R0,@#MODES
	JSR	PC,SETBRK
3$:	BIT	#CHKDNE,@#ARMS	;JUST WAITING FOR JOINTS TO FINISH?
	BEQ	NEWSET		;NO
	BR	SETDAC		;BY PASS STOP CHECK

4$:	BIT	#CHKDNE,@#ARMS	;WAITING FOR JOINTS TO FINISH?
	BEQ	NEWSET		;BRANCH IF STILL MOVING SET POINTS
	INC	@#DNECNT	;ASSUME JOINTS WITHIN FINAL RANGE
	MOV	@#JTBITS,R2	;IN RANGE?
	BIC	R0,R2
	BEQ	5$		;YES
	CLR	@#DNECNT	;ELSE ZERO IN RANGE COUNT
	MOV	R2,@#BADBIT	;SAVE ERROR BITS
5$:	CMP	#6,@#DNECNT	;DON'T STOP TILL IN RANGE 6 TIMES
	BGT	SETDAC		;  IN A ROW
	BR	RUNDNE		;ALL DONE, STOP MOTION

NEWSET:	MOV	#DACVAL,R0	;OLD SET POINTS
	BIT	#3,@#PTIME	;TIME TO COMPUTE NEW JOINT ANGLES?
	BEQ	CALRUN		;YES
	MOV	#ONE4TH,R1	;NO, NEW ANG← OLD ANG + 1/4 CHANGE IN ANG
	MOV	#7,R2		;7 JOINTS IN ALL
1$:	ADD	(R1)+,(R0)+
	SOB	R2,1$
	BR	SETDAC

CALRUN:	BIT	#WORKNG,@#ARMS	;RUNSUB CLOCK OVERRUN?
	BNE	SIGER3

	JSR	PC,TODAC	;DACVAL ← NEW CONVERTED DANGLES
	MOV	#DACVAL,R0	;COMPUTE INCREMENTAL CHANGE = (NEWDAC-OLDDAC)/4
	MOV	#NEWDAC,R1
	MOV	#ONE4TH,R2
	MOV	#7,R3
1$:	MOV	(R0),R4		;NEWDAC ← NEW CONVERTED ANGLES
	MOV	(R1),(R0)	;DACVAL ← OLD CONVERTED ANGLES
	MOV	R4,(R1)+
	SUB	(R0)+,R4	;1/4 CHANGE IN ANG
	ASR	R4
	ASR	R4
	ADC	R4		;ROUND UP
	MOV	R4,(R2)+	;SAVE AWAY
	SOB	R3,1$

	BIS	#STRWRK,@#ARMS	;ASSUME RUNSUB TO BE EXECUTED
	BIT	#FINAL,@#ARMS	;FINAL SET POINT?
	BEQ	SETDAC		;NO
	BIC	#STRWRK,@#ARMS	;YES, RUNSUB ALL DONE

	BIT	#ISFIN,@#ARMS	;ALL DONE INTERPOLATING?
	BEQ	2$		;NOPE
	MOV	#NOINTG,R0	;RE-ENABLE HARDWARE INTEGRATION
	JSR	PC,SETBRK
	BIS	#CHKDNE,@#ARMS	;INDICATE JUST NULLING ERRORS
	TST	@#JTBITS	;DO NULLING?
	BEQ	RUNDNE		;NO
	BIT	#NNUL,@#MODES
	BNE	RUNDNE
2$:	BIS	#ISFIN,@#ARMS

SETDAC:	JSR	PC,REFRESH	;OUTPUT DAC VALUES

	BIT	#STRWRK,@#ARMS	;START UP RUNSUB?
	BEQ	CLKDNE
	ADD	#STRWRK,@#ARMS	;INDICATE WORKING
	MOV	@#PTIME,R0	;TIME TO EVALUATE SET POINT
	ADD	#4,R0
	MOV	#DANGLES,R1	;STORE NEW ANGLES IN HERE
	MTPS	UNLOCK		;ALLOW CLOCK INTERRUPTS
	JSR	PC,@RUNSUB
	BIC	#WORKNG,@#ARMS
	BIT	#STPMVE,@#ARMS	;STOP ARM IMMEDIATELY?
	BEQ	CLKDNE		;NO
	BR	STPJT		;YES

SIGER3:	ADD	#RUNERR-NOHDWR,R1
SIGER2:	ADD	#NOHDWR-PANBUT,R1
SIGER1:	ADD	#PANBUT-ABORT,R1
SIGERR:	BIS	#CANPRO,@#ARMS	;SET ERROR BITS
	MOV	#-1,R0		;SET THE BRAKES
	JSR	PC,SETBRK
STPJT:	MOV	R1,@#DRVERR
RUNDNE:	BIC	#177400,@#ARMS

CLKDNE:	MOV	(SP)+,@#DR11S
	MOV	(SP)+,R5
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	MOV	(SP)+,R1
	MOV	(SP)+,R0
	RTI


;END OF "CLKSER" ROUTINE
;"ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING

;JOINT ANGLES ARE READ FROM THE ADC AND CONVERTED INTO DEGREES AND
;INCHES.  THE FIRST JOINT TO BE READ MUST BE SPECIFIED BY REGISTER
;R0 ( I.E. 0-12 FOR THE ARM JOINTS AND 14 FOR THE HAND ) AND THE NUMBER
;OF JOINTS TO BE READ MUST BE SPECIFIED IN R1. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
;		MOV	#FRSTJT,R0	;(FIRST JOINT - 1)*2
;		MOV	#JTS,R1		;NUMBER OF JOINTS TO BE READ
;		MOV	#JANGLES,R2	;SAVE READING IN HERE
;		JSR	PC,ANGLES	;NO ARGUMENTS REQUIRED
;
;IF THE ADC READ TAKES TOO LONG TO EXECUTE, THE C BIT IS SET, "ADERR"
;IS INDICATED IN THE ARM STATUS WORD, AND R1 CONTAINS AN ERROR CODE
;WHEN THIS ROUTINE EXITS.  OTHERWISE, THE C BIT IS CLEARED.

;REGISTERS USED:
;	R0,R1,R2  PASS ARGUMENTS AND ARE GARBAGED

ANGLES:
   .IFNZ DR11
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	BIC	#3+ADCENB,@#DR11S	;SET DR11 TO ADC MODE
	BIS	#ADCMDE,@#DR11S	
	ADD	#ACHAN,R0		;SEND OFF FIRST POT CHANNEL
	MOV	(R0)+,@#DR11O
1$:	JSR	PC,ADCRDR		;R4←ADC READING
	BCS	ANGDNE			;ERROR?
	MOV	(R0)+,@#DR11O		;START CONVERTING NEXT POT
	MOV	SCALE-ACHAN-4(R0),R5	;CONVERT TO DEG. OR INCHES
	MUL	R5,R4
	ASHC	@#K5,R4
	ROL	R5
	ADC	R4
	ADD	OFFSET-ACHAN-4(R0),R4
	MOV	R4,(R2)+
	SOB	R1,1$		;MORE POTS?
	CLC
ANGDNE:	MOV	(SP)+,R5
	MOV	(SP)+,R4
	RTS	PC
   .IFF
	CLC
	RTS	PC
   .ENDC


GETADC:	BIC	#3+ADCENB,@#DR11S	;ADC MODE
	BIS	#ADCMDE,@#DR11S
	MOV	R0,@#DR11O	;READ CHANNEL
	JSR	PC,ADCRDR
	RTS	PC


ADCRDR:	MOV	#100.,R4	;ADC TIME OUT COUNT
1$:	TSTB	@#DR11S		;ADC FINISHED?
	BMI	2$
	SOB	R4,1$		;LOOP TILL TIME RUNS OUT
	MOV	#CANPRO,@#ARMS	;INDICATE ADC ERROR
	MOV	#ADCERR,R1	;SIGNAL ERROR AND EXIT
	SEC
2$:	MOV	@#DR11I,R4	;ADC READING
	RTS	PC


;END OF "ANGLES"
;"TODAC"&"REFRESH" - SUBRS. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES       

;THESE TWO ROUTINES ARE USED FOR OUTPUTING DAC VALUES CORRESPONDING
;TO GIVEN ANGLES.  "TODAC" CONVERTS THE JOINT ANGLES IN THE DANGLE
;ARRAY INTO DAC VALUES AND RETURNS THE VALUES.  "REFRESH" SENDS
;OFF THE DAC VALUE CONTAINED IN THE DACVAL ARRAY TO THE DR11C INTERFACE.
;SAMPLE CALLING SEQUENCES FOLLOW:
;
;		MOV	#ARRAY,R0	;GET DAC VALUES IN HERE
;		JSR	PC,TODAC
;
;		JSR	PC,REFRESH	;NO ARGUMENTS
;
;NEITHER OF THESE ROUTINES RETURNS AN ERROR CODE.

;REGISTERS USED:
;	R0 PASSES ARGUMENT AND IS ALTERED
;	R1,R2,R3,R4 ARE GARBAGED

TODAC:	MOV	#7,R1		;NUMBER OF DAC VALUES TO CONVERT
	MOV	#BSCALE,R4
1$:	MOV	DANGLE-BSCALE(R4),R2	;DESIRED JOINT ANGLE
	SUB	BOFFST-BSCALE(R4),R2	; - POT OFFSET
	MOV	(R4)+,R3	;CONVERT TO DAC UNITS
	MUL	R3,R2
	ROL	R3
	ADC	R2
	MOV	R2,(R0)+	;SAVE AWAY RESULT
	SOB	R1,1$
	ASHC	@#K2,R2		;HAND VALUE GETS SHIFTED
	MOV	R2,-(R0)
	RTS	PC

REFRESH:
   .IFNZ DR11
	BIC	#3,@#DR11S	;SET DAC MODE
	MOV	#DCHAN,R0	;SEND OFF ALL DAC VALUES
	MOV	#7,R1
1$:	MOV	DACVAL-DCHAN(R0),R2	;GET A DAC VALUE
	ROR	R2
	ADC	R2
	BIC	#170000,R2	;MAKE ROOM FOR CHANNEL #
	BIS	(R0)+,R2
	MOV	R2,@#DR11O	;OUTPUT DAC VALUE AND CHANNEL
	SOB	R1,1$
	RTS	PC
   .IFF
	RTS	PC
   .ENDC

;END OF "TODAC"&"REFRESH"
;"RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS        

;THESE ROUTINES ARE USED TO SELECTIVELY TURN THE BITS IN THE BRAKE
;REGISTER ON AND OFF.  "RELBRK" SETS BITS AND "SETBRK" TURNS THEM
;OFF.  THE SOFTWARE REGISTER "BRAKES" IS USED TO KEEP TRACK OF
;THE STATE OF THE BITS IN BETWEEN CALLS TO THESE ROUTINES.  SAMPLE
;CALLING SEQUENCES TO THESE ROUTINES FOLLOWS:
;
;		MOV	#BRAKES,R0	;SEND BRAKE BITS
;		JSR	PC,RELBRK
;			or
;		JSR	PC,SETBRK

;REGISTERS USED:
;	R0 PASSES ARGUMENTS AND IS NOT MODIFIED

RELBRK:	MOV	#BRKMDE,@#DR11S	;SET BRAKE BIT MODE
	BIS	R0,@#BRAKES	;TURN ON OLD BITS + NEW BITS
	MOV	@#BRAKES,DR11O
	RTS	PC


SETBRK:	MOV	#BRKMDE,@#DR11S	;SET BRAKE BIT MODE
	BIC	R0,@#BRAKES	;TURN OFF THESE BITS
	MOV	@#BRAKES,@#DR11O
	RTS	PC
	

;END OF BRAKE ROUTINES