COMMENT ⊗   VALID 00023 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00004 00002	ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS
C00005 00003	SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM
C00007 00004		["SOLVE" - JOINT 3]
C00010 00005		["SOLVE" - JOINTS 1 & 2]  
C00013 00006		["SOLVE" - JOINT 3 REFERENCE VECTOR]
C00016 00007		["SOLVE" - JOINT 4]
C00019 00008		["SOLVE" - JOINTS 5 & 6] 
C00022 00009		["SOLVE" - CLEAN UP AND DEGENERATE CASE]
C00023 00010		["SOLVE" - LOCAL STORAGE AREA]
C00025 00011	UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES
C00027 00012		[CONTINUATION OF "UPDATE"]
C00030 00013		["UPDATE" LOCAL STORAGE AREA]
C00031 00014	SUBROUTINE "CROSS" - COMPUTES THE CROSS PRODUCT OF TWO VECTORS
C00034 00015	SUBROUTINE "SECOND" - COMPUTES THE MAGNITUDE SQUARED OF A VECTOR
C00036 00016	SUBROUTINE "ANGLE" - COMPUTES THE ANGLE BETWEEN TWO VECTORS ABOUT A THIRD VECTOR
C00039 00017		[CONTINUATION OF "ANGLE"]
C00040 00018	SUBROUTINE "MULTA" - MULTIPLIES AN "A" MATRIX BY A "T" MATRIX
C00043 00019		[CONTINUATION OF "MULTA"]
C00045 00020	SUBROUTINE "ARAY" - GENERATES THE A MATRIX FOR A GIVEN ARM LINK
C00048 00021		[CONTINUATION OF "ARAY"]
C00050 00022		["ARAY" - LOCAL STORAGE AREA]
C00052 00023	PHYSICAL CONSTANTS FOR MODEL STANFORD ARM (WESTERN ELECTRIC VERSION)
C00054 ENDMK
C⊗;
;ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS

.TITLE ARMSOL

;THE FOLLOWING PROGRAMS ARE  USED TO COMPUTE THE JOINT  ANGLES FOR THE
;SCHEINMAN  MANIPULATOR  GIVEN A  REQUIRED TRANSFORM  OR  VISE VERSA.

;THE  THEORETICAL BASIS  FOR THE  KINEMATIC  SOLUTIONS IMPLEMENTED  IN
;THESE PROGRAMS IS PRESENTED IN "MODELLING, TRAJECTORY CALCULATION AND
;SERVOING  OF A  COMPUTER  CONTROLLED  ARM"  BY  RICHARD  (LOU)  PAUL,
;STANFORD A.I. MEMO AIM-177, NOVEMBER  1972.  WHERE EVER POSSIBLE, THE
;VARIABLE  NAMES USED IN  THAT DOCUMENT AND IN  THE FOLLOWING PROGRAMS
;ARE THE SAME. 
;SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM

;GIVEN A TRANSFORM "T", THE REQUIRED JOINT ANGLES ARE DETERMINED IN DEGREES.
;IF ANY JOINT ANGLE IS OUTSIDE OF THE PERMITTED RANGE OF MOVEMENT, THE ANGLE
;RETURNED IS THE AVERAGE OF THE OLD JOINT ANGLE AND THE JOINT STOP LIMIT.
;IN THE DETERMINATION OF JOINT ANGLE 4, IF THE REQUIRED CHANGE IN ANGLE IS
;GREATER THAN 90 DEGRESS AND LESS THAN 180, THE HAND IS FLIPPED OVER AND THE
;JOINT ANGLE CHANGE BECOMES THETA = THETA-90.  ON COMPLETION OF EXECUTION, 
;REGISTER R0 CONTAINS THE NUMBER OF NON-EXACT JOINT SOLUTIONS RETURNED.  A
;SAMPLE CALLING SEQUENCE TO "SOLVE" FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDRESS OF TRANSFORM "T"
;		MOV	#THETA,R1	;LOAD POINTER TO JOINT ANGLES.  INITIALLY
;					;  THIS ARRAY SHOULD CONTAIN THE PREVIOUS ANGLES
;		JSR	PC,SOLVE	;CALLED USING PC
;		TST	R0		;CHECK FOR NUMBER OF NON-EXACT SOLUTIONS
;
;ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT REPRESENATATION.

;EXECUTION TIME:  4820 MICRO SECONDS

;REGISTERS USED:
;
;	R0,R1 PASS ARGUMENTS AND R0 IS ALTERED BY "SOLVE"
;	AC0, AC1, AC2, AC3, AC4, AND AC5 ARE GARBAGED
;	["SOLVE" - JOINT 3]

SOLVE:	CLR	EXACTS		;ASSUME EXACT SOLUTION FOUND
	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)	
	MOV	#OV1,R3		;START TRANSFERING TRANSFORM INTO OV1
	ADD	#20,R0
	MOV	#22.,R4		;TRANSFER WORDS UNTIL PV3
SBLT1:	MOV	(R0)+,(R3)+	;MOVE A WORD FROM THE TRANSFORM 
	SOB	R4,.-2		;REPEAT UNTIL DONE
	CLRF	AC0		;CLEAR F.P. ACCUM
	MOV	#10,R3		;SET COUNTER TO START WITH Z COMPONENTS
SOL1:	LDF	AV1(R3),AC1	;GET COMPONENT OF THE APPROACH VECTOR A
	MULF	S6,AC1	 	;MULT BY THE HAND LENGTH
	ADDF	SHOLDX(R3),AC1	;ADD SHOULDER ORIGIN
	SUBF	PV1(R3),AC1	;SUB. ARM POSITION IN TABLE COORDINATES
				;  NOW HAVE -LINK 3 COMPONENT OF LENGTH
	STF	AC1,W(R3)	;SAVE -W
	MULF	AC1,AC1		
	STF	AC1,W2(R3)	;SAVE W**2
	ADDF	AC1,AC0		;GET THE SUM OF THE SQUARES
	SUB	#4,R3		;POINT TO NEXT COMPONENT OF ARM POSITION
	BGE	SOL1		;DO Z,Y,X DIRECTIONS

