# disassembled code for the Mark I 6503s at mcgill

 	CLD	     ---* Clear decimal mode
	LDA  #$AA    ---+
	LDY  #$02	| Test the RAM with the bit
L1:	LDX  #$7F	| pattern AA, and then clear
L2:	STA  $00,X	| all locations 01 - 7F
	CMP  $00,X	|
	BNE  L5		|
	DEX		|
	BPL  L2		|
	DEY		|
	BEQ  L3		|
	BMI  L4		|
	LDA  #$FF	|
	BNE  L1		|
L3:	LDA  #$00    	|
	BEQ  L1      ---+
L4:	LDA  #$00    ---+ 
	STA  $00 	| Clear (00)
	BEQ  L6	     ---+
L5:	LDA  #$01    ---+ Bail out with MEMORY_FAILURE
	JSR  SUB12   ---+
L6:	LDX  #$7E    ---+ Stack starts at 7E
	TXS	     ---+
	LDY  #$00    ---+
	BEQ  L8		| Initialize memory
L7:	INY		|
	LDA  $0F9A,Y	|
	STA  $00,X	|
	INY		|
L8:	LDX  $0F9A,Y	|
	CPX  #$FF	|
	BNE  L7      ---+
	LDA  $0600   ---+ rawEnc2,lastRawEnc <- Cnt[7-0]
	STA  $1F	|
	STA  $20     ---+
	LDA  $2A     ---+
	LDY  $2B	| 'Stop' at current position
	JSR  SUB9    ---+
	LDA  $08     ---+ PIO_timer <- tickRate
	STA  $9D     ---+
	CLI	     ---* Enable PIO interrupts
L9:	JMP  L9	     ---* Wait for some action

	Prime interrupt handler for joint commands (non-maskable)

	PHA	     ---+
	TYA		| Push (A),(Y),(X)
	PHA		|
	TXA		|
	PHA	     ---+
	LDA  $0603   ---+
	LSR		| Read the joint command off
	LSR		| the bus, and jump to the
	LSR		| appropriate handler as
	AND  #$0F	| indexed in the NMI dispatch
	TAY		| table.
	LDA  $0F7A,Y	|
	STA  $3F	|
	JMP  ($003F) ---+

	Prime interrupt handler for PIO (maskable) interrupts

	DEC  $0602   ---* Disable Command interrupts
	PHA	     ---* Push (A)
	TYA	     ---* (A) <- (Y)
	BIT  $85     ---+ if (zero index interrupt)
	BVC  L10	|   GOTO L55
	JMP  L55     ---+

	Timer interrupt handler

L10:	LDY  $08     ---+ PIO_timer <- tickRate
	STY  $9D     ---+
	DEC  $07     ---* Decrease tickCnt
	LDY  $0600   ---* (Y) <- Cnt [7-0]
	INC  $0602   ---* Enable Command interrupts
	PHA	     ---+
	TXA		| Push (A),(X)
	PHA	     ---+
	LDA  $80     ---+
	AND  #$10	| if (au2 set)
	BNE  L11        |   Clear overheatTimeout
	INC  $4F        | else
	LDA  $4F     	|   if ( overheatTimeout++ >= 5)
	CMP  #$05	|     Bail out OVERHEATED
	BCC  L111	|
	LDA  #$20	|
	JSR  SUB12      |
L11:	LDA  #$00	|
	STA  $4F     ---+
L111:	STY  $1E     ---* rawEnc1 <- Cnt [7-0]
	LDA  #$10    ---+
	BIT  $01	| if (HALF_COUNT)
	BEQ  L12	|   Clear bit 0 of rawEnc1
	TYA		|
	AND  #$FE	|
	STA  $1E     ---+
L12:	JSR  SUB3    ---* Calculate current position
	CLC	     ---+ 
	LDA  $45	| zindexCnt <- zindexCnt + vEst
	ADC  $0E	|
	STA  $45	|
	LDA  $46	|
	ADC  $0F	|
	STA  $46     ---+
	BMI  L13     ---+
	LDA  #$00	| if (abs (zindexCnt) > 0x500)
	CMP  $45	|   bail out on NO_ZERO_INDEX
	LDA  #$05	|
	SBC  $46	|
	BCS  L15	|
	BCC  L14	|
L13:	CLC		|
	LDA  #$00	|
	ADC  $45	|
	LDA  #$05	|
	ADC  $46	|
	BCS  L15	|
L14:	LDA  #$02	|
	JSR  SUB12   ---+
L15:	JSR  SUB4    ---* CALL sub4
	LDA  #$40    ---+ if (Opflag 6 clear)
	BIT  $50	|   CALL sub5
	BNE  L16	|
	JSR  SUB5	|
	JMP  L18     ---+
L16:	JSR  SUB1    ---+ else
	LDX  #$00	|   CALL sub1
	LDA  $60	|   if ( (60) != 0 ) 
	BEQ  L17	|     Clear (68)
	STX  $68	|     if ( xDelta != 0 )
	LDA  $16	|       Clear (60)
	BNE  L17	|       (68) <- 1
	LDA  $17	|   else
	BEQ  L18	|     (68) <- (68) + 1
L17:	STX  $60	|
	INC  $68     ---+
