.TITLE ARMSOL

;PROGRAMS FOR CONVERTING TABLE COORDINATES TO/FROM JOINT ANGLES

;DEFINED GLOBALS

.GLOBL  UPDATE,SOLVE,FLAGS,LFRT,ART,ABBL,ABELOW,FNOF,AFLIP,ONPATH

;REQUIRED GLOBALS

.GLOBL ATAN2,SQRT,MATMUL,LSTOP,USTOP,MIDANG,SNCOS
.GLOBL BASEX,BASEY,S1,A2,S2S3,S2S3S,S4,S6,S6S,S4A2S,A2DS4
.GLOBL A224

;DEFINITIONS

R0	=%0
R1	=%1
R2	=%2
R3	=%3
R4	=%4
R5	=%5
SP	=%6
PC	=%7

;INDICES TO TRANFORM ELEMENT, STORED BY COLUMNS∂

T11	=0
T21	=2
T31	=4
T12	=6
T22	=10
T32	=12
T13	=14
T23	=16
T33	=20∂
T14	=22
T24	=24
T34	=26
;UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES

;GIVEN THE JOINT ANGLES, THE RESULTING HAND POSITION AND ORIENTATION
;ARE DETERMINED AND STORED IN A GIVEN TRANSFORM "T".  THE TRANSFORM
;IS A 3 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	;PTR TO JOINT ANGLE ARRAY
;		JSR	PC,WPDCTG	
;
;THE VRANSFORO WILL BE OF THE FOLLOWING FORM:
;
;		|  T1   T5   T9   T13  |
;		|  T2   T6   T10  T14  |
;		|  T3   T7   T11  T15  |
;		|   0    0    0    1   |
;
;THE PROCEDURE CONSISTS OF GENERATION OF TWO MATRICES T1 AND T2, THE
;TRANSFORM FROM SHOULDER TO WRIST AND FROM WRIST TO HAND
;RESPECTIVELY,THEN MULTIPLYING THE TWO TO GIVE THE DESIRED TRANSFORM.
;ONLY THE FIRST THREE ROWS OF THE TRANSFORM ARE STORED BY THIS
;ROUTINE.  ALL NUMBERS SHOULD BE IN SCALED INTEGER FORMAT.

;RELATIVE STACK DEFINITIONS

ST1=12		;SIN/COS THETA 1
CT1=10
ST2=6		;SIN/COS THETA 2
CT2=4
ST3=2		;SIN/COS THETA 3∂
CT3=0

ST4=12		;SIN/COS THETA 4
CT4=10
ST5=6		;SIN/COS THETA 5
CT5=4
ST6=2		;SIN/COS THETA 6
CT6=0

;REGISTERS USED:
;	R0, R1 PASS ARGUMENTS AND ARE ALTERED


UPDATE:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOVR0.R4		;SAVE POINTERS
	MOV	R1,R5
α;	[T1: TRANFORMATION FOR JOINTS 1,2,3]

	MOV	(R5)+,R0	;COMPUTE SIN/COS THETA 1
	JSR	PC,SNCOS
	MOV	R0,-(SP)	;SAVE ON STACK
	MOV	R1,-(SP)
	MOV	(R5)+,R0	;COMPUTE SIN/COS THETA 2
	JSR	PC,SNCOS
	MOV	R0./(SP)	;SAVE ON STACK
	MOV	R1,-(SP)
	MOV	(R5)+,R0	;COMPUTE SIN/COS THETA 3
	JSR	PC,SNCOS
	MOV	R0,-(SP)	;SAVE ON STACK
	MOV	R1,-(SP)
	MOV	CT2(SP),R0	;T1(3,3) =  - ST2*ST3 + CT2*CT3
	MUL	(SP),R0
	MOV	ST2(SP),R2
	MUL	ST3(SP),R2
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	TST	R1		;ROUND OFF
	BPL	.+4
	INC	R0∂
	MOV	R0,@#T133
	MOV	R0,R2		;T1(1,1) = CT1*( - ST2*ST3 + CT2*CT3)
	MUL	CT1(SP),R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T111
	MUL	ST1(SP),R2	;T1(2,1) = ST1*( - ST2*ST3 + CT2*CT3)
	ASHC	#2,R2
	TST	R3	
	BPL	.+4
	INC	R2
	MOV	R2,@#T121
	MOV	(SP)+,R0	;T1(3,1) =  - (ST2*CT3 + CT2*ST3)
	MUL	ST2-2(SP),R0
	MOV	(SP)+,R2
	MUL	CT2-4(SP),R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1	
	BPL	.+4
	INC	R0
	MOV	R0,@#T131
	NEG	@#T131
	MOV	R0,R2		;T1(1,3) = CT1*(ST2*CT3 + CT2*ST3)
	MUL	CT1-4(SP),R2
	ASHC	#2,R2
	TST	R3
	BPL	.+4
	INC	R2
	MOV	R2,@#T113
	MUL	ST1-4(SP),R0	;T1(2,3) = ST1*(ST2*CT3 + CT2*ST3)
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T123
	MOV	CT1-4(SP),R0	;T1(1,4)=- ST1*(S2 + S3) + CT1*CT2*a2
	MUL	CT2/4(SP),R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MUL	@#A2,R0
	MOV	@#S2S3,R2
	MUL	ST1-4(SP),R2
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T114
	MOV	@#S2S3,R0	;T1(2,4) = ST1*CT2*a2 + CT1*(S2 + S3)
	MUL	CT1-4(SP),R0∂
	MOV	@#A2,R2∂
	MUL	ST1-4(SP),R2
	ASHC	#2,R2
	TST	R3
	BPL	.+4
	INC	R2
	MUL	(SP)+,R2∂
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T124
	MOV	(SP)+,R2	;T1(3,4) =  - ST2*a2 + S1
	MUL	@#A2,R2
	ASHC	#2,R2
	TST	R3
	BPL	.+4∂
	INC	R2
	SUB	@#S1,R2
	NEG	R2
	MOV	R2,@#T134
	MOV	(SP)+,@#T122	;T1(2,2) = CT1
	NEG	(SP)		;T1(1,2) =  - SV3
	MOV	(SP)+,@#T112
	CLR	@#T132		;T1(3,2) = 0