;COMPUTE JOINT 3 POSITION

	SUBF	S22,AC0		;SUB. JOINT 2 OFFSET**2 FROM SUM OF SQUARES
	CFCC			;  NOW HAVE JOINT 3**2
	BGE	S3REAL		;SKIP IF JOINT EXTENSION REAL
	CLRF	AC0		;IF NOT, SET TO ZERO AND SKIP SQUARE ROOT
	BR	NSR
S3REAL: JSR	PC,SQRTF	;COMPUTE SQUARE ROOT TO GET JOINT 3
NSR:	CMPF	STOP3+4,AC0	;COMPARE TO MAX. JOINT EXTENSION
	CFCC
	BGE	S3LE		;BRANCH IF LESS THAN MAX
	LDF	STOP3+4,AC0	;ELSE SUBSTITUE MAX ALLOWABLE EXT.
	BR	M3
S3LE:	CMPF	STOP3,AC0	;COMPARE TO MIN. JOINT EXTENSION
	CFCC
	BLE	S3OK		;BRANCH IF IN RANGE
	LDF	STOP3,AC0	;ELSE SUBSTITUTE MIN LENGTH
M3:	INC	EXACTS		;INDICATE EXACT SOLUTION NOT FOUND
	ADDF	10(R1),AC0	;ADD THE PREVIOUS JOINT ANGLE TO THE STOP LIMIT
	MULF	#40000,AC0	;TAKE THE AVERAGE OF THE TWO ANGLES
S3OK:	STF	AC0,10(R1)	;SEND BACK JOINT 3
;	["SOLVE" - JOINTS 1 & 2]  

;COMPUTE JOINT 1 ANGLE

	LDF	W2+4,AC0	;LOAD W(2)**2
	ADDF	W2,AC0		; + W(1)**2
	JSR	PC,SQRTF	;COMPUTE ( W(1)**2 + W(2)**2 ) **.5
	LDF	S2,AC1		;GET THE JOINT 2 OFFSET
	DIVF	AC0,AC1		;NOW HAVE SIN THETA= S2/(W(1)**2+W(2)**2)**.5
	STF	AC1,AC0
	JSR	PC,ASIN		;GET THETA
	STF	AC0,AC4		;SAVE TEMP.
	LDF	W+4,AC0		;LOAD -W(2)
	LDF	W,AC1		;LOAD -W(1)
	JSR	PC,ATAN2	;COMPUTE PHI = ATAN -W(2)/(-W(1))
	ADDF	AC4,AC0		;JOINT 1 = PHI +THETA
	MULF	RADTOD,AC0	;CONVERT FROM RADIANS TO DEGREES
	STF	AC0,AC1
    	SUBF	STOP1+4,AC1	;CHECK IF GREATER THAN MAX STOP LIMIT
	CFCC
	BMI	J1N		;SKIP IF LESS
	SUBF	C360,AC0	;ELSE SUBTRACT 360 DEG
J1N:	CMPF	STOP1+4,AC0	;COMPARE ANGLE TO MAX STOP LIMIT
	CFCC
	BGE	J1LE		;BRANCH IF LESS THAN MAX
	LDF	STOP1+4,AC0	;ELSE REPLACE ANGLE BY STOP
	BR	M1
J1LE:	CMPF	STOP1,AC0	;COMPARE TO MIN STOP LIMIT
	CFCC
	BLE	J1OK		;BRANCH IF WITHIN STOP LIMITS
	LDF	STOP1,AC0	;ELSE REPLACE ANGLE BY MIN STOP
M1:	INC	EXACTS		;INDICATE NO EXACT SOLUTION
	ADDF	(R1),AC0	;ADD PREVIOUS ANGLE TO STOP LIMIT
	MULF	#40000,AC0	;TAKE ANGLE HALF WAY BETWEEN THE TWO
J1OK:	STF	AC0,(R1)	;SEND OFF NEW JOINT 1 ANGLE

;COMPUTE JOINT 2 ANGLE

	LDF	W+10,AC0	;GET -W(3)
	NEGF	AC0		;NOW HAVE +W(3)
	DIVF	10(R1),AC0	;DIVID BY NEW LENGTH OF JOINT 3
	STF	AC0,AC4		;SAVE THE COS JOINT 2 = W(3)/S3
	JSR	PC,ACOS		;GET ANGLE FOR JOINT 2
	MULF	NRADTD,AC0	;CONVERT FROM RADIANS TO DEGREES AND NEGATE
	STF	AC0,4(R1)	;SEND NEW JOINT 2 ANGLE.  NO STOP LIMIT
				;  CHECKING SINCE ALL SOLUTIONS WITHIN 0 TO Pi
;	["SOLVE" - JOINT 3 REFERENCE VECTOR]

;FORM THE UNIT VECTORS AT END OF LINK 3

	JSR	PC,SNCOSD	;COMPUTE THE SIN/COS FOR THE NEW JOINT 2
	STF	AC0,AC5        	;ONLY NEED THE SINE, SAVE FOR LATER
	NEGF	AC0		;GET - SIN J2
	STF	AC0,Y3+10	;Z-COMPONENT OF Y(3) VECTOR IN TABLE COORDINATES
	LDF	(R1),AC0	;COMPUTE THE SIN/COS FOR THE NEW JOINT 1
	JSR	PC,SNCOSD
	STF	AC0,AC2  	;SAVE SINE AND COSINE J1
	STF	AC1,AC3  
	MULF	AC4,AC1		;COMPUTE COS J2*COS J1
	STF	AC1,Y3		;X-COMPONENT OF Y(3) VECTOR
	MULF	AC4,AC0		;COMPUTE COS J2*SIN J1
	STF	AC0,Y3+4	;Y-COMPONENT OF Y(3) 
	MULF	AC5,AC3		;COMPUTE SIN J2*COS J1
	STF	AC3,Z3		;X-COMPONENT OF Z(3) VECTOR IN TABLE COORDINATES
	MULF	AC5,AC2		;COMPUTE SIN J2*SIN J1
	STF	AC2,Z3+4	;Y-COMPONENT OF Z(3)
	LDF	AC4,AC0		;GET COS J2
	STF	AC0,Z3+10	;Z-COMPONENT OF Z(3)