L18:	LDY  $0600   ---+ rawEnc2 <- Cnt [7-0]
	STY  $1F     ---+
	LDA  #$10    ---+
	BIT  $01	| if (HALF_COUNT)
	BEQ  L19	|   Clear bit 0 of rawEnc2
	TYA		|
	AND  #$FE	|
	STA  $1F     ---+
L19:	LDA  $30     ---+
	BNE  L20	| if commandRequest > 0
	JMP  L90	|   lookup the appropriate
L20:	LDX  $32	|   routine and jump to it;
	LDY  $3B,X	|
	LDA  $0F8A,Y	| else
	STA  $3F	|   GOTO interrupt return
	JMP  ($003F) ---+

SUB1:	CLC	     ---+
	LDA  $24	| xd <- xd + xdInc
	ADC  $21	|
	STA  $24	|
	LDA  $25	|
	ADC  $22	|
	STA  $25	|
	LDA  $26	|
	ADC  $23	|
	STA  $26     ---+
	CLC	     ---+
	LDA  $10	| vd <- vd + vdInc
	ADC  $13	|
	STA  $10	|
	LDA  $11	|
	ADC  $14	|
	STA  $11	|
	LDA  $12	|
	ADC  $15	|
	STA  $12     ---+
	LDA  #$00    ---+
	STA  $27	| Clear (27),(2C)
	STA  $2C     ---+
	LDA  $07     ---+
	BNE  L21	| if (tickCnt = 0)
	LDA  #$00	|  { xdInc = 0;
	STA  $21	|    vdInc = 0;
	STA  $22	|  }
	STA  $23	|
	STA  $13	|
	STA  $14	|
	STA  $15     ---+