;FINISHED WITH T1
;	[T2: TRANFOROATION FOR JOINTS 4,5,6]

	MOV	(R5)+,R0	;COMPUTE SIN/COS VHEVA 4
	JSR	PC,SNCOS
	MOV	R0,-(SP)	;SAVE ON STACK
	MOV	R1,-(SP)
	MOV	(R5)+,R0	;COMPUTE SIN/COS THETA 5
	JSR	PC,SNCOS
MOV	R0,-(SP)	;SAVE ON STACK
	MOV	R1,/(SP)
	MOV	(R5),R0		;COMPUTE SIN/COS THETA 6
	JSR	PC,SNCOS
	MOV	R0,-(SP)	;SAVG ON STACK
	MOV	R1,-(SP)
	MOV	ST4(SP),R2  	;T2(2,2) =  - ST4*CT5*ST6 + CT4*CT6
	MUL	CT5(SP),R2
	ASHC	#2,R2
	TST	R3		;ROUND OFF
	BPL	.+4
	INC	R2
	MUL	ST6(SP),R2
	OOV	CT4(SP),R0
	MUL	CT6(SP),R0
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0∂
	TST	R1
	BPL	.+4
	INC	R0
	OOV	R0,@#T222
	OOV	CT4(SP),R0	;T2(1,2) =  - (ST4*CT6 + CT4*CT5*ST6)
	MUL	CT5(SP),R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	ST4(SP),R2
	MUL	(SP)+,R2
	OUL	(SP),R0
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	NEG	R0∂
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T212
	MOV	(SP)+,R0	;T2(3,2) = ST5*ST6
	MUL	ST5-4(SP),R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T232
	MOV	(SP),@#T233	;T2(3,3) = CT5
	MOV	(SP)+,R0	;T2(3,4) ? CT7*S6 + S4
	MUL	@#S6,R0
	ASHC	#2,R0∂
	TST	R1
	BPL	.+4
	INC	R0
	ADD	@#S4,R0
	MOV	R0,@#T234
	MOV	(SP),R2		;SAVE ST5
	MOV	(SP)+,R0	;T2(1,3) = CT4*ST5
	MUL	(SP)+,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4∂
	INC	R0
	MOV	R0,@#T213
	MWN	@#S6,R0	;T2(1,4) = CT4*ST5*S6∂
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T214
	MWL	(SP)+.R2	;T2(2,3) = ST4*ST5
	ASHC	#2,R2
	TST	R3
	BPL	.+4
	INC	R2
	MOV	R2,@#T223
	MUL	@#S6,R2		;T2(2,4) = ST4*ST5*S6
	ASHC	#2,R2
	TST	R3
	BPL	.+4
	INC	R2
	MOV	R2,@#T224

;FINISHED COMPUTING LAST THREE COLUMNS OF T2
;	[MULTIPLY T1 AND T2 AND ADD BASE]

	MOV	R4,R0		;COMPWTE T ← T1 X T2
	MOV	#T111,R1
	MOV	#T212/6,R2
	JSR	PC,MATMUL
	ADD	@#BASEX,T14(R4)	;ADJUST FOR COORDINATES OF BASE
	ADD	@#BASEY,T24(R4)