;COMPUTE "RR" VECTOR, PERPENDICULAR TO PLANE OF LINK 3 AND HAND, AND
;CHECK FOR DEGENERATE CONFIGURATION, RR APPROX. 0 → HAND AND LINK 3 IN LINE

	MOV	#RR,R0		;COMPUTE RR ← Z(3) CROSS Z(6)
	MOV	#Z3,R3		
	MOV	#AV1,R4		;AV1 IS THE SAME AS Z(6)
	JSR	PC,CROSS	;GET CROSS PRODUCT
	MOV	#RR,R0		;GET THE SQUARE THE MAGNITUDE OF RR
	JSR	PC,SECOND
	CMPF	MINANG,AC0	;CHECK IF DEGENERATE CASE → Z(3) IN LINE WITH 
				;  Z(6) → |RR|**2 SMALL
	CFCC
	BLE	NODEGN		;SKIP IF NOT DEGENERATE CASE
	JMP	DEGEN		
NODEGN:	JSR	PC,SQRTF	;ELSE GET MAGNITUDE OF RR = |RR|
	MOV	#RR,R0		;CONVERT RR TO UNIT VECTOR
	MOV	#3,R3		;DIVID ALL THREE COMPONENTS BY MAGNITUDE
UNITRR:	LDF	(R0),AC1	;LOAD A COMPONENT OF RR
	DIVF	AC0,AC1		;DIVID BY MAGNITUDE
	STF	AC1,(R0)+	;PUT BACK IN ARRAY
	SOB	R3,UNITRR	;REPEAT 3 TIMES
;	["SOLVE" - JOINT 4]

;COMPUTE JOINT ANGLE 4

	MOV	#RR,R3		;JT 4 ANGLE IS JUST THE ANGLE BETWEEN RR AND Y(3)
	MOV	#Y3,R4	
	MOV	#Z3,R0		;JT 4 MUST BE DIRECTED ABOUTED Z(3)
	JSR	PC,ANGLE	;COMPUTE ANGLE
	CMPF	STOP4+4,AC0	;IF GREATER THAN MAX SUBT 360 DEGREES
	BGT	.+6
	SUBF	C360,AC0
	STF	AC0,AC1		;GET ANGLE INTO AC1
	SUBF	14(R1),AC1	;GET DIFFERENCE BETWEEN OLD ANGLE AND NEW
	CMPF	C180,AC1	;CHECK IF DIFFERENCE GREATER THAN 180 DEGRESS
	CFCC
	BGE	JT4LTP		;BRANCH IF LESS THAN 180
	SUBF	C360,AC0	;ELSE SUBT. 360 DEG - GO THE SHORTEST DIRECTION
	SUBF	C360,AC1
	BR	TN
JT4LTP:	CMPF	CM180,AC1	;CHECK IF DIFF. GREATER THAN -180 DEGREES
	CFCC
	BLE	TN		;BRANCH IF GREATER THAN -180 DEG
	ADDF	C360,AC0	;ELSE ADD 360 DEG - GO THE SHORTEST DIRECTION
	ADDF	C360,AC1
TN:	CMPF	C90,AC1		;CHECK IF DIFF. LESS THAN 90 DEG
	CFCC
	BGE	JT4LP2		;SKIP IF IT IS
	SUBF	C180,AC0	;ELSE GO THE OTHER DIRECTION AND FLIP HAND
	BR	FLIPJ4
JT4LP2:	CMPF	CM90,AC1	;CHECK IF DIFF. GREATER THAN -90 DEG
	CFCC
	BLE	NOFLIP		;SKIP IF IT IS
	ADDF	C180,AC0	;ELSE GO THE OTHER DIRECTION
FLIPJ4:	MOV	#100000,R0	;FLIP HAND OVER
	XOR	R0,RR		;COMPLEMENT THE THREE COMPONENTS OF VECTOR RR
	XOR	R0,RR+4
	XOR	R0,RR+10
NOFLIP:	CMPF	STOP4+4,AC0	;COMPARE THE ANGLE TO THE MAX. STOP LIMIT FOR JT 4
	CFCC
	BGE	JT4LE		;BRANCH IF LESS THAN MAX
	LDF	STOP4+4,AC0	;ELSE REPLACE ANGLE BY STOP LIMIT
	BR 	M4
JT4LE:	CMPF	STOP4,AC0	;COMPARE THE ANGLE TO THE MIN STOP LIMIT
	CFCC
	BLE	JT4OK		;BRANCH IF GREATER THAN MIN
	LDF	STOP4,AC0	;ELSE REPLACE ANGLE BY STOP LIMIT
M4:	INC	EXACTS		;INDICATE NO EXACT SOLUTION
	ADDF	14(R1),AC0	;ADD THE OLD JOINT ANGLE TO THE STOP LIMIT
	MULF	#40000,AC0	;TAKE THE AVERAGE OF THE TWO VALUES
JT4OK:	STF	AC0,14(R1)	;SEND BACK THE NEW JOINT ANGLE
;	["SOLVE" - JOINTS 5 & 6] 

;COMPUTE JOINT 5 ANGLE

	MOV	#AV1,R3		;JT 5 ANGLE IS JUST THE ANGLE BETWEEN Z(6) AND Z(3)
	MOV	#Z3,R4	
	MOV	#RR,R0		;JT 5 MUST BE DIRECTED ABOUTED RR  
	JSR	PC,ANGLE	;COMPUTE ANGLE
	CMPF	STOP5+4,AC0	;COMPARE ANGLE TO MAX STOP LIMIT OF JOINT 5
	CFCC
	BGE	JT5LE		;BRANCH IF ANGLE LESS THAN STOP LIMIT
	LDF	STOP5+4,AC0	;ELSE REPLACE ANGLE BY STOP LIMIT
	BR	M5
JT5LE:	CMPF	STOP5,AC0	;COMPARE ANGLE TO MIN STOP LIMIT
	CFCC
	BLE	JT5OK		;BRANCH IF ANGLE IN RANGE
	LDF	STOP5,AC0	;ELSE REPLACE ANGLE BY MIN STOP LIMIT