L21:	SEC	     ---+
	LDA  $2A	| Calculate error:
	SBC  $25	| 
	STA  $28	| 
	STA  $61	| 
	STA  $2D	| xErr = xErrSave = x - xd;
	LDA  $2B	| DACvalue = xErr;
	SBC  $26	|
	STA  $29	|
	STA  $62	|
	STA  $2E     ---+
	LDX  #$47    ---+
	JSR  SUB11	| if (posEnvelope < abs (xErr)
	BCS  L22	|   if (Opflag 5 clear)
	LDA  #$20	|     signal OUT_OF_ENVELOPE
	BIT  $50	|
	BNE  L22	|
	LDA  #$04	|
	JSR  SUB12   ---+
L22:	LDX  #$49    ---+
	JSR  SUB11	| if (ABS(xErr) <= posTolerence)
	BCS  L23	|    clear control reg TOLERANCE;
	LDA  $0602	| else
	AND  #$7F	|    set control reg TOLERANCE;
	JMP  L24	|
L23:	LDA  $0602	|
	ORA  #$80    ---+
L24:	STA  $0602   ---+ if INTEGRATION
	BIT  $01     ---+   GOTO L26
	BVS  L26     ---+
	LDX  $1A	| if (abs (vAvg) < 04)
	BMI  L25	|   GOTO L30
	BNE  L26	|
	LDA  $19	|
	CMP  #$04	|
	BPL  L26	|
	BMI  L30     	|
L25:	INX		|
	BNE  L26	|
	SEC		|
	LDA  #$00	|
	SBC  $19	|
	CMP  #$04	|
	BMI  L30     ---+
L26:	LDA  $0C     ---+
	BMI  L27	| 
	LDA  #$60	| if ( 060 < abs (integral) )
	CMP  $0B	|   GOTO L30
	LDA  #$00	| else
	SBC  $0C	|   if (sign(xErr) = 
	BCS  L28	|      sgn (integral))
	LDA  $29	|     GOTO L30
	BMI  L28	|
	BPL  L30	|
L27:	CLC		|
	LDA  #$60	|
	ADC  $0B	|
	LDA  #$00	|
	ADC  $0C	|
	BCS  L28	|
	LDA  $29	|
	BPL  L28	|
	BMI  L30     ---+
L28:	LDX  $05     ---+
	LDA  $29	| xErr <- xErr / integralGain
L29:	CMP  #$80	|
	ROR		|
	ROR  $28	|
	ROR  $27	|
	DEX		|
	BNE  L29	|
	STA  $29     ---+
	CLC	     ---+
	LDA  $0A	| integral <- integral + xErr
	ADC  $27	|
	STA  $0A	|
	LDA  $0B	|
	ADC  $28	|
	STA  $0B	|
	LDA  $0C	|
	ADC  $29	|
	STA  $0C     ---+
L30:	CLC	     ---+
	LDA  $2C	|
	ADC  $0A	| DACvalue <- DACvalue + integral
	STA  $2C	|
	LDA  $2D	|
	ADC  $0B	|
	STA  $2D	|
	LDA  $2E	|
	ADC  $0C	|
	STA  $2E     ---+
	LDX  $03     ---+
	BEQ  L32	| DACvalue <- DACvalue / kGain
L31:	CMP  #$80	|
	ROR		|
	ROR  $2D	|
	ROR  $2C	|
	DEX		|
	BNE  L31	|
	STA  $2E     ---+
L32:	SEC	     ---+
	LDA  $0D	| vEst <- vEst - vd
	SBC  $10	|
	STA  $0D	|
	LDA  $0E	|
	SBC  $11	|
	STA  $0E	|
	LDA  $0F	|
	SBC  $12	|
	STA  $0F     ---+
	LDX  $5C     ---+
	BEQ  L34	| vEst <- vEst / vGain
L33:	CMP  #$80	|
	ROR		|
	ROR  $0E	|
	ROR  $0D	|
	DEX		|
	BNE  L33	|
	STA  $0F     ---+
L34:	CLC	     ---+
	LDA  $2C	| DACvalue <- DACvalue + vEst
	ADC  $0D	|
	STA  $2C	|
	LDA  $2D	|
	ADC  $0E	|
	STA  $2D	|
	LDA  $2E	|
	ADC  $0F	|
	STA  $2E     ---+
	SEC	     ---+
	LDA  $2C	| DACvalue <- DACvalue - accelEst
	SBC  $63	|
	STA  $2C	|
	LDA  $2D	|
	SBC  $64	|
	STA  $2D	|
	LDA  $2E	|
	SBC  $65	|
	STA  $2E     ---+
	LDX  #$00    ---+
	LDA  $2E	| (2F) <- signextnd (DACvalue)
	BPL  L35	|
	LDX  #$FF	|
L35:	STX  $2F     ---+
	LDX  $06     ---+
	TXA		| if oGain != 0
	BEQ  L37	|   DACvalue <- DACvalue * oGain
	LDA  $2C	|
L36:	ASL		|
	ROL  $2D	|
	ROL  $2E	|
	ROL  $2F	|
	DEX		|
	BNE  L36     ---+
L37:	LDY  $2D     ---+
	LDX  $2E	| Send output to DAC
	JSR  SUB2    ---+
	RTS 	     ---* RETURN

	Outputs DACvalue to the DAC. Assumes that DACvalue is passed
	in as (X,Y).

SUB2:	TXA	     ---+
	ROL		| if (2F,X,Y) >= 0x7FE
	LDA  $2F	|   (X,Y) = 0x7FE
	ADC  #$00	| else if (2F,X,Y) < 0xFFF800
	BEQ  L38	|   (X,Y) = 0xF800
	LDA  $2F	|
	BPL  L39	|
	BMI  L41	|
L38:	LDA  $2F	|
	BMI  L40	|
	SEC		|
	TYA		|
	SBC  #$FE	|
	TXA		|
	SBC  #$07	|
	BMI  L43	|
L39:	LDY  #$FE	|
	LDX  #$07	|
	BNE  L42	|
L40:	SEC		|
	TYA		|
	SBC  #$00	|
	TXA		|
	SBC  #$F8	|
	BPL  L43	|
L41:	LDY  #$00	|
	LDX  #$F8    ---+
L42:	INC  $4D     ---+
	BNE  L44	| if (X,Y) were coerced,
	INC  $4E	|   if (DACtimeout >= 0x2C00)
	LDA  #$2C	|       bail out with DAC_SATURATE
	CMP  $4E	|
	BNE  L44	| else
	LDA  #$10	|   Clear DACtimeout
	JSR  SUB12	|
	BNE  L44	|
L43:	LDA  #$00	|
	STA  $4D	|
	STA  $4E     ---+
L44:	STY  $52     ---+
	TXA		| Shift 8 signifigant bits
	ASL  $52	| into (X) using (52) as a
	ROL		| scratch register
	ASL  $52	|
	ROL		|
	ASL  $52	|
	ROL		|
	ASL  $52	|
	ROL		|
	TAX		|
	TYA	     ---+
	EOR  $80     ---+
	AND  #$0F	| Store data into the DAC
	EOR  $80	|
	STA  $80	|
	STX  $82     ---+
	RTS	     ---+ RETURN
 
	Calculates present position

SUB3:	LDX  #$00    ---* Clear (X)
	SEC	     ---+
	LDA  $1F	| (52) <- rawEnc2 - lastRawEnc
	SBC  $20	| (53) <- signextnd (52)
	STA  $52	|
	BPL  L45	|
	LDX  #$FF	|
L45:	STX  $53     ---+
	LDX  #$00    ---+
	SEC		|
	LDA  $1E	| vEst <- rawEnc1 - rawEnc2 + temp
	SBC  $1F	|
	BPL  L46	|
	LDX  #$FF       |
L46:	CLC		|
	ADC  $52	|
	STA  $0E	|
	TXA		|
	ADC  $53	|
	STA  $0F	|
	LDX  #$00       |
	STX  $0D     ---+
	LDA  #$10    ---+
	BIT  $01	| if (HALF_COUNT_MODE)
	BEQ  L47	|   vEst <- vEst / 2
	LDA  $0F	|
	CMP  #$80	|
	ROR  $0F	|
	ROR  $0E     ---+
L47:	LDA  $1E     ---+
	STA  $20	| lastRawEnc,rawEnc2 <- rawEnc1
	STA  $1F     ---+
	DEC  $0602   ---* Disable Command interrupts
	CLC	     ---+
	LDA  $2A	| x <- x + vEst
	ADC  $0E	|
	STA  $2A	|
	LDA  $2B	|
	ADC  $0F	|
	STA  $2B     ---+
	INC  $0602   ---* Enable Command interrupts
	RTS	     ---* RETURN


SUB4:	LDX  #$05    ---+
L48:	ASL  $0D	| vEst <- vEst * 0x20
	ROL  $0E	|
	ROL  $0F	|
	DEX		|
	BNE  L48     ---+
	SEC	     ---+
	LDA  $0D	| temp <- vEst - vAvg
	SBC  $18	|
	STA  $51	|
	LDA  $0E	|
	SBC  $19	|
	STA  $52	|
	LDA  $0F	|
	SBC  $1A	|
	STA  $53     ---+ 
	LDY  #$08    ---+
	LDA  $07	|
	CMP  #$1F	| if (tickCnt = 0x1F),
	BEQ  L49	|     temp <- temp / 08
	LDY  $56	| else
	BEQ  L51	|   if ( vAvgWin != 0 )
L49:	LDA  $53	|     temp <- temp / vAvgWin
L50:	CMP  #$80	|
	ROR		|
	ROR  $52	|
	ROR  $51	|
	DEY		|
	BNE  L50	|
	STA  $53     ---+
L51:	CLC	     ---+
	LDA  $51	| vAvg <- vAvg + temp
	ADC  $18	|
	STA  $18	|
	LDA  $52	|
	ADC  $19	|
	STA  $19	|
	LDA  $53	|
	ADC  $1A	|
	STA  $1A     ---+
	SEC	     ---+
	LDA  $0D	| vEst <- vEst - vAvg
	SBC  $18	|
	STA  $0D	|
	LDA  $0E	|
	SBC  $19	|
	STA  $0E	|
	LDA  $0F	|
	SBC  $1A	|
	STA  $0F     ---+
	LDX  $54     ---+
	LDA  $5C	| if ( vGain != 1 )
	CMP  #$01	|   temp <- vEstScale2
	BEQ  L52	| else
	LDX  $55	|   temp <- vEstScale1
L52:	STX  $52	| 
	SEC		| if ( vAvgWin - temp > 0 )
	LDA  $56	|   vEst <- vEst / (vAvgWin - temp )
	SBC  $52	|
	BEQ  L54	|
	BMI  L54	|
	TAX		|
	BEQ  L54	|
	LDA  $0F	|
L53:	CMP  #$80	|
	ROR		|
	ROR  $0E	|
	ROR  $0D	|
	DEX		|
	BNE  L53	|
	STA  $0F     ---+
L54:	CLC	     ---+
	LDA  $0D	| vEst, vEstSave <- vEst + vAvg
	ADC  $18	|
	STA  $0D	|
	STA  $1B	|
	LDA  $0E	|
	ADC  $19	|
	STA  $0E	|
	STA  $1C	|
	LDA  $0F	|
	ADC  $1A	|
	STA  $0F	|
	STA  $1D     ---+
	RTS	     ---* RETURN

L55:	LDY  $0601   ---* (Y) <- Cnt [15-8]
	INC  $0602   ---* Enable Command interrupts
	PHA	     ---+
	TXA		| Push (A) and (X)
	PHA	     ---+
	STY  $41     ---* zindexEnc <- Cnt [15-8]
	LDA  #$10    ---+
	BIT  $01	| if (HALF_COUNT)
	BEQ  L56	|   Clear LSB of zindexEnc
	TYA		|
	AND  #$FE	|
	STA  $41     ---+
L56:	LDA  #$00    ---+
	STA  $45	| Clear zindexCnt
	STA  $46     ---+
	LDA  #$01    ---+
	BIT  $01	| if (LOOK_ZINDEX)
	BEQ  L60	|   (52) <- rawEnc2 - lastRawEnc
	LDX  #$00	|   (53) <- signextnd (52)
	SEC		|
	LDA  $1F	|
	SBC  $20	|
	STA  $52	|
	BPL  L57	|
	LDX  #$FF	|
L57:	STX  $53     ---+
	LDX  #$00    ---+
	SEC		|   (A) <- (Y) - rawEnc2
	TYA		|   (X) <- signextnd (A)
	SBC  $1F	|
	BPL  L58	|
	LDX  #$FF    ---+
L58:	CLC          ---+
	ADC  $52	|   temp <- temp + (X,A)
	STA  $52	|
	TXA		|
	ADC  $53	|
	STA  $53     ---+
	LDA  #$10    ---+
	BIT  $01	|   if (HALF_COUNT)
	BEQ  L59	|     temp <- temp / 2
	LDA  $53	|
	CMP  #$80	|
	ROR  $53	|
	ROR  $52     ---+
L59:	CLC	     ---+
	LDA  $2A	|
	ADC  $52	|   stop_pos <- x + temp
	STA  $43	|
	LDA  $2B	|
	ADC  $53	|
	STA  $44     ---+
	LDA  $43     ---+   (Y,A) <- stop_pos
	LDY  $44     ---+
	JSR  SUB10   ---*   Stop at position stop_pos
	DEC  $0602   ---*   Disable Command interrupts
	INC  $01     ---*   INDEX_FOUND = true
	LDA  #$F7    ---+
	AND  $00	|   LOST_SYNC = false
	STA  $00     ---+
	LDA  #$03    ---+   Enable Command interrupts
	ORA  $0602	|     and request attention
	STA  $0602   ---+
	JMP  L62     ---*   GOTO L62
L60:	LDA  #$08    ---+ else (not looking for index)
	BIT  $00	|
	BNE  L62	| if (not LOST_SYNC)
	BIT  $50	|   if (not FOOBAR)
	BMI  L61        |     FOOBAR = true
	LDA  #$80	|
	ORA  $50	|
	STA  $50	|
	BNE  L62     ---+
L61:	SEC	     ---+   else
	LDA  $41	|     (Y),(A) <- zindexEnc - zindexLast
	SBC  $42	|
	TAY	     ---+
	SEC	     ---+
	SBC  $09	|     (A) <- (A) - zSize + 0x04
	CLC		|
	ADC  #$04    ---+
	AND  #$F8    ---+     if ( A[7-3] = 0)
	BEQ  L62     ---+       GOTO L62
	TYA	     ---+
	CLC		|     (A) <- (Y) + zSize + 0x04
	ADC  $09	|
	CLC		|
	ADC  #$04    ---+
	AND  #$F8    ---+     if ( A[7-3] = 0)
	BEQ  L62     ---+	GOTO L62
	TYA	     ---+
	CLC		|     (A) <- (Y) + 0x04
	ADC  #$04    ---+
	AND  #$F8    ---+     if ( A[7-3] = 0)
	BEQ  L62     ---+	GOTO L62
	LDA  #$08    ---+     
	JSR  SUB12   ---+     Bail out with LOST_SYNC
L62:	LDA  $41     ---+
	STA  $42     ---+ zindexLast <- zindexEnc
	JMP  L90     ---* RETURN from interrupt

	POSITION, CURRENT >

	LDA  $01     ---+ if (not SERVO_ENABLE or ZINDEX_FOUND),
	AND  #$82	| 
	CMP  #$80	|   Don't do anything
	BNE  L68     ---+
	LDA  $07     ---+
	BNE  L64	| if (tickCnt = 0)
	LDA  $9C	|   if ( (9C) >= 0xA0 )
	CMP  #$A0	|     tickRate <- 0xDB
	BMI  L63	|   else if ( (9C) < 0xA0 )
	LDA  #$DB	|     tickRate <- 0xDC
	STA  $08	|
	JMP  L64	|
L63:	CMP  #$90	|
	BPL  L64	|
	LDA  #$DC	|
	STA  $08     ---+

	STOPM, CALIB, TOLBND, INTBND, SETPAR, STOPP,
	GRAVITY, UNKNOWN >

L64:	LDX  $31     ---+ 
	LDA  $0400	| Read data into the dataBuf, and the
	STA  $33,X	| command into the commandBuf.
	LDA  $0403	| 
	STA  $37,X	|
	STY  $3B,X   ---+
	DEX	     ---+ Advance dataPtr;
	BPL  L65	| if (dataPtr < 0)
	LDX  #$03	|   dataPtr <- 03
L65:	STX  $31     ---+
	INC  $30     ---* commandRequest <- commandRequest + 1
	JMP  L90     ---* Goto to interrupt return

	READM>

	LDX  $02     ---+
	LDA  $00,X	| Load memory word pointed to
	LDY  $01,X	| by memIndex into (Y,A) for output
	INX		| to the bus.
	INX		|
	STX  $02	|
	JMP  L66     ---+

	JSTAT>

	LDA  $0602   ---+
	AND  #$FD	| Load jstat into (Y,A) for
	STA  $0602	| output.
	LDA  $00	| ATTENTION_REQ = false
	LDY  $01	|
	JMP  L66     ---+

	READ>

	LDA  $2A     ---+ (Y,A) <- x
	LDY  $2B     ---+

	Output to the bus:

L66:	STA  $0400   ---+ 
	STY  $0403	| Write (Y,A) out onto the bus
	JMP  L90     ---+

	READP>

	LDX  #$2C    ---+
L67:	DEX		| Command doesn't do anything.
	BNE  L67	| Just waits a bit and then
	LDA  $01	| sends back high byte of jstat
	STA  $0401   ---+

	unused>

L68:	LDA  $0402   ---+ Aknowledge data
	JMP  L90     ---* GOTO interrupt return

	position routine>
	
	JSR  SUB6    ---+ Call SUB6 and continue
	JMP  L70     ---+

	current routine>

	JSR  SUB7    ---+ Call SUB7 and continue
	JMP  L70     ---+

	gravity routine>

	LDA  $33,X   ---+
	STA  $69	| gravityOffset <- dataBuf [dataPtr]
	LDA  $37,X	|
	STA  $6A	|
	JMP  L71     ---+

	stopm, calib>

	LDA  #$02    ---+
	BIT  $01	| if (ZINDEX_FOUND)
	BEQ  L69	|   Call SUB8
	JSR  SUB8	| else
	JMP  L70	|   Call "stop" routine 9
L69:	LDA  $33,X	|
	LDY  $37,X	|
	JSR  SUB9	|
	JMP  L70     ---+

	stopp>

	LDA  $2A     ---+
	LDY  $2B	| (Y,A) <- x
	JSR  SUB9	| Call 'Stop' routine
	JMP  L70     ---+

	tolbnd>

	LDA  $33,X   ---+
	STA  $49	| posTolerence <- dataBuf [dataPtr]
	LDA  $37,X	|
	STA  $4A	|
	JMP  L70     ---+

	unknown>

	LDA  #$00    ---+ 
	STA  $00	| Clear low byte of jstat;
	LDA  #$FD	| ATTENTION_REQ = false
	AND  $0602	|
	STA  $0602      |
	JMP  L71     ---+

	intbnd>

	LDA  $33,X   ---+
	STA  $4B	| intBand <- dataBuf [dataPtr]
	LDA  $37,X	|  (not used)
	STA  $4C	|
	JMP  L71     ---+

	setpar>

	LDY  $37,X   ---+
	LDA  $33,X	| [Y] <- dataBuf [dataPtr]
	STA  $0000,Y    |
	JMP  L71     ---+

	unused, read, jstat, readp, readm>

L70:	LDX  $32     ---+
L71:	DEX		|
	BPL  L72	| Advance the commandPtr
	LDX  #$03	|
L72:	STX  $32     ---+
	DEC  $30     ---* commandRequest <- commandRequest - 1
	JMP  L90     ---* GOTO interrupt return


SUB5:	LDX  #$FD    ---+
	LDA  #$00	| Clear vd,vdInc,xdInc,(24)
L73:	STA  $0D,X	|
	STA  $24,X	|
	STA  $16,X	|
	STA  $13,X	|
	INX		|
	BNE  L73	|
	STA  $24     ---+
	LDA  $2A     ---+
	STA  $25	| xd <- x
	LDA  $2B	|
	STA  $26     ---+
	LDX  #$8C    ---+
L74:	DEX		| wait a bit ...
	BNE  L74     ---+
	RTS	     ---* RETURN

	Subroutine to process a new position request:

SUB6:	LDA  $01     ---+ if (not SERVO_ENABLE or ZINDEX_FOUND),
	AND  #$82	|
	CMP  #$80	|   Don't do anything
	BEQ  L75	|
	JMP  L83     ---+
L75:	LDA  #$00    ---+
	STA  $24	| clear low order bytes of xd,
	STA  $21	|    xdInc, and vdInc
	STA  $13     ---+
	LDA  $16     ---+
	STA  $52	| temp = xDelta
	LDA  $17	|
	STA  $53     ---+
	SEC	     ---+
	LDA  $33,X	| xGoal <- dataBuf [dataPtr]
	STA  $5D	| xDelta <- xGoal - xd
	SBC  $25	| xdInc <- xDelta / ticksPerSample
	STA  $22	|
	STA  $16	|
	LDA  $37,X	|
	STA  $5E	|
	SBC  $26	|
	STA  $17	|
	LDY  $5A	|
L76:	CMP  #$80	|
	ROR		|
	ROR  $22	|
	ROR  $21	|
	DEY		|
	BNE  L76	|
	STA  $23     ---+
	LDA  $59     ---+ tickCnt <- ticksPerSample
	STA  $07     ---+
	SEC	     ---+
	LDA  $16	| accelEst = xDelta - previous xDelta
	SBC  $52	|
	STA  $52	|
	STA  $64	|
	TAY		|
	LDA  $17	|
	SBC  $53	|
	STA  $53	|
	STA  $65     ---+
	BPL  L77     ---+
	SEC		| 
	LDA  #$00	|
	SBC  $52	|
	TAY		|
	LDA  #$00	|
	SBC  $53	|
L77:	STA  $53	|
	STY  $52        |
	CMP  #$02       |
	BCS  L79	| if ( ABS(accelEst) < 0x200 )
	LDA  $5F	|   if ( accelCnt != 0 )
	BEQ  L78	|     accelCnt <- accelCnt - 1
	DEC  $5F	|   if ( accelCnt = 0 )
	BNE  L80	|     Clear Opflag 5
L78:	LDA  $50	|
	AND  #$DF	| else
	STA  $50	|   Set Opflag 5
	JMP  L80	|   accelCnt <- 0x0A
L79:	LDA  $50	|
	ORA  #$20	|
	STA  $50	|
	LDA  #$0A	|
	STA  $5F     ---+
L80:	SEC	     ---+
	LDA  #$00	| 
	SBC  $10	| vdInc <- xDelta - vd
	STA  $13	|
	LDA  $16	|
	SBC  $11	|
	STA  $14	|
	LDA  $17	|
	SBC  $12	|
	STA  $15     ---+
	LDX  $5A     ---+
	LDA  $15	|
L81:	CMP  #$80	| vdInc <- vdInc / ticksPerSample
	ROR		|
	ROR  $14	|
	ROR  $13	|
	DEX		|
	BNE  L81	|
	STA  $15     ---+
	LDA  $50     ---+
	ORA  #$40	| Set Opflag 6
	STA  $50     ---+
	LDA  $0602   ---+
	AND  #$DB	| Clear some Control register 
	STA  $0602   ---+ bits
	LDA  #$00    ---+
	STA  $63	| accelEst /= 8
	LDX  #$03	| 
	LDA  $65	|
L82:	CMP  #$80	|
	ROR		|
	ROR  $64	|
	ROR  $63	|
	DEX		|
	BNE  L82	|
	STA  $65     ---+
L83:	RTS	     ---* RETURN

	Routine to process a set current request:

SUB7:	LDY  $33,X   ---+
	LDA  $37,X	| DACvalue <- dataBuf [dataPtr]
	STA  $2E	|
	STY  $2D     ---+
	LDX  #$00    ---+
	LDA  $2E	| (2E) <- signextnd (DACvalue)
	BPL  L84	|
	LDX  #$FF	|
L84:	STX  $2F     ---+
	LDX  $2E     ---+ Send the current to the DAC
	JSR  $0A90   ---+
	LDA  $50     ---+
	AND  #$BF	| Clear Opflag 6
	STA  $50     ---+
	LDA  $0602   ---+ Clear bits 2, 5 and set bits
	AND  #$DB	|  4, 3 in Control register
	ORA  #$18	|
	STA  $0602   ---+
	RTS	     ---* RETURN

SUB8:	SEC	     ---+
	LDA  $33,X	| temp <- dataBuf [dataPtr]
	TAY		| temp <- temp - stop_pos
	SBC  $43	| stop_pos <- temp
	STA  $52	|
	STY  $43	|
	LDA  $37,X	|
	TAY		|
	SBC  $44	|
	STA  $53	|
	STY  $44     ---+
	CLC	     ---+
	LDA  $52	| x <- x + temp
	ADC  $2A	|
	STA  $2A	|
	LDA  $53	|
	ADC  $2B	|
	STA  $2B     ---+
	LDA  $01     ---+
	AND  #$FC	| LOOK_ZINDEX, ZINDEX_FOUND = false
	STA  $01     ---+
	LDA  $43     ---+
	LDY  $44	| Call "Stop" routine
	JSR  SUB10   ---+
	RTS	     ---+ RETURN

	This is a "stop motion" routine

SUB9:	STA  $2A     ---+ (Y,A) <- xition
	STY  $2B     ---+
SUB10:	STA  $25     ---+ xd <- (Y,A)
	STY  $26     ---+
	LDA  #$19    ---+ Load Control register w/0x19
	STA  $0602   ---+
	LDA  $50     ---+
	ORA  #$40	| Set Opflag 6
	STA  $50     ---+
	LDA  #$00    ---+
	STA  $24	| Clear vd,vdInc,xdInc,(24)
	LDX  #$02	|
L85:	STA  $21,X	|
	STA  $13,X	|
	STA  $10,X	|
	DEX		|
	BPL  L85     ---+
	RTS	     ---* RETURN

	Subroutine compares word pointed to by X, with the absolute
	value of the position error. The status bits may be examined
	by the calling program after the subroutine returns.

SUB11:  LDA  $29     ---+
	BMI  L86	| if (xErr => 0)
	LDA  $00,X	|   COMPARE ( w[X], xErr )
	CMP  $28	|
	LDA  $01,X	|
	SBC  $29	|
	JMP  L87     ---+
L86:	CLC	     ---+
	LDA  $00,X	| else
	ADC  $28	|   COMPARE ( w[X], -xErr )
	LDA  $01,X	|
	ADC  $29     ---+
L87:	RTS	     ---* RETURN

	"Ring for help" routine. Checks to see if the flag parameter
	passed in A is set in (00). If not, it is set, and an
	ATTENTION REQUEST is signalled in the Control register.

SUB12:	BIT  $00     ---+
	BEQ  L88	| if (flag not set in low byte of jstat)
	JMP  L89	|   set the flag
L88:	ORA  $00	|   set ATTN REQ
	STA  $00	|
	LDA  #$02	|
	ORA  $0602	|
	STA  $0602   ---+
L89:	RTS	     ---* RETURN

Return from interrupt sequence:

L90:	PLA	     ---+
	TAX		| Pop (X),(Y),(A)
	PLA		|
	TAY		|
	PLA	     ---+
	RTI          ---* RETURN from interrupt

NMI interrupt dispatch table: 
Entry point for a particular joint command = x0D00 + offset

      Offset    Command

	00 	CURRENT
	21	STOPM
	21	GRAVITY
	21 	SETPAR
	21      UNKNOWN
	6E 	unused
	48 	JSTAT
	3B	READM
	00	POSITION
	21 	TOLBND
	21	CALIB
	21 	INTBND
	21	STOPP
	6E 	unused
	57 	READ
	64	READP

Command routine table. Each joint command is also processed 
later by a routine in this table. The entry point for the 
routine is given by entry_point = x0D00 + offset.

      Offset    Command

	7A	CURRENT
	8B	STOPM
	80	GRAVITY
	D0 	SETPAR
	B6	UNKNOWN
	DA	unsued
	DA	JSTAT
	DA	READM
	74	POSITION
	AB	TOLBND
	8B	CALIB
	C5 	INTBND
	A1	STOPP
	DA	unused
	DA	READ
	DA	READP

Initial memory table: Various memory locations are initialized
according to this table, which is set up as a list of pairs
of (ADDRESS, VALUE).

      Address  Value

	03	00
	04 	03
	5C 	01
	05 	07
	06 	05
	57 	03
	54 	02
	55 	03
	56 	04
	58 	05 
	08 	DC
	40 	0D 
	59 	20
	5A 	05 
	49 	14
	4A 	00
	4B 	46 
	4C 	00
	47 	08
	48 	07
	80 	00
	82 	00
	86 	00
	81 	0F
	83 	FF


   0FF9 00
   0FFA 53
   0FFB 08
   0FFC 00
   0FFD 08
   0FFE 69 08
   1000 FF
		     MEMORY MAP FOR THE PUMA 260 MICROPROCESSORS
		     ___________________________________________

	00	jstat(2): Joint status register
	01
	02	memIndex(1): Used by external commands to reference memory
*	03  00  kGain(1): Position error servo gain
	04  03  cc(1): Control constant (not used)
	05  07  integralGain(1): Control constant stored in log 2 form
	06  05  oGain(1): Final output gain.
	07	tickCnt(1): Number of ticks left in present sample period
	08  DC  tickRate(1): Time count between tick interrupts.
	09      zSize(1): Number of encoder counts between zero indexes.
	0A	integral(3): Time integral of (position error / integral gain)
	0B
	0C
*	0D	vEst(3): initially set to encoder change between ticks. Is
        0E         later massaged into an estimate of the current velocity
        0F         by averaging it in with vAvg, and finally is combined with
                   vd to produce the velocity error term

*	10	vd(3): desired velocity, interpolated for each tick
	11
	12
*	13	vdInc(3): per-cycle change in the velocity setpoint
	14
	15
	16	xDelta(2): desired position change for the current cycle
	17
	18	vAvg(3): a decaying time average of the velocity
	19	
	1A
	1B	vEstSave(3): used to store the velocity estimate 'vEst'.
	1C	   The stored value does not appear to be used.
	1D
	1E	rawEnc1(1): 1st time sample of 8 bit encoder value.
	1F	rawEnc2(1): 2nd time sample of 8 bit encoder value.
	20	lastRawEnc(1): Previous first time sample of 8 bit encoder

	21	xdInc(3): per-cycle change in position setpoint
	22
	23
	24	xd(3): interpolated position setpoint for each tick
	25
	26
	27	xErr(3): Position error; used in computing integral term
	28	
	29
	2A	x(2): Current robot position (updated each tick)
	2B
	2C	DACvalue(4): current setpoint for DAC
	2D
	2E
	2F


	30	commandRequest(1): Number of outstanding input commands
	31	dataPtr(1): pointer to beginning of data buffer
	32	commandPtr(1): pointer to beginning of command buffer
	33	dataBuf(8): Data input buffer
	34
	35
	36
	37
	38
	39
	3A
	3B	commandBuf(4): Command input buffer
	3C
	3D
	3E

	3F	jumpAddress(2): For computing jump table addresses
	40  0D
	41	zindexEnc(1): Raw encoder value at present zero index
	42	zindexLast(1): Raw encoder value at last zero index
	43	stopPosition(2): used to store the reference point for
	44	   when the joint is brought to a halt
	45      zindexCnt(2): Number of encoder counts since last zero index.
	46
	47  08  posEnvelope(2): maximum desirable encoder count error
	48  07
	49  14	posTolerence(2): Position tolerance. 
	4A  00
	4B  46	intBand(2): Integration band variable. Not used.
	4C  00
	4D	DACtimeout(2): timeout counter for DAC output saturation
	4E
	4F	overheatTimeout(1): Timeout counter for motor overheat signal

	50	opFlags(1): Operational flags
	51	temp(3): Scratch space
	52
	53
	54  02	vEstScale1(1): averaging constant to est. velocity from vAvg
		   and encoder deltas, stored as fraction (log 2)
	55  03	vEstScale2(1): an alternative value for vEstScale 
	56  04	vAvgWin(1): size of window for decaying velocity average
                   (log 2 form)
	57  03	***
	58  05	***
	59  20  ticksPerSample(1): number of ticks per cycle period
	5A  05  log2ticks(1): log 2 of number of ticks per cycle
	5B	***
	5C  01	vGain(1): Velocity servo gain
	5D	xGoal(2): goal position for the present sample.
	5E	
	5F	accelCnt(1): no. of cycles in accelTreshold refresh period

	60	???:
	61	xErrSave(2): Saved value of xErr, which is not used
	62
	63	accelEst(3): per-cycle estimate of the desired acceleration
	64
	65
	66	***
	67	***
	68	???: Some sort of counter
	69	gravityOffset(2): gravity loading torgue offset. Not used.
	6A		Loaded by external command.
	6B	STACK SPACE -->
	6C	
	6D
	6E
	6F

	70
	71
	72
	73
	74
	75
	76
	77
	78
	79
	7A
	7B
	7C
	7D
	7E
	7F

				FLAG DEFINITIONS


	For jstat:
	
	Bit 0	MEMORY_FAILURE -- error occured in ram
	    1	NO_ZERO_INDEX  -- failed to find a zero index
	    2	OUT_OF_ENVELOPE - posser error went outside of it's limit
	    3	LOST_SYNC ------- lost sync with optical encoder
	    4	DAC_SATURATE ---- DAC is staturated
	    5	OVERHEATED ------ overheated motor (not supported yet)
	    
	    8   LOOK_ZINDEX ----- looking for zero index
	    9	ZINDEX_FOUND ---- zero index has been found
	   12	HALF_COUNT ------ position being computed with 1/2 precision
	   14	INTEGRATION ----- integration servoing enabled
	   15	SERVO_ENABLE ---- servoing is enabled

	For opflags:
	
	    5	ACCELERATING ---- joint has been accelerating recently
	    6	SERVOING -------- joint is position servoing
*	    7	FOOBAR ---------- Not sure. Doesn't seem to be used

	For control_reg:
	
	    0	NMI_ENABLE ------ enables non maskable interrupts
	    1	ATTENTION_REQ --- request attention from master CPU
*	    2	
*	    3
*	    4
*	    5
	    7	TOLERENCE ------- set when joint is within posTolerence