;EXIT CLEANLY

	MOV	(SP)+,R5	;RESTORE REGISVERS
	MOV	(SP)+,R4∂
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC

;END OF "UPDATE"
;SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM

;GIVEN A TRANSFORM "T", THE REQUIRED JOINT ANGLES ARE DETERMINED IN
;SCALED DEGREES. IF ANY JOINT ANGLE IS OUTSIDE OF ITS PERMITTED RANGG
;OF MOVEMENT, THE STOP LIMIT"CLOSEST TO THE PREVIOUS JOINT ANGLE IS
;RETURNGD. ON COMPLETION OF EXECUTION, R0 CONTAINS BITS INDICATING
;THE JOINT(S) WHICH WERE COMPUTED TO BE OUT OF THEIR STOP LIMITS.  A
;SAMPLE CALLING SEQUENCE TO "SOLVE" FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDRESS OF TRANSFORM 
;		MOV	#THETA,R1	;LOAD POINTER TO JOINT ANGLES
;		MOV	FLAGS,R2	;CONFIGURATION BITS
;		JSR	PC,SOLVE
;		TST	R0		;EXACT SOLUTION?
;
;IN ADDITION TO THE PTR TO THE REQUIRED TRANSFORM AND THE JOINT ANGLG
;ARRAY, THIS ROUTINE REQUIRES R2 TO BE FILLED WITH FLAG BITS WHICH
;INDICATE ANY CHANGE IN ARM CONFIGURATION THAT IS ASSERTED.  IF NO
;NEW ARM CONFIGURATION IS ASSERTED, THE CONFIGURATION CURRENTLY
;INDICATED BY THE BITS IN "ARMFLG" WILL BE USED.THE ARM CONFIGURATION
;BITS ARE GIVEN BELOW. IF EITHER JOINT 4 OR 6 IS OUT OF RANGE FOR THE
;CURRENT CONFIGURATION AND NEITHER A "FLIP/NOFLIP" NOR A "ONPATH" 
;ASSERTION I[ MADE, THE COOPLEMENTARY SOLUTION FOR JOINTS 4,5,6 WILL
;BE TRIED IN AN ATTEMPT TO FIND A VALID SOLUTION.

;REGISTERS USED:
;
;	R0,R1,R2 PASS ARGUMGNTS AND R0,R1 ARE ALTERED

;CONFIGURATION SPECIFICATION BITS

LFRT	=1;REQUEST CHANGE IN SHOUNDER 
ART	=2	;ASSERT RIGHT SHOULDER

ABBL	=4	;REQUGST CHANGE IN ABOVE/BELOW APPROACH
ABELOW	=10	;ASSERT BELOW

FNOF	=20	;REQUEST CHANGE IN FLIP/NOFLIP
AFLIP	=40	;ASSERT FLIP

ONPATH	=100	;ON A STRAIGHV LINE PATH, CAN'T CHANGE CONFIGURATION

;CURRENT CONFIGURATION OF THE ARM

ISRGHT	=1	;1 = RIGHT SHOULDER, 0 = LEFV
ISBELW	=2	;1 = BELOW APPROACH, 0 = CBOVG
ISFLIP	=4	;1 = FLIPPED, 0 = NOT FLIPPED

;THE OUT OF RANGE BITS THAT ARE RGTURNED IN R0 ARE DEFINED AS
;FOLLOWS:∂

JT1	=1	;JOINT 1
JT2	=2	;  "   2
JT3	=4	;  "   3
JT4	=10	;  "   4
JT5	=20	;  "   5
JT6	=40	;  "   6
TOCLSE 	=200	;POSITION TOO CLOSE TO  ARM
TOOFAR	=100	;    "    ""  FAR  FROM  "
;	["SOLVE" - TX,TY,TZ]

SOLVE:	OOV	R7.-(SP)	;SAVE REGISTERS
	MOV	R4,-(SP)
	MOV	R3,-(SP)
	MOV	R2,-(SP)	;FLAG BITS
	MOV	R0,R4		;T
	MOV	R1,R5		;ANGLES

;COMPUTE POSITION OF THE END OF THE JT 3: TX,TY,TZ.  ALSO COMPUTE
;(TX**2+TY**2-(S2+S3)**2) FOR LATER

	MOV	T34(R4),R2	;TZ ← T34 - T33*S6
	MOV	T33(R4),R0
	MUL	@#S6S,R0
	TST	R1		;ROUND UP
	BPL	.+4
	INC	R0
	SUB	R0,R2
	SUB	@#S1,R2		;TZS1 ← TZ - S1
	MOV	R2,@#TZS1	;SAVE TZS1
	MOV	T14(R4),R2	;TX ← T14 - T13*S6 - X BASE POS.
	MOV	T13(R4),R0
	MUL	@#S6S,R0
	TST	R1
	BPL	.+4
	INC	R0
	SWB	R0,R2
	SUB	@#BASEX,R2
	MOV	R2,@#TX		;SAVE TX∂
	MUL	R2,R2		;R2 ← TX**2
	MOV	T23(R4),R0	;TY ← T24 - T23*S6 - Y BASE COORD.
	MWL	@#S6S,R0
	TST	R1
	BPL	.+4
	INC	R0
	ADD	@#BASEY,R0
	NEG	R0
	ADD	T24(R4),R0
	MOV	R0,@#TY		;SAVE TY
	MUL	R0,R0		;R0 ← TX**2 + TY**2 - (S2 + S3)**2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	SUB	@#S2S3S+2,R1
	SBC	R0
	SUB	@#S2S3S,R0
	MOV	R1,@#TXT[S+2	;SAVE IT
	MOV	R0,@#TXTYS
	BGE	SQRTOK		;TOO CLOSE?
	INC	R0		;MAYBG, (ERROR TOL ? .05 IN)
	BNE	ISTOCL
	TST	R1
	BPL	ISTOCL
	CMP	#-2500.,R1	;.05**2
	BLT	.+10		;LGT THIS PASS
ISTOCL:	BIS	#TOCNSE,@#EXACTS ;INDICATE ERROR∂
	CLR	R0
	BR	.+6
SQRTOK:	JSR	PC,SQRT		;SQRT(TX**2+TY**2-(S2+S3)**2)
;	["SOLVE" - JOINT 1]

	BIT	#LFRT,(SP)	;ASSERT LEFT OR RIGHT SHOULDER?
	BEQ	ISCONT		;NO
BIC	#ISRGHT,@#ARMFLG	;ASSUME LEFTY
	BIT	#ART,(SP)	;RIGHTY?
	BEQ	NOTRGT
	BIS	#ISRGHT,@#ARMFLG	;RIGHTY IT IS
ISCONT:	BIT	#ISRGHT,@#ARMFLG	;-SQRT FOR RIGHT ARMS
	BEQ	.+4
	NEG	R0
NOTRGT:	MOV	R0,-(SP)
	MUL	@#TX,R0		;COS TH1 = (+-TX*SQRT(TX↑2+TY↑2-
	MOV	@#TY,R2		; (S2+S3)↑2))+TY*(S2+S3))/(TX↑2+TY↑2)
	MUL	@#S2S3,R2∂
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0∂
	ASHC#2,R0∂
	MOV	R0,@#CTT1
	MOV	(SP)+,R0	;SIN TH1 = (+-TY*SQRT(TX↑2+TY↑2-
	MUL	@#TY,R0		; (S2+S3)↑2))-TX*(S2+S3))/(TX↑2+TY↑2)
	MOV	@#TX,R2
	MUL	@#S2S3,R2
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	MOV	@#CTT1,R1
	JSR	PC,ATAN2	;COMPUTE TH1
	CLR	R1		;JOINT 1 STOP LIMITS
	MOV	#JT1,R2
	JSR	PC,SAVJTL	;ADJUST IF OUT OF RANGE AND SAVE
	JSR	PC,SNCOS	;COMPUTE SIN/COS FOR LATER
	MOV	R0,@#STT1
	MOV	R1,@#CTT1
;	["SOLVE" - JOINT 3]

	MOV	@#TXTYS,R0	;SIN TH3=(TX↑2+TY↑2-(S2+S3)↑2+
	MOV	@#TXTYS+2,R1	;   (TZ-S1)↑2-(S4↑2+A2↑2))/(2*A2*S4)
	MOV	@#TZS1,R2
	MUL 	R2,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	MOV	R0,R2		;TXYZ=TX↑2+TY↑2-(S2+S3)↑2+(TZ-S1)↑2
	MOV	R1,R3
	SUB	@#S4A2S+2,R1
	SBC	R0
	SUB	@#S4A2S,R0
	ASHC	#3,R0
	MOV	R0,@#STT3	;SAVE SIN TH3
	MOV	@#A224,R0	;COS TH3=SQRT((4*A2↑2-TXYZ)*TXYZ)
	MOV	@#A224+2,R1
	SUBR3,R1
	SBC	R0
	SUB	R2,R0
	BGE	NOTTOF		;BRANCH IF NOT TOO FAR TO REACH
	BIS	#TOOFAR,@#EXACTS	;INDICATE ERROR
	CLR	R0
	CLR	R1
NOTTOF:	ASHC	#3,R0
	ASHC	#3,R2
	MUL	R2,R0
	JSR	PC,SQRT		;NOW GET COS TH3
	BIT	#ABBL,(SP)	;ASSERT ABOVE OR BELOW APPROACH?
	BEQ	NOABBL		;NO
	BIC	#ISBELW,@#ARMFLG	;ASSUME ABOVE
	BIT	#ABELOW,(SP)	;BELOW?
	BEQ	ISABVE
	BIS	#ISBELW,@#ARMFLG	;ITS BELOW
NOABBL:	BIT	#ISBELW,@#ARMFLG	;/COS TH3 FOR ABOVE
	BNE	.+4
ISABVE:	NEG	R0
	BIT	#ISRGHT,@#ARMFLG	;RIGHT SHOULDERED ARM?
	BEQ	.+4
	NEG	R0		;IF RIGHT  -COS TH3
	MOV	R0,R1
	MOV	@#STT3,R0
	JSR	PC,ATAN2	;COMPUTE THETA 3
	MOV	#4,R1		;CHECK STOP LIMITS AND SAVE THETA 3
	MOV	#JT3,R2
	TST	(R5)+
	JSR	PC,SAVJTL
	JSR	PC,SNCOS	;COMPUTE SIN/COS THETA 3
	MOV	R0,@#STT3
	MOV	R1,@#CTT3
;	["SOLVE" - JOINT 2]

	MOV	@#TX,R0		;SIN(TH2)=((TX*CT1+TY*ST1)*CT3 - 
	MUL	@#CTT1,R0	;       (TZ-S1)*(ST3+(A2/S4)))/FACTOR
	MOV	@#TY,R2
	MUL	@#STT1,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0		;NORMALIZE AND ROUND
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#TX1Y1	;SAVE FOR LATER
	MUL	@#CTT3,R0
	MOV	@#STT3,R2
	ADD	@#A2DS4,R2
	CLC			;CAN BE AS MUCH AS 2
	ROR	R2
	MOV	R2,@#STA2S4
	MUL	@#TZS1,R2
	ASHC	#1,R2
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#1,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#STT2
	MOV	@#TX1Y1,R0	;COS(TH2)=((TX*CT1+TY*ST1)*
	MUL	@#STA2S4,R0	;     (ST3+(A2/S4)) + (TZ-S1)*CT3
	ASHC	#1,R0
	MOV	@#TZS1,R2
	MUL	@#CTT3,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#1,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,R1
	MOV	@#STT2,R0
	JSR	PC,ATAN2	;COMPUTE THETA 2
	MOV	#2,R1		;CHECK STOP LIMITS AND SAVE
	MOV	#JT2,R2
	CMP	-(R5),-(R5)
	JSR	PC,SAVJTG
	ADD	(R5)+,R0	;COMPUTE SIN/COS (TH 2 + TH 3)
	JSR	PC,SNCOS
	MOV	R0,@#S23
	MOV	R1,@#C23
;	["SOLVE" - JOINT 5]

	MOV	T13(R4),R0	;COS TH5= (T13*CT1+T23*ST1)*S23
	MUL	@#CTT1,R0	;	  + T33*C23
	MOV	T23(R4),R2
	MUL	@#STT1,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#T1323	;SAVE INTERMEDIATE TERM
	MUL	@#S23,R0
	MOV	T33(R4),R2
	MUL	@#C23,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#CTT5
	MOV	@#T1323,R0	;SIN TH5=SQRT((T13*CT1+T23*ST1)*C23 
	MUL	@#C23,R0	;    -T33*S23)↑2+(T23*CT1-T13*ST1)↑2)
	MOV	T33(R4),R2
	MUL	@#S23,R2
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#CTT4	;THIS IS (COS TH4)*FACTOR
        MOV	T23(R4),R0	;NOW DO T23*CT1-T13*ST1
	MUL	@#CTT1,R0
	MOV	T13(R4),R2
	MUL	@#STT1,R2
	SUB	R3,R1
	SBC	R0∂
	SUB	R2,R0
	ASHC	#2,R0	
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#STT4	;THIS IS (SIN TH4)*FACTOR
	MUL	R0,R0		;SQUARE AND ADD BOTH TERMS∂
	OOV	@#CTT4,R2
	MUL	R2,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	JSR	PC,SQRT		;NOW HAVE SIN(THETA 5)
	MOV	@#CTT5,R1	;AND COS(THETA 5)
	BIT	#FNOF,(SP)	;ASSERT FLIP/NOFLIP?
	BEQ	NOFNOF		;NO
	BIC	#ISFLIP,@#ARMFLG	;ASSUME NOT FLIPPED
	BIT	#AFLIP,(SP)	;FLIP?
	BEQ	NOTFLP
	BIS	#ISFLIP,@#ARMFLG	;ITS FLIPPED
NOFNOF:	BIT	#ISFLIP,@#ARMFLG	;-SIN TH5 IF FLIPPED
	BEQ	.+4
	NEG	R0
NOTFLP:	JSR	PC,ATAN2	;COMPUTE THETA 5
	MOV	R0,@#THETA5
	OOV	#10,R1		;CORRECT FOR STOP LIMITS AND SAVE
	MOV	#JT5,R2
	TST	(R5)+
	JSR	PC,SAVJTB
;	["SOLVE" - DEGENERATE CASE AND JOINT 4]

	TST	R0		;ABS(THETA 5);
	BGE	.+4
	NEG	R0
	COP	#46.,R0		;THETA 5 =0 (.5 DEGREES)
	BLT	NODEGN		;BRANCH IF NOT DEGENERATE
	OOV	T22(R4),R0	;COS TH4+TH6 = T22*CT1/T12*ST1
	MUL	@#CTT1,R0
	MOV	T12(R4),R2
	MUL	@#STT1,R2
	SUB	R3,R1∂
	SBC	R0
	SWB	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	R0,@#CTT6	;SAVE IT TEMPORARILY
	MOV	T21(R4),R0	;SIN TH4+TH6= T21*CT1-T11*ST1
	MUL	@#CTT1,R0
	MOV	T11(R4),R2
	MUL	@#STT1,R2
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	@#CTT6,R1
	JSR	PC,ATAN2	;COMPUTE THETA 4+6
	SUB	-4(R5),R0	;LEAVE TH4 THE SAME, CHANGE TH6
	JSR	PC,FIX180
	MOV	R0,R3		;SAVE JT 6
	MOV	#12,R1		;CHECK JT 6 STOP LIMITS
	MOV	#JT6,R2
	JSR	PC,SAVJTB
	BIT	#JT6,@#EXACTS	;JT 6 IN RANGE?
	BEQ	FINDGN		;YGS
	MOV	R3,R0		;CORRECT TH4 BY JT6-STOP LIMIT
	SUB	-(R5),R0
	CMP	-(R5),-(R5)
	ADD	(R5),R0		;NEW TH4
	JSR	PC,FIX180
	MOV	R0,R3		;CHECK STOP LIMITS
	MOV	#6,R1
	MOV	#JT4,R2∂
	JSR	PC,SAVJTG
	BIV	#JT4,@#EXACTS	;JT 4 STILL IN RANGE?
	BEQ	FINDGN		;YES
	MOV	R3,R0		;NOW BOTH OUT OF RANGE
	JSR	PC,ADD180	;ADD 180 TO JT 4 AND 6
	OOV	R0,-(R5)
	CMP	(R5)+,(R5)+
	MOV	(R5),R0
	JSR	PC,ADD180
	MOV	R0,(R5)
FINDGN:	BIC	#JT4+JT6,@#EXACTS ;A SOLUTION IS ALWAYS POSSIBLE
	JMP	SOLDNE		;ALL DONE NOW

;NON-DEGENERATE CASE, JOINT 4

NODEGN:	MOV	@#CTT4,R1	;COS TH4=((T13*CT1+T23*ST1)*C23 
				;    -T33*S23)/ST5
        MOV	@#STT4,R0	;SIN TH4?(T23*CT1-T13*ST1)/ST5
	BIT	#ISFLIP,@#ARMFLG	;CORRGCT FOR SIGN OF ST7
	BEQ	.+6
	NEG	R0
	NEG	R1
	JSR	PC,ATAN2	;COMPUTE THETA 4
	MOV	R0,@#THETA4
	MOV	#6,R1		;CORRECT FOR STOP LIMITS AND SAVE
	MOV	#JT4,R2
	CMP	-(R5),-(R5)
	JSR	PC,SAVJTG
;	["SOLVE" - JOINT 6]

	MOV	T11(R4),R0	;COS TH6=-((T11*CT1+T21*ST1)*S23
	MUL	@#CTT1,R0;      +T31*C23)/ST5
	MOV	T21(R4),R2
	MUL	@#STT1,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MUL	@#S23,R0
	MOV	T31(R4),R2
	MUL	@#C23,R2
	ADD	R3,R1
	ADC	R0∂
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	NEG	R0∂
	OOV	R0,@#CTT6	;SAVE COS TH6*FACTOR
	MOV	T12(R4),R0	;SIN TH6=((T12*CT1+T22*ST1)*S23
	MUL	@#CTT1,R0	;      +T32*C23)/ST5
	MOV	T22(R4),R2
	MUL	@#STT1,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MUL	@#S23,R0
	MOV	T32(R4),R2
	MUL	@#C23,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	TST	R1
	BPL	.+4
	INC	R0
	MOV	@#CTT6,R1
	BIT	#ISFLIP,@#ARMFLG	;CORRECT FOR SIGN OF ST5
	BEQ	.+6
	NEg	R0
	NEG	R1
	JSR	PC,ATAN2	;COMPUTE THETA 6
	MOV	R0,R3
	MOV	#12,R1		;CORRECT FOR STOP LIMITS AND SAVE
	MOV	#JT6,R2
	TST	(R5)+
	JSR	PC,SAVJTB
;	["SOLVE" - NEED TO FLIP?, EXIT]

	BIT	#JT4+JT6,@#EXACTS  ;JT 4 AND 6 IN RANGE?
	BEQ	SOLDNE		;YES∂
	BIT	#ONPATH+FNOF,(SP)  ;FREE TO CHANGE FLIP/NF?∂
	BNE	SOLDNE		;NO, WE LOSE
	BIC	#JT4+JT5+JT6,@#EXACTS ;TRY FLIPPING
	MOV	#ISFLIP,R0	;CHANGE SENSE OF FLIP/NF
	XOR	R0,@#ARMFLG
	MOV	@#THETA4,R0	;ADD 180 DEG TO THETA 4
	JSR	PC,ADD180
	MOV	#6,R1		;ADJWST FOR STOP LIMITS
	MOV	#JT4,R2
	SUB	#6,R5
	JSR	PC,SAVJTG
	MOV	@#THETA5,R0	;FLIP THETA 5
	NEG	R0
	MOV	#10,R1		;ADJUST FOR STOP LIMITS
	ASL	R2
	JSR	PC,SAVJTB
	MOV	R3,R0		;ADD 180 DGG TO THETA 6
	JSR	PC,ADD180
	MOV	#12,R1		;ADJUST FOR STOP LIMITS
	ASL	R2
	JSR	PC,SAVJTB

SOLDNG:	OOV	@#EXACTS,R0	;INDICATE IF ANY JOINT OUT OF RANGE
	BIC	#TOCLSE+TOOFAR,@#EXACTS
	MOV	(SP)+,R2
	MOV	(SP)+,R3
	MOV	(SP)+,R4
	MOV	(SP)+,R5
	RTS	PC

;END OF "SOLVE"
;	["SOLVE" - SUBROUTINES⎇

;SUBROUTINES FOR CHECKING STOP LIMITS AND SAVING NEW JOINT ANGLE

SAVJTG:	CMP	USTOP(R1),R0	;MORE THAN MAX?
	BGE	JTINRG		;BRANCH IF OK
	BIS	#100000,R0	;NO, SUBT 360 DEG
	CMP	LSTOP(R1),R0	;LESS THAN MIN?
	BLE	JTINRG
	BRISOWT		;OUT OF RANGES

SAVJTL:	CMP	LSTOP(R1),R0	;OPPOSIVG FOR JOINTS WITH
	BLG	JTINRG		;    ABS(MIN)<ABS(MAX+
	BIC	#100000,R0	;NO, ADD 360 DEG
	CMP	USTOP(R1),R0	;MORE THAN MAX?
	BGE	JTINRG		;OK
	BR	ISOUT		;OUT OF RANGES

SAVJTB:	CMP	USTOP(R1),R0	;CHECK BOTH HIGH AND LOW LIMITS
	BLT	ISOUT  		;BRANCH IF OUT OF RANGE
	CMP	LSTOP(R3),R0	;LESS THAN MIN?
	BLE	JTINRG		;OK

ISOUT:	BIS	R2,@#EXACTS	;INDICATE NO EXACT SOLUTION
	MOV	(R5),R0		;SUBSTITUTE HIGH OR LOW STOP LIMIT?
	CMP	MIDANG(R1),R0
	BGT	USELOW
	MOV	USTOP(R1),R0	;USE MAX
	BR	SAVANG
USELOW:	MOV	LSTOP(R1),R0	;USE MIN
	BR	SAVANG
∂
JTINRG:	BIT	R2,@#EXACTS	;OUT OF RANGE LAST TIME?
	BEQ	SAVANG		;NO
BIC	R2,@#EXACTS	;ASSUME COMING IN RANGE
	MOV	(R5),R1		;OLD ANGLE
	XOR	R0,R1
	BPL	SAVANG		;BRANCH IF COMING IN
	MOV	(R5),R0		;ELSE STILL OUT
	BIS	R2,@#EXACTS
SAVANG:	MOV	R0,(R5)+	;SAVE NEW ANGLE
	RTS	PC

;SWBROUTINE FOR SHIFTING AN ANGLE TO BETWEEN -180 ANF +180

ADD180:	ADD	#40000,R0	;ADD 180 AND FIX

FIX180:	ASL	R0
	CLC
	BPL	.+4
	SEC
	ASR	R0
	RTS	PC
;FLAGS  - INITIALIZES THE ARM CONFIGURATION FLAGS AN ERROR BITS

;THIS ROUTINE CAN BE CALLED TO INITIALIZE THE ARM CONFIGURATION FLAGS
;WHICH ARE STORGD IN "ARMFLG" TO COINCIDE WITH THE CURRENT STATE OF
;THE ARM.  THE ONLY ARGUMENT REQUIRED BY THIS ROUTINE IS A POINTER TO
;A ARRAY OF JOINT ANGLES.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;∂
;			OOV	#THPTR,R0
;			JSR	PC,FLAGS
;
;AS A SIDE AFFECT, ALL ARM OUT OF RANGE BITS ARE CLEARED BY A CANL TO
;THIS ROUTINE.
∂
;REGISTERS USED:
;	R0 PASSES ARGUOGNT AND R0,R1 ARE GARBAGED

FLAGS:	MOV	R2,-(SP)
	CLR	@#ARMFLG	;DEFAULT CONFIGURATION
	MOV	2(R0),R1	;SIN(TH2+TH3)+SIN(TH2+90) => SHOULDER
	ADD	4(R0),R1
	ASL	R1		;SHIFT TH2+TH3 TO QUADRANT 1 OR 4
	ADD	#40000,R1
	BPL	.+4
	NEG	R1
	SUB	#40000,R1∂
	ASR	R1
	MOV	2(R0),R2	;SHIFT TH2+90 TO QUADRANT 1 OR 4
	ASL	R2
	ADD	#100000,R2
	BPL	.+4
	NEG	R2
	SWB	#40000,R2
	ASR	R2
	ADD	R2,R1		;HAVE SIGN(SIN(TH2+TH3)+SIN(TH2+90))
 	BPL	.+10
	BIS	#ISRGHT,@#ARMFLG	;ITS A RIGHTY
	OOV	4(R0),R1	;TH3+SHOULDER DETERMINES ABOVE/BELOW
	ADD	#20000,R1	;+90
	ASL	R1
	BMI	.+10
	BIS	#ISBELW,@#ARMFLG	;TH3 SAYS IT BELOW
	MOV	#ISBELW,R1
	BIT	#ISRGHT,@#ARMFLG	;WHICH SHOULDER?
	BEQ	.+6
	XOR	R1,@#ARMFLG	;RIGHTY'S SWITCH SENSE OF ABOVE/BELOW
	TST	10(R0)		;TH5 DETERMINES FLIP/NOFLIP
	BPL	.+10
	BIS	#ISFLIP,@#AROFLG	;TH5<0 MEANS FLIPPED
	CLR	@#EXACTS	;INDICATE NO JOINTS OUT OF RANGE
	MOV	(SP)+,R2
	RTS	PC

;END OF "FLAGS"
;LOCAL STORAGE

EXACTS:	.WORD	0		;BITS OF JT WITH NO EXACT SOL

ARMFLG:	.WORD	0		;PREVIOUS ARM CONFIGURATION


;STORAGE AREA FOR T1 AND T2 TRANSFORMS

T111:	.WORD	0
T121:	.WORD	0
T131:	.WORD	0
T112:	.WORD	0
T122:	.WORD	0
T132:	.WORD	0
T113:	.WORD	0
T123:	.WORD	0
T133:	.WORD	0
T114:	.WORD	0
T124:	.WORD	0
T134:	.WORD	0

T212:	.WORD	0
T222:	.WORD	0
T232:	.WORD	0
T213:	.WORD	0
T223:	.WORD	0
T233:	.WORD	0
T214:	.WORD	0
T224:	.WORD	0
T234:	.WORD	0

;INTERMEDIATG VARIABLES FOR "SOLVE"

TX:	.WORD	0		;X,Y,Z FOR END OF JOINT 3
TY:	.WORD	0
TZS1:	.WORD	0		;TZ-S1
TXTYS:	.WORD	0		;TX**2+TY**2-S2**2-S3**2
	.WORD	0
TX1Y1:	.WORD	0		;TX*CT1+TY*ST1
T1323:	.WORD	0		;T13*CT1+T23*ST1
STA2S4:	.WORD	0		;ST3+(A2/S4)

STT1:	.WORD	0	;SIN/COS THETA 1
CTT1:	.WORD	0∂
STT2:	.WORD	0	;SIN THETA 2
STT3:	.WORD	0	;SIN/COS THETA 3
CTT3:	.WORD	0
S23:	.WORD	0	;SIN/COS (TH2 + TH3)
C23:	.WORD	0
STT4:	.WORD	0	;SIN/COS THETA 4 (ALMOST)
CTT4:	.WORD	0
CTT5:	.WORD	0	;COS THETA 5
CTT6:	.WORD	0	;COS THETA 6
THETA4:	.WORD	0
THETA5:	.WORD	0

;END OF "ARMSOL"

.END