M5:	INC	EXACTS		;INDICATE NO EXACT SOLUTION FOUND
	ADDF	20(R1),AC0	;ADD PREVIOUS ANGLE TO STOP LIMIT
	MULF	#40000,AC0	;TAKE AVERAGE VALUE
JT5OK:	STF	AC0,20(R1)	;SEND BACK ANGLE

;COMPUTE JOINT 6 ANGLE

	MOV	#OV1,R3		;JT 6 ANGLE IS THE ANGLE BETWEEN ORIENT. VECT AND RR
	MOV	#RR,R4
	MOV	#AV1,R0		;DIRECTED ABOUT Z(6)
	JSR	PC,ANGLE	;COMPUTE ANGLE
TOL:	STF	AC0,AC1		;GET ANGLE
	SUBF	24(R1),AC1	;GET DIFFERENCE BETWEEN NEW ANGLE AND OLD JOINT 6
	CMPF	C180,AC1	;CHECK IF DIFFERENCE GREATER THAN 180 DEG
	CFCC
	BGE	JT5LTP		;BRANCH IF LESS THAN Pi
	SUBF	C360,AC0	;ELSE SUBT. 360 DEG - GO THE SHORTEST DIRECTION
	BR	ST6
JT5LTP:	CMPF	CM180,AC1	;CHECK IF DIFFERENCE LESS THAN -180 DEG
	CFCC
	BLE	ST6		;BRANCH IF GREATER THAN -180
	ADDF	C360,AC0	;ELSE ADD 2Pi - GO THE SHORTEST DIRECTION
ST6:	CMPF	STOP6+4,AC0	;CHECK IF ANGLE LESS THAN MAX STOP LIMIT
	CFCC
	BGE	JT6LE		;SKIP IF ANGLE LESS THAN MAX STOP
	LDF	STOP6+4,AC0	;ELSE REPLACE ANGLE BY STOP LIMIT
	BR	M6
JT6LE:	CMPF	STOP6,AC0	;CHECK IF ANGLE GREATER THAN MIN STOP LIMIT
	CFCC
	BLE	JT6OK		;BRANCH IF IN RANGE
	LDF	STOP6,AC0	;ELSE REPLACE ANGLE BY MIN STOP LIMIT
M6:	INC	EXACTS		;INDICATE NO EXACT SOLUTION FOUND
	ADDF	24(R1),AC0	;ADD PREVIOUS JOINT 6 ANGLE TO STOP LIMIT
	MULF	#40000,AC0	;TAKE AVERAGE VALUE
JT6OK:	STF	AC0,24(R1)	;SEND BACK ANGLE
;	["SOLVE" - CLEAN UP AND DEGENERATE CASE]

;ALL DONE COMPUTING ANGLES, CLEAN UP

	MOV	EXACTS,R0	;LEAVE EXACT SOLUTION FLAG IN R0
	MOV	(SP)+,R4	;RESTORE REGISTERS
	MOV	(SP)+,R3
	MOV	(SP)+,R2	
	RTS	PC		;RETURN


;HERES WHAT WE DO WITH DEGENERATE ANGLES FOR JOINTS 4 AND 6

DEGEN:	CLR	20(R1)		;SEND BACK JOINT 5 EQUAL TO ZERO
	CLR	22(R1)
	MOV	#OV1,R3		;JT 6 IS THE ANGLE BETWEEN THE ORIEN. VECT AND Y(3)
	MOV	#Y3,R4
	MOV	#AV1,R0		;DIRECTED ABOUT Z(6)
	JSR	PC,ANGLE	;COMPUTE ANGLE
	SUBF	14(R1),AC0	;SUBTRACT CONTRIBUTION OF JOINT 4
	JMP	TOL		;LEAVE JOINT 4 AS IT IS AND PROCEED TO JOINT 6 COMPU


;END OF "SOLVE"
;	["SOLVE" - LOCAL STORAGE AREA]

OV1:	.BLKW	10		;ARRAY FOR ORIENTATION VECTOR "O"
AV1:	.BLKW	10		;ARRAY FOR APPROACH VECTOR "A"
PV1:	.BLKW	6		;ARRAY FOR POSITION VECTOR "P"

W:	.BLKW	6		;THREE COMPONENTS OF "W"
W2:	.BLKW	6		;"W**2"

Y3:	.BLKW	6		;UNIT VECTOR IN Y DIR. OF JOINT 3 IN TABLE COORDIN.
Z3:	.BLKW	6		; "      "    " Z  "    "   "   "  "   "      "
RR:	.BLKW	6		;VECTOR CROSS PRODUCT OF Z3 AND Z6

EXACTS:	0			;COUNT OF NON-EXACT SOLUTION JOINTS

MINANG:	.WORD	33206,33675	;MINIMUM JOINT 5 ANGLE WITHOUT BEING DEGENERATE
				;  CASE = 0.000001
C360:	.WORD	 42264,     0	; 360.0000    
C90:	.WORD	 41664,     0	; 90.00000    
CM90:	.WORD	141664,     0	;-90.00000    
C180:	.WORD	 42064,     0	; 180.0000    
CM180:	.WORD	142064,     0	;-180.0000    
RADTOD:	.WORD    41545,27341	;CONVERSION FROM RADIANS TO DEGREES
NRADTD:	.WORD   141545,27341	;- RADTOD
;UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES

;GIVEN THE JOINT ANGLES "THETA", THE RESULTING HAND POSITION AND ORIENTATION ARE
;DETERMINED AND STORED IN A GIVEN TRANSFORM "T".  THE TRANSFORM IS A 4 BY 4 
;MATRIX STORED BY COLUMNS WITH EACH VALUE REPRESENTED IN TABLE COORDINATES.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD TRANSFORM ADDRESS IN R0
;		MOV	#THETA,R1	;LOAD ADDRESS OF JOINT ANGLE LIST         
;		JSR	PC,UPDATE	;CALLED USING PC
;
;THE TRANSFORM WILL BE OF THE FOLLOWING FORM:
;
;		|  T1   T5   T9   T13  |
;		|  T2   T6   T10  T14  |
;		|  T3   T7   T11  T15  |
;		|   0    0    0   1.0  |
;
;ALL OF THE NUMBERS MUST BE IN SINGLE PRECISION FLOATING POINT REPRESENTATION.
;ALL ANGLES ARE IN DEGREES

;EXECUTION TIME:  5700 MICRO SECONDS

;REGISTERS USED:     
;
;	R0 - T TRANSFORM/LINK NUMBER
;	R1 - THETA LIST/ARM CONSTANTS LIST POINTER
;	AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED

;DEFINITIONS:

LINK==%0


;START OF EXECUTABLE CODE

UPDATE:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	R0,-(SP)	;SAVE T TRANSFORM ADDRESS
	MOV	#12.,R4		;TRANSFER THE ANGLES
	MOV	#THET,R3	;PUT THEM IN THET
	MOV	(R1)+,(R3)+	;MOVE A HALF OF A F.P. NUMBER
	SOB	R4,.-2		;DO 12 TIMES
;	[CONTINUATION OF "UPDATE"]

	MOV	#10.,LINK	;START WORK AT LINK 6
	JSR	PC,ARAY   	;COMPUTE "A" MATRIX
	MOV	#8.,LINK	;NOW AT LINK 5
	MOV	#30.,R3		;TRANSFER A6 TO TN MATRIX
	MOV	#AM1,R4
	MOV	#TN1,R5
	MOV	(R4)+,(R5)+	;TRANSFER ONE NUMBER
	SOB	R3,.-2		;DONT STOP TILL FIRST 15 TERMS DONE
	MOV	#TN1,R5		;FIRST MULT. T ← A5 x TN
	MOV	(SP),R4
HLLP: 	JSR	PC,ARAY   	;COMPUTE "A" MATRIX  FOR NEXT LINK
	MOV	#3,COL4		;SET COLUMN FOUR NOT TRUE, START AT COL 1
MUT:	JSR	PC,MULTA	;MULTIPLY "A" BY A COLUMN OF T1
	ADD	#4,R4		;POINT TO THE NEXT COLUMN OF T2 MATRIX
	ADD	#4,R5		;  "   "   "    "    "    "  T1   "
	DEC	COL4		;DO ALL FOUR COLUMNS
	BPL	MUT		;REPEAT IF NOT DONE         
	MOV	#-100,R2	;SET T1 AND T2 TO POINT TO COLUMN 1 AGAIN
        ADD     R2,R4
        ADD	R5,R2
	MOV	R4,R5		;REVERSE THE DIRECTION OF MULTIPLICATION BETWEEN
	MOV	R2,R4		;  TN AND T
	SUB	#2,LINK		;POINT TO NEXT LINK
	BGE	HLLP		;DO ALL 6 LINKS
	MOV	(SP)+,R0	;GET THE ADDRESS OF THE "T" TRANSFORM AGAIN
 	LDF	60(R0),AC0	;ADJUST X,Y,Z FOR ORIGIN OF TABLE
 	ADDF	SHOLDX,AC0	;ADD IN X LOCATION OF JOINT 1
 	STF	AC0,60(R0)
 	LDF	64(R0),AC0
 	ADDF	SHOLDY,AC0	;ADD IN Y LOCATION OF JOINT 1
 	STF	AC0,64(R0)
	CLR	14(R0)		;COMPLETE INITIALIZATION OF TRANSFORM
	CLR	16(R0)
	CLR	34(R0)
	CLR	36(R0)
	CLR	54(R0)
	CLR	56(R0)
	MOV	#40200,74(R0)
	CLR	76(R0)
	MOV	(SP)+,R5	;RESTORE THE REGISTERS
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC		;EXIT


;END OF "UPDATE"
;	["UPDATE" LOCAL STORAGE AREA]

;JOINT ANGLES

THET:	.WORD  	0,0,0,0
THET3:	.WORD	0,0,0,0,0,0,0,0

;TEMPORARY "T" MATRICES

TN1:	.WORD	0,0,0,0,0,0,0,0
     	.WORD	0,0,0,0,0,0,0,0
    	.WORD	0,0,0,0,0,0,0,0
     	.WORD	0,0,0,0
TN15:	.WORD	0,0
	.WORD	40200,0

TM1:	.WORD	0,0,0,0,0,0,0,0
	.WORD	0,0,0,0,0,0,0,0
	.WORD	0,0,0,0,0,0,0,0
	.WORD	0,0,0,0,0,0
	.WORD	40200,0
;SUBROUTINE "CROSS" - COMPUTES THE CROSS PRODUCT OF TWO VECTORS

;COMPUTES THE CROSS PRODUCT OF "A x B" AND RETURNS THE RESULT IN "C".  A,B,
;AND C MUST BE ARRAYS CONTAINING THREE ELEMENTS.  THE FOLLOWING OPERATION IS
;CARRIED OUT:
;
;	C(1) ← A(2)B(3) - A(3)B(2)
;	C(2) ← A(3)B(1) - A(1)B(3)
;	C(3) ← A(1)B(2) - A(2)B(1)
;
;A SAMPLE CALL FOLLOWS:
;
;	MOV	#C,R0		;LOAD ADDRESS OF C(1) INTO R0
;	MOV	#A,R3		; "      "    "  A(1)   "  R3
; 	MOV 	#B,R4		; "      "    "  B(1)   "  R4
;	JSR	PC,CROSS	;CALLED USING PC
;
;ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT REPRESENTATION.

;REGISTERS USED:
;
;	R0,R3,R4  PASS ARGUMENTS AND ARE ALTERED DURING EXECUTION
;	AC0,AC1,AC2,AC3 GARBAGED


;START OF EXECUTABLE CODE

CROSS:	LDF	(R3)+,AC0	;LOAD A(1)
	LDF	(R3)+,AC1	;LOAD A(2)
	LDF	(R3),AC2	;LOAD A(3)
	MULF	(R4),AC1	;A(2)B(1)
	MULF	(R4)+,AC2	;A(3)B(1)
	LDF	(R4),AC3	;LOAD B(2)
	MULF	AC0,AC3		;A(1)B(2)
	SUBF	AC1,AC3		;C(3) = A(1)B(2)-A(2)B(1)
	LDF	(R4)+,AC1	;LOAD B(2)
	MULF	(R3),AC1	;A(3)B(2)
	MULF	(R4),AC0	;A(1)B(3)
	SUBF	AC0,AC2		;C(2) = A(3)B(1)-A(1)B(3)
	LDF	(R4),AC0	;LOAD B(3)
	MULF	-(R3),AC0	;A(2)B(3)
	SUBF	AC1,AC0		;C(1) = A(2)B(3)-A(3)B(2)
	STF	AC0,(R0)+	;SAVE C(1)
	STF	AC2,(R0)+	;SAVE C(2)
	STF	AC3,(R0)	;SAVE C(3)
	RTS	PC		;RETURN

;END OF "CROSS"
;SUBROUTINE "SECOND" - COMPUTES THE MAGNITUDE SQUARED OF A VECTOR

;THIS PERFORMS THE FUNCTION OF TAKING THE DOT PRODUCT OF A VECTOR WITH
;ITSELF.  A SAMPLE CALL FOLLOWS:
;
;	MOV	#A,R0		;LOAD ADDRESS OF VECTOR INTO R0
;	JSR	PC,SECOND	;CALLED USING THE PC
;
;THE VECTOR IS ASSUMED TO BE THREE DIMENSIONAL WITH ALL TERMS STORED
;IN SINGLE PRECISION FLOATING POINT REPRESENTATION.  THE RESULT IS
;RETURNED IN AC0.

;REGISTERS USED:
;
;	R0, AC0  PASS ARGUMENTS AND ARE ALTERED DURING EXECUTION
;	AC1 IS GARBAGED


;START OF EXECUTABLE CODE

SECOND:	LDF	(R0)+,AC0	;GET FIRST ELEMENT
	MULF	AC0,AC0		;SQUARE VALUE
	LDF	(R0)+,AC1	;GET SECOND ELEMENT
	MULF	AC1,AC1		;SQUARE VALUE
	ADDF	AC1,AC0		;GET SUM OF SQUARES
	LDF	(R0),AC1	;GET LAST ELEMENT
	MULF	AC1,AC1		;SQUARE VALUE
	ADDF	AC1,AC0		;NOW HAVE SUM OF SQUARES IN AC0
	RTS	PC		;RETURN

;END OF "SECOND"
;SUBROUTINE "ANGLE" - COMPUTES THE ANGLE BETWEEN TWO VECTORS ABOUT A THIRD VECTOR

;THIS ROUTINE FIRST USES THE DIRECTION COSINES OF TWO UNIT VECTORS (X AND Y) TO
;COMPUTE THE ANGLE BETWEEN THE VECTORS, i.e. IT TAKES THE DOT PRODUCT OF X
;AND Y.  IT THEN COMPUTES THE CROSS PRODUCT OF X AND Y ABOUT Z TO DETERMINE
;THE SIGN OF THE ANGLE.  THE IMPLEMENTED EQUATIONS ARE AS FOLLOWS:
;
;	ABS(ANGLE) ← ACOS( X DOT Y )
;		   ← ACOS( X(1)Y(1) + X(2)Y(2) + X(3)Y(3) )
;
;			  | X(1)  X(2)  X(3) |
;	SIGN(ANGLE)← -DET | Y(1)  Y(2)  Y(3) |
;			  | Z(1)  Z(2)  Z(3) |
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;	MOV	#X,R3		;LOAD ADDRESS OF X ARRAY INTO R3
;	MOV	#Y,R4		;  "     "    "  Y   "     "  "
;	MOV	#Z,R0		;  "     "    "  Z   "     "  "
;	JSR	PC,ANGLE	;CALLED USING PC
;
;EACH VECTOR IS ASSUMED TO BE THREE DIMENSIONAL WITH ALL TERMS STORED
;IN SINGLE PRECISION FLOATING POINT REPRESENTATION.  THE RESULTING ANGLE
;IS RETURNED IN AC0 IN DEGREES.

;REGISTERS USED:
;
;	R0, R3, R4, AC0  PASS ARGUMENTS AND ARE ALTERED DURING EXECUTION
;	AC1, AC2, AC3 ARE GARBAGED


;START OF EXECUTABLE CODE

ANGLE:	MOV	R2,-(SP)	;SAVE REGISTER
	CLRF	AC0		
	MOV	#3,R2		;SET LOOP COUNTER TO COMPUTE DOT PRODUCT
DOTLP:	LDF	(R3)+,AC1	;LOAD X COMPONENT
	MULF	(R4)+,AC1	;MULT BY CORRESPONDING Y COMPONENT
	ADDF	AC1,AC0		;SUM DIRECTION COSINES
	SOB	R2,DOTLP	;REPEAT FOR 3 DIRECTIONS
	JSR	PC,ACOS		;GET ANGLE IN RADIANS 
	MULF	RADTOD,AC0	;CONVERT ANGLE TO DEGREES
	LDF	(R0)+,AC1	;LOAD Z(1)
	MULF	-10(R4),AC1	;x Y(2)
	LDF	(R0)+,AC2	;LOAD Z(2)
	MULF	-14(R4),AC2	;x Y(1)
	SUBF	AC1,AC2		;NOW HAVE Y(1)Z(2)-Y(2)Z(1)
	MULF	-(R3),AC2	;x X(3)
;	[CONTINUATION OF "ANGLE"]

	LDF	-(R4),AC1	;LOAD Y(3)
	MULF	-10(R0),AC1	;x Z(1)
	LDF	-10(R4),AC3	;LOAD Y(1)
	MULF	(R0),AC3	;x Z(3)
	SUBF	AC3,AC1		;NOW HAVE Y(3)Z(1)-Y(1)Z(3)
	MULF	-(R3),AC1	;x X(2)
	ADDF	AC2,AC1		;ADD FIRST 6 TERMS
	LDF	-(R4),AC2	;LOAD Y(2)
	MULF	(R0),AC2	;x Z(3)
	LDF	-(R0),AC3	;LOAD Z(2)
	MULF	4(R4),AC3	;x Y(3)
	SUBF	AC3,AC2		;NOW HAVE Y(2)Z(3)-Y(3)Z(2)
	MULF	-(R3),AC2	;x X(1)
	MOV	(SP)+,R2	;RESTORE REGISTER
	ADDF	AC2,AC1		;DETERMINATE NOW IN AC1
	CFCC
	BLT	.+4		;SKIP IF DETERMINATE NEGATIVE
	NEGF	AC0		;IF POSITIVE, COMPLEMENT ANGLE
	RTS	PC		;RETURN

;END OF "ANGLE"
;SUBROUTINE "MULTA" - MULTIPLIES AN "A" MATRIX BY A "T" MATRIX

;THE MATRIX "A" IS MULTIPLIED BY THE COLUMN MATRIX "T1" AND THE RESULTS ARE
;STORED IN THE COLUMN MATRIX "T2".  THIS IS THE FOLLOWING OPERATION:
;
;	| T2(1) |      |  A1   A5   A9   0   |     |  T1(1)  |
;	| T2(2) |      |  A2   A6   A10  0   |     |  T1(2)  |
;	| T2(3) |  ←   |  A3   A7   A11  A15 |  X  |  T1(3)  |
;	|  0**  |      |  0    0     0   1.0 |     |   0**   |
;
;** THESE ARE 1.0 IF T1 IS THE FOURTH COLUMN OF A T MATRIX.  A SAMPLE CALLING
;SEQUENCE FOLLOWS.
;
;		MOV	#1,COL4		;ZERO IF T1 IS FOURTH COLUMN OF T MATRIX
;		MOV	#T1,R5		;ADDRESS OF T1 MATRIX IN R5
;		MOV	#T2,R4		;ADDRESS OF T2 MATRIX IN R4
;		JSR	PC,MULTA	;CALLED USING PC
;
;THE A MATRIX IS ASSUMED TO BE THE AM MATRIX. ALL NUMBERS IN SINGLE PRECISION
;FLOATING POINT REPRESENTATION.

;REGISTER ASSIGNMENTS
;
;	R0 - LINK NUMBER*2 -2
;	R2 - GARBAGED
;	R3 - GARBAGED
;	R4 - T2 MATRIX POINTER
;	R5 - T1 MATRIX POINTER

COL4:	0	;ZERO IF T1 IS THE FOURTH COLUMN IN THE T MATRIX

;START OF EXECUTABLE CODE

MULTA:	MOV     #AM1,R3		;START WITH 1ST ROW OF "A" MATRIX
      	LDF	(R5)+,AC2	;LOAD A COLUMN OF THE "T" MATRIX
	LDF	(R5)+,AC3
	LDF	(R5)+,AC0
	STF	AC0,AC4
	MOV	AZERO(LINK),R2	;PICK UP BITS INDICATING ZERO "A" ELEMENTS
    	ASL	R2		;CHECK IF FIRST "A" ELEMENT ZERO
MU0:	BMI	MU1		;SKIP IF NOT
	CLRF	AC0		;ELSE CLEAR ACCUM. AND SKIP MULTIPLY
	BR	MU2
MU1:	LDF	(R3),AC0	;LOAD FIRST "A" ELEMENT
	MULF 	AC2,AC0		;MULT BY "T" ELEMENT 
;	[CONTINUATION OF "MULTA"]

MU2:	ASL	R2		;CHECK IF SECOND TERM ZERO
	BPL	MU3		;SKIP IF IT IS
	LDF	20(R3),AC1	;ELSE MULT SECOND ELEMENT IN A ROW BY "T1" TERM
	MULF	AC3,AC1
	ADDF	AC1,AC0		;ADD TO PREVIOUS PRODUCT
MU3:	ASL	R2		;CHECK IF THIRD TERM ZERO
	BPL	MU4		;SKIP IF ZERO
	LDF	40(R3),AC1	;ELSE MULT THIRD TERM BY "T1" TERM
	MULF	AC4,AC1	
	ADDF	AC1,AC0		;ADD TO PREVIOUS SUMS
MU4:	STF	AC0,(R4)+  	;SAVE RESULT IN T2 MATRIX
 	ADD     #4,R3    	;POINT TO NEXT ROW OF "A" MATRIX 
	ASL	R2		;CHECK IF ALL MULTIPLICATION DONE
	BNE	MU0		;DO ALL THREE ROWS
    	TST	COL4		;IS THIS THE LAST COLUMN?
	BNE	MU5		;SKIP IF NOT
	ADDF	AM15,AC0	;ELSE ADD IN A ELEMENT 15 TO LAST SUM 
	STF	AC0,-4(R4)	;SAVE NEW SUM IN T2 MATRIX
MU5:	RTS	PC		;RETURN


;END OF "MULTA"
;SUBROUTINE "ARAY" - GENERATES THE A MATRIX FOR A GIVEN ARM LINK

;THE A MATRIX IS GENERATED AND STORED BY COLUMNS IN THE "AM" LIST.  THE
;NORMAL MATRIX REPRESENTATION AND THE AM LIST ARE RELATED IN THE FOLLOWING
;FASHION:
;
;	|  AM1    AM5    AM9    AM13   |
;	|  AM2    AM6    AM10   AM14   |
;	|  AM3    AM7    AM11   AM15   |
;	|   0      0      0      1.0   |
;
;IN TERMS OF "THET", THE JOINT ANGLE,"AL", THE ANGLE ALPHA FOR THE LINK, AND
;"S", THE JOINT OFFSET, THE MATRIX IS WRITTEN AS FOLLOWS:
;
;	|  COS(TH)    -COS(AL)SIN(TH)    SIN(AL)SIN(TH)      0  |
;	|  SIN(TH)     COS(AL)COS(TH)   -SIN(AL)COS(TH)      0  |
;	|   0          SIN(AL)		 COS(AL)	     S  |
;	|   0		  0		     0		    1.0 |
;
;THE AM13 AND AM14 TERMS ARE NOT IN GENERAL ZERO, BUT THEY ARE FOR THE
;SCHEINMAN ARMS.

;REGISTER ASSIGNMENT 
;
;	R0 - LINK NUMBER*2 -2
;	R2 - USED FOR TEMP STORAGE
;	R3 - USED FOR TEMP STORAGE

;DEFINITIONS

SN==%0
CS==%1
FLINK==%3

;START OF EXECUTABLE CODE

ARAY:  	MOV	#18.,R3		;CLEAR THE A MATRIX
	MOV 	#AM3,R2
	CLR	(R2)+
	SOB	R3,.-2		;ZERO ELEMENTS 2 TO 12
	MOV	LINK,FLINK	;GET FLOATING POINT LINK INDEX
	ASL	FLINK
       	CMP	LINK,#4		;IS THIS THE SLIDING JOINT?
	BNE	REVOL		;SKIP IF REVOLUTE JOINT
	LDF	SI3,SN 		;LOAD EQUIVALENT SINE FOR JOINT 3        
	MOV	THET3,AM15  	;JOINT VARIBLE BECOMES LINK OFFSET
	LDF	CS3,CS 		;LOAD EQUIVALENT COSINE FOR JOINT 3
 	MOV	THET3+2,AM15+2  ;SECOND HALF OF JOINT VARIABLE FLOATING POINT NUMBER
	BR	CONA
;	[CONTINUATION OF "ARAY"]

REVOL:	LDF	THET(FLINK),AC0	;LOAD THE JOINT ANGLE
	JSR	PC,SNCOSD	;COMPUTE SIN/COS
	MOV	S1(FLINK),AM15	;MOVE LINK OFFSET "S" INTO A MATRIX 15
	MOV	S1+2(FLINK),AM15+2	

;CONSTRUCT THE A MATRIX 

CONA:	STF	CS,AM1		;STORE COS THETA IN ELEMENT 1
	STF	SN,AM2		;STORE SIN THETA IN ELEMENT 2
	TST	AZERO(LINK)	;CHECK IF COS ALPHA OR SIN ALPHA IS ZERO
	BGE	NOCA		;SKIP IF COS ALPHA IS ZERO
	LDF	CA(FLINK),AC2	;ELSE LOAD COSINE ALPHA
	STF	AC2,AM11	;STORE IN ELEMENT 11
	MULF	AC2,CS		;COMPUTE  COS ALPHA * COS THETA
	STF	CS,AM6		;STORE IN ELEMENT 6
	NEGF	AC2		;NEGATE COS ALPHA
	MULF	AC2,SN		;COMPUTE - COS ALPHA * SINE THETA
	STF	SN,AM5		;STORE IN ELEMENT 5
	RTS	PC  		;RETURN
NOCA:	LDF	SA(FLINK),AC2	;LOAD THE SINE OF ALPHA
	STF	AC2,AM7		;STORE IN ELEMENT 7 
	MULF	AC2,SN		;COMPUTE  SINE ALPHA * SIN THETA
	STF	SN,AM9		;STORE IN ELEMENT 9
	NEGF	AC2		;GET - SINE ALPHA
	MULF	AC2,CS		;COMPUTE -SINE ALPHA * COS THETA
	STF	CS,AM10		;STORE IN ELEMENT 10
      	RTS	PC		;RETURN


;END OF "ARAY"
;	["ARAY" - LOCAL STORAGE AREA]

;"A" MATRIX: 4 BY 4 STORED BY COLUMNS

AM1:	.WORD	0,0
AM2:	.WORD	0,0
AM3:	.WORD	0,0
     	.WORD	0,0
AM5:	.WORD	0,0
AM6:	.WORD	0,0
AM7:	.WORD	0,0
    	.WORD	0,0
AM9:	.WORD	0,0
AM10:	.WORD	0,0
AM11:	.WORD	0,0
     	.WORD	0,0
AM13:	.WORD	0,0
AM14:	.WORD	0,0
AM15:	.WORD	0,0
	.WORD	40200,0

;SINE AND COSINE ALPHA FOR THE JOINTS

SA:	.WORD	140200,0
	.WORD	40200,0
	.WORD	0,0
	.WORD	140200,0
	.WORD	40200,0
	.WORD	0,0
CA:	.WORD	0,0
	.WORD	0,0
	.WORD	40200,0
	.WORD	0,0
	.WORD	0,0
	.WORD	40200,0

;SINE AND COSINE THETA FOR JOINT 3

SI3:	.WORD	140200,0
CS3:	.WORD	0,0

;BIT FLAGS INDICATING NON-ZERO ELEMENTS IN "A" MATRIX.  BIT 15 INDICATES WHERE
;THE COS ALPHA IS ZERO, EACH BIT AFTER THAT CORRESPONDS TO 1 ELEMENT OF THE "A"
;MATRIX.

AZERO:	.WORD	055200
	.WORD	055200
	.WORD	124100
	.WORD	055200
	.WORD	055200
	.WORD	166100
;PHYSICAL CONSTANTS FOR MODEL STANFORD ARM (WESTERN ELECTRIC VERSION)

;TABLE COORDINATES OF MODEL STANFORD ARM SHOULDER AND JOINT OFFSETS

SHOLDX:	.FLT2	0.0		;SHOULDER LOCATION
SHOLDY:	.FLT2	0.0
SHOLDZ:	
S1:	.FLT2	17.805		;JT 1 OFFSET, SAME AS SHOLDZ
S2:	.FLT2	6.535		;JT 2 OFFSET
	.FLT2	0.0		;JOINT VARIABLE 3 IS THE JOINT 3 OFFSET
	.FLT2	0.0		;		JOINT 4 OFFSET
	.FLT2	0.0		;		JOINT 5 OFFSET
S6:	.FLT2	7.985		;		JOINT 6 OFFSET
S22:	.FLT2	42.70623	;S2**2


;ARM JOINT STOP LIMITS

STOP1:	.FLT2	-203.0		;JOINT 1 MIN
	.FLT2	90.0		;	 MAX
STOP3:	.FLT2	7.63		;JOINT 3 MIN
	.FLT2	35.7		;	 MAX
STOP4:	.FLT2	-265.0		;JOINT 4 MIN
	.FLT2	36.0		;	 MAX
STOP5:	.FLT2	-109.0		;JOINT 5 MIN
	.FLT2	109.0		;	 MAX
STOP6: 	.FLT2	-84.0		;JOINT 6 MIN
	.FLT2	234.0		;	 MAX


;END OF "ARMSOL"