  	disassembled rom 0x2000 - 0x27ff 	

	0800 d8         cld 		--* Clear decimal mode
	0801 a9 00      lda #$00	--\
	0803 a2 7f      ldx #$7f	  | Clear memory locations 01 - 7f and
L0      0805 95 00      sta $00,x	  | test to make sure they contain 0.
	0807 d5 00      cmp $00,x	  |
	0809 d0 06      bne L1      	  | If not, signal memory failure.
	080b ca         dex 		  | 
	080c 10 f7      bpl L0      	  |
	080e 4c 16 08   jmp L2      	  |
L1      0811 a9 01      lda #$01	  |
	0813 20 44 0f   jsr SUB11   	--/
L2      0816 a2 7e      ldx #$7e	--\ Initialize the stack to 0x7e
	0818 9a         txs 		--/
	0819 a0 00      ldy #$00	--\
	081b f0 07      beq L4      	  | 
L3      081d c8         iny 		  | Initialize memory using the 
	081e b9 7e 0f   lda $0f7e,y	  | address - value table at address 
	0821 95 00      sta $00,x	  | 0xf7e.
	0823 c8         iny 		  | 
L4      0824 be 7e 0f   ldx $0f7e,y	  |
	0827 e0 ff      cpx #$ff	  |
	0829 d0 f2      bne L3      	--/
	082b ad 00 06   lda $0600	--\
	082e 85 20      sta $20		  | rawEnc2, lastRawEnc = cnt [7-0]
	0830 85 21      sta $21 	--/
	0832 a5 23      lda $23		--\
	0834 a4 24      ldy $24		  | STOP at x
	0836 20 21 0f   jsr SUB9    	--/
	0839 a5 17      lda $17		--\ PIO_timer = tickRate
	083b 85 9e      sta $9e		--/
	083d 58         cli 		--* Enable PIO interrupts
L5      083e 4c 3e 08   jmp L5      	--* Wait for some action

	Prime interrupt handler for joint commands (tied to the
	non-maskable interrupt line)

	0841 48         pha 		--\
	0842 98         tya 		  |
	0843 48         pha 		  | Push A, Y, X
	0844 8a         txa 		  |
	0845 48         pha 		--/
	0846 ad 03 06   lda $0603	--\
	0849 4a         lsr 		  |
	084a 4a         lsr 		  | Read the joint command off the bus
	084b 4a         lsr 		  | and jump to the appropriate 
	084c 29 0f      and #$0f	  | handler as indexed in the NMI
	084e a8         tay 		  | dispatch table
	084f b9 5e 0f   lda $0f5e,y	  |
	0852 85 53      sta $53		  |
	0854 6c 53 00   jmp ($0053)	--/
	
	Prime interrupt handler for timer interrupts (these are arbitrated
	by the PIO chip and are tied to the maskable interrupt line).

	0857 ce 02 06   dec $0602	--* Disable Command interrupts
	085a 48         pha 		--* Push A
	085b 98         tya 		--* A = Y
	085c 24 85      bit $85	        --\ if (zero index interrupt)
	085e 50 03      bvc L6      	  |    goto L52
	0860 4c d0 0b   jmp L52         --/

	On timer interrupt:

L6      0863 a4 17      ldy $17		--\ PIO_timer = tickRate
	0865 84 9e      sty $9e		--/
	0867 c6 18      dec $18		--* tickCnt = tickCnt - 1
	0869 ac 00 06   ldy $0600	--\
	086c ee 02 06   inc $0602	  | Reenable command interrupts
	086f 48         pha 		  |
	0870 8a         txa 		  | push A, X
	0871 48         pha 		  |
	0872 84 1f      sty $1f		  | rawEnc1 = cnt [7-0]
	0874 20 af 0a   jsr SUB2    	  | compute current encoder count
	0877 20 2f 0b   jsr SUB3        --/ compute velocity control term
	087a a9 40      lda #$40	--\
	087c 24 63      bit $63		  | if (opFlags SERVOING)
	087e d0 06      bne L7      	  |    do position control
	0880 20 c9 0d   jsr SUB5    	  | else
	0883 4c 89 08   jmp L8      	  |    do current control
L7      0886 20 a3 08   jsr SUB0    	--/
L8      0889 e6 68      inc $68		--* time++
	088b ac 00 06   ldy $0600	--\ rawEnc2 = cnt [7-0]
	088e 84 20      sty $20		--/
	0890 a5 44      lda $44		--\
	0892 d0 03      bne L9      	  | if (commandRequest != 0)
	0894 4c 58 0f   jmp L91     	  |    lookup the appropriate routine
L9      0897 a6 46      ldx $46		  |    and jump to it
	0899 b4 4f      ldy $4f,x	  | else
	089b b9 6e 0f   lda $0f6e,y	  |    goto interrupt return
	089e 85 55      sta $55		  |
	08a0 6c 55 00   jmp ($0055)	--/

	Main position servo control routine:

SUB0    08a3 18         clc 		--\
	08a4 a5 1c      lda $1c		  |
	08a6 65 19      adc $19		  | /* Compute the next encoder and 
	08a8 85 1c      sta $1c		  |    velocity setpoints: */
	08aa a5 1d      lda $1d		  | 
	08ac 65 1a      adc $1a		  |
	08ae 85 1d      sta $1d		  | xd += xdInc;
	08b0 a5 1e      lda $1e		  |
	08b2 65 1b      adc $1b		  |
	08b4 85 1e      sta $1e		--/
	08b6 18         clc 		--\
	08b7 a5 2d      lda $2d		  |
	08b9 65 2a      adc $2a		  |
	08bb 85 2d      sta $2d		  | 
	08bd a5 2e      lda $2e		  | vd += vdInc;
	08bf 65 2b      adc $2b		  |
	08c1 85 2e      sta $2e		  |
	08c3 a5 2f      lda $2f		  |
	08c5 65 2c      adc $2c		  |
	08c7 85 2f      sta $2f		--/
	08c9 a5 18      lda $18		--\
	08cb d0 0e      bne L10     	  |
	08cd a9 00      lda #$00	  | if (tickCnt == 0)
	08cf 85 19      sta $19		  |  { /* then we are 'there': */
	08d1 85 1a      sta $1a		  |    xdInc = 0;
	08d3 85 1b      sta $1b		  |    vdInc = 0;
	08d5 85 2a      sta $2a		  |  }
	08d7 85 2b      sta $2b		  |
	08d9 85 2c      sta $2c		--/
L10     08db 38         sec 		--\
	08dc a5 22      lda $22		  |
	08de e5 1c      sbc $1c		  |
	08e0 85 25      sta $25		  |
	08e2 85 3e      sta $3e		  | 
	08e4 a5 23      lda $23		  | 
	08e6 e5 1d      sbc $1d		  | xErr = xErrSave = x - xd;
	08e8 85 26      sta $26		  | DACvalue = xErr;
	08ea 85 28      sta $28		  | 
	08ec 85 3f      sta $3f		  |
	08ee a5 24      lda $24		  |
	08f0 e5 1e      sbc $1e		  |
	08f2 85 27      sta $27		  | 
	08f4 85 29      sta $29		  |
	08f6 85 40      sta $40		--/
	08f8 a2 07      ldx #$07	--\ if (ABS(xErr) > posEnvelope)
	08fa 20 b7 0b   jsr SUB4    	  |    if (bit 13 of jstat is set OR
	08fd b0 11      bcs L12     	  |        opFlags ACCELERATING clear)
	08ff a9 20      lda #$20	  |       signal OUT_OF_ENVELOPE
	0901 24 01      bit $01		  |    
	0903 d0 06      bne L11     	  |    
	0905 a9 20      lda #$20	  |
	0907 24 63      bit $63		  |
	0909 d0 05      bne L12     	  |
L11     090b a9 04      lda #$04	  |
	090d 20 44 0f   jsr SUB11   	--/
L12     0910 a2 57      ldx #$57	--\ if (ABS(xErr) <= posTolerence)
	0912 20 b7 0b   jsr SUB4    	  |    clear control reg TOLERENCE;
	0915 b0 08      bcs L13     	  | else
	0917 ad 02 06   lda $0602	  |    set control reg TOLERENCE;
	091a 29 7f      and #$7f	  |
	091c 4c 24 09   jmp L14     	  |
L13     091f ad 02 06   lda $0602	  |
	0922 09 80      ora #$80	  |
L14     0924 8d 02 06   sta $0602	--/
	0927 24 01      bit $01		--\ 
	0929 70 1a      bvs L16     	  |     
	092b a6 34      ldx $34		  |    
	092d 30 0a      bmi L15     	  |    
	092f d0 14      bne L16     	  | 
	0931 a5 33      lda $33		  | 
	0933 c9 04      cmp #$04	  | 
	0935 10 0e      bpl L16     	  |    
	0937 30 56      bmi L21           |
L15     0939 e8         inx 		  | Compute integration control term
	093a d0 09      bne L16     	  | if necessary:
	093c 38         sec 		  | 
	093d a9 00      lda #$00	  | if (INTEGRATION is set OR
	093f e5 33      sbc $33		  |     ABS(vAvg) >= 0x4)
	0941 c9 04      cmp #$04	  |  {
	0943 30 4a      bmi L21         --/
L16     0945 a5 16      lda $16		--\ 
	0947 30 10      bmi L17     	  |
	0949 a9 ff      lda #$ff	  |    if ( ABS(integral) < 0x1fff OR
	094b c5 15      cmp $15		  |         sgn(xErr) != sgn(integral) )
	094d a9 1f      lda #$1f	  |
	094f e5 16      sbc $16		  | 
	0951 b0 17      bcs L18           |     { scale integration term and
	0953 a5 27      lda $27	  	  | 	  add it in:
	0955 30 13      bmi L18     	  |    
	0957 10 36      bpl L21		  | 
L17     0959 18         clc		  |
	095a a9 ff      lda #$ff	  | 
	095c 65 15      adc $15		  |   
	095e a9 1f      lda #$1f	  |
	0960 65 16      adc $16		  |
	0962 b0 06      bcs L18     	  |
	0964 a5 27      lda $27		  |
	0966 10 02      bpl L18           |
	0968 30 25      bmi L21         --/
L18     096a a5 27      lda $27		--\
	096c a6 0e      ldx $0e		  |
	096e f0 0c      beq L20     	  |       if (integralGain != 0)
L19     0970 c9 80      cmp #$80	  |          xErr >> integralGain
	0972 6a         ror 		  |
	0973 66 26      ror $26		  |
	0975 66 25      ror $25		  |
	0977 ca         dex 		  |
	0978 d0 f6      bne L19     	  |
	097a 85 27      sta $27		--/
L20     097c 18         clc 		--\
	097d a5 14      lda $14		  |
	097f 65 25      adc $25		  |
	0981 85 14      sta $14		  |       integral += xErr
	0983 a5 15      lda $15		  |     }
	0985 65 26      adc $26		  |  }
	0987 85 15      sta $15		  |
	0989 a5 16      lda $16		  |
	098b 65 27      adc $27		  |
	098d 85 16      sta $16		--/
L21     098f 18         clc 		--\
	0990 a5 3e      lda $3e		  |
	0992 65 14      adc $14		  |
	0994 85 3e      sta $3e		  | DACvalue += integral
	0996 a5 3f      lda $3f		  |
	0998 65 15      adc $15		  |
	099a 85 3f      sta $3f		  |
	099c a5 40      lda $40		  |
	099e 65 16      adc $16		  |
	09a0 85 40      sta $40		--/
	09a2 a2 00      ldx #$00	--\ 
	09a4 a5 40      lda $40		  | sign extend DACvalue to high byte
	09a6 10 02      bpl L22           |
	09a8 a2 ff      ldx #$ff 	  |
L22     09aa 86 41      stx $41		--/
	09ac a6 0d      ldx $0d		--\
	09ae f0 0e      beq L24     	  | if (kGain != 0)
	09b0 a5 3e      lda $3e		  |    DACvalue << kGain
L23     09b2 0a         asl 		  |
	09b3 26 3f      rol $3f		  |
	09b5 26 40      rol $40		  |
	09b7 26 41      rol $41		  |
	09b9 ca         dex 		  |
	09ba d0 f6      bne L23     	  |
	09bc 85 3e      sta $3e		  |
L24     09be 38         sec 		--/
	09bf a5 35      lda $35		--\
	09c1 85 69      sta $69		  | vEstSave = vEst
	09c3 e5 2d      sbc $2d		  | vEst -= vd
	09c5 85 35      sta $35		  | vErrSave = vEst
	09c7 85 39      sta $39		  |
	09c9 a5 36      lda $36		  |
	09cb 85 6a      sta $6a		  |
	09cd e5 2e      sbc $2e		  |
	09cf 85 36      sta $36		  |
	09d1 85 3a      sta $3a		  |
	09d3 a5 37      lda $37		  |
	09d5 85 6b      sta $6b		  |
	09d7 e5 2f      sbc $2f		  |
	09d9 85 37      sta $37		  |
	09db 85 3b      sta $3b		--/
	09dd a2 00      ldx #$00	--\
	09df a5 37      lda $37		  | sign extend vEst to high byte
	09e1 10 02      bpl L25     	  |
	09e3 a2 ff      ldx #$ff	  |
L25     09e5 86 38      stx $38		--/
	09e7 a6 11      ldx $11		--\
	09e9 f0 0e      beq L27     	  | if (vGain != 0)
	09eb a5 35      lda $35		  |    vEst << vGain
L26     09ed 0a         asl 		  |
	09ee 26 36      rol $36		  |
	09f0 26 37      rol $37		  |
	09f2 26 38      rol $38		  |
	09f4 ca         dex 		  |
	09f5 d0 f6      bne L26     	  |
	09f7 85 35      sta $35		--/
L27     09f9 18         clc 		--\
	09fa a5 3e      lda $3e		  |
	09fc 65 35      adc $35		  | DACvalue += vEst
	09fe 85 3e      sta $3e		  |
	0a00 a5 3f      lda $3f		  |
	0a02 65 36      adc $36		  |
	0a04 85 3f      sta $3f		  |
	0a06 a5 40      lda $40		  |
	0a08 65 37      adc $37		  |
	0a0a 85 40      sta $40		  |
	0a0c a5 41      lda $41		  |
	0a0e 65 38      adc $38		  |
	0a10 85 41      sta $41		--/
	0a12 a2 00      ldx #$00	--\
	0a14 a5 3d      lda $3d		  | sign extend accelEst into temp
	0a16 10 02      bpl L28     	  |    for next subtraction
	0a18 a2 ff      ldx #$ff	  |
L28     0a1a 86 66      stx $66		--/
	0a1c 38         sec 		--\
	0a1d a5 3f      lda $3f		  | DACvalue = DACvalue - accelEst
	0a1f e5 3c      sbc $3c		  |
	0a21 85 3f      sta $3f		  |
	0a23 a5 40      lda $40		  |
	0a25 e5 3d      sbc $3d		  |
	0a27 85 40      sta $40		  |
	0a29 a5 41      lda $41		  |
	0a2b e5 66      sbc $66		  |
	0a2d 85 41      sta $41		--/
	0a2f a6 13      ldx $13		--\
	0a31 8a         txa 		  |
	0a32 f0 10      beq L30     	  | if (oGain != 0)
	0a34 a5 41      lda $41     	  |    DACvalue >> oGain
L29     0a36 c9 80      cmp #$80     	  |   
	0a38 6a         ror      	  |
	0a39 66 40      ror $40     	  |
	0a3b 66 3f      ror $3f     	  |
	0a3d 66 3e      ror $3e     	  |
	0a3f ca         dex      	  |
	0a40 d0 f4      bne L29           |
	0a42 85 41      sta $41     	--/
L30     0a44 a4 3f      ldy $3f		--\ 
	0a46 a6 40      ldx $40		  | output DAC values
	0a48 20 4c 0a   jsr SUB1    	--/
	0a4b 60         rts 		--* return

	DAC output signal routine. Assumes that the signal value is passed in
	registers X and Y.

SUB1    0a4c 8a         txa 		--\ Clip signal value if necessary:
	0a4d 2a         rol 		  | 
	0a4e a5 41      lda $41		  | The signal value is stored in
	0a50 69 00      adc #$00	  | [2f, X, Y]. 
	0a52 f0 06      beq L31		  | 
	0a54 a5 41      lda $41		  | if (signal value >= 0x7fe)
	0a56 10 0f      bpl L32     	  |    [X, Y] = 0x7fe
	0a58 30 1c      bmi L34     	  | else if (signal value < -0x800)
L31     0a5a a5 41      lda $41		  |    [X, Y] = -0x800
	0a5c 30 0f      bmi L33     	  |
	0a5e 38         sec 		  |
	0a5f 98         tya 		  |
	0a60 e9 fe      sbc #$fe	  |
	0a62 8a         txa 		  |
	0a63 e9 07      sbc #$07	  |
	0a65 30 26      bmi L36	  	  |
L32     0a67 a0 fe      ldy #$fe	  |
	0a69 a2 07      ldx #$07	  |
	0a6b d0 0d      bne L35     	  |
L33     0a6d 38         sec		  |
	0a6e 98         tya 		  |
	0a6f e9 00      sbc #$00	  |
	0a71 8a         txa 		  |
	0a72 e9 f8      sbc #$f8	  |
	0a74 10 17      bpl L36     	  |
L34     0a76 a0 00      ldy #$00	  |
	0a78 a2 f8      ldx #$f8	--/
L35     0a7a e6 42      inc $42		--\ if (signal value was clipped)
	0a7c d0 15      bne L37           |    DACtimeout++
	0a7e e6 43      inc $43		  |    if (DACtimeout > 0x700)
	0a80 a9 07      lda #$07	  |	  signal DAC_SATURATE
	0a82 c5 43      cmp $43		  |
	0a84 d0 0d      bne L37     	  |
	0a86 a9 10      lda #$10	  |
	0a88 20 44 0f   jsr SUB11         |
	0a8b d0 06      bne L37     	  | else
L36     0a8d a9 00      lda #$00	  |    DACtimeout = 0
	0a8f 85 42      sta $42		  |
	0a91 85 43      sta $43		--/
L37     0a93 84 66      sty $66		--\
	0a95 8a         txa 		  | All 12 bits of the DAC signal are
	0a96 06 66      asl $66		  | in [X, Y].
	0a98 2a         rol 		  | 
	0a99 06 66      asl $66		  | Shift the bits around so that
	0a9b 2a         rol 		  | X contains the high order 8 bits,
	0a9c 06 66      asl $66		  | and the A contains the low order
	0a9e 2a         rol 		  | 4 bits.
	0a9f 06 66      asl $66		  |
	0aa1 2a         rol 		  |
	0aa2 aa         tax 		  |
	0aa3 98         tya 		--/
	0aa4 45 80      eor $80		--\ write X and A into the DAC 
	0aa6 29 0f      and #$0f	  | registers 82 and 80 such that the
	0aa8 45 80      eor $80		  | high 4 bits of 80 are preserved.
	0aaa 85 80      sta $80		  |
	0aac 86 82      stx $82		--/
	0aae 60         rts 		--* return

	Subroutine to compute the current encoder value:

SUB2    0aaf a2 00      ldx #$00	--\ clear low byte of vEst
	0ab1 86 35      stx $35		--/
	0ab3 38         sec 		--\
	0ab4 a5 20      lda $20		  | temp = rawEnc2 - lastRawEnc
	0ab6 e5 21      sbc $21		  | Calculate 'vEst' using the
	0ab8 85 66      sta $66		  | last raw encoder count, and the 
	0aba 10 02      bpl L38     	  | intermediate count 'rawEnc2':
	0abc a2 ff      ldx #$ff	  | 
L38     0abe 86 67      stx $67		  | vEst =  (rawEnc2 - lastRawEnc)
	0ac0 a2 00      ldx #$00	  |	       + (rawEnc1 - rawEnc2)
	0ac2 38         sec 		  | 
	0ac3 a5 1f      lda $1f		  | (Using the extra sample 'rawEnc2' 
	0ac5 e5 20      sbc $20		  | increases the maximum observable
	0ac7 10 02      bpl L39     	  | difference from 128 to 256.)
	0ac9 a2 ff      ldx #$ff	  |
L39     0acb 18         clc 		  |
	0acc 65 66      adc $66		  |
	0ace 85 36      sta $36		  |
	0ad0 8a         txa 		  |
	0ad1 65 67      adc $67		  |
	0ad3 85 37      sta $37		--/
	0ad5 18         clc 		--\
	0ad6 a5 5e2     lda $5e		  |  zindexCnt += vEst
	0ad8 65 36      adc $36		  |
	0ada 85 5e      sta $5e		  |
	0adc a5 5f      lda $5f		  |
	0ade 65 37      adc $37		  |
	0ae0 85 5f      sta $5f		--/
	0ae2 30 0c      bmi L40     	--\
	0ae4 a9 00      lda #$00	  | if (vEst >= 0)
	0ae6 c5 5e      cmp $5e		  |    if (ABS(zindexCnt) > 0x1a00)
	0ae8 a9 1a      lda #$1a	  |	  signal NO_ZERO_INDEX
	0aea e5 5f      sbc $5f		  |    
	0aec b0 12      bcs L42     	  |
	0aee 90 0b      bcc L41           |
L40     0af0 18         clc 		  |    
	0af1 a9 00      lda #$00	  |       
	0af3 65 5e      adc $5e		  |
	0af5 a9 1a      lda #$1a	  |
	0af7 65 5f      adc $5f		  |
	0af9 b0 05      bcs L42     	  |
L41     0afb a9 02      lda #$02	  |
	0afd 20 44 0f   jsr SUB11   	--/
L42     0b00 a4 06      ldy $06		--\
	0b02 f0 0d      beq L44     	  | if (encoderScale != 0)
	0b04 a5 37      lda $37		  |    vEst >> encoderScale
L43     0b06 c9 80      cmp #$80	  |
	0b08 66 37      ror $37		  |
	0b0a 66 36      ror $36		  |
	0b0c 66 35      ror $35		  |
	0b0e 88         dey 		  |
	0b0f d0 f5      bne L43     	--/
L44     0b11 a5 1f      lda $1f		--\ lastRawEnc = rawEnc1
	0b13 85 21      sta $21		--/
	0b15 ce 02 06   dec $0602	--* disable command interrupts
	0b18 18         clc 		--\
	0b19 a5 22      lda $22		  |
	0b1b 65 35      adc $35		  | x += vEst
	0b1d 85 22      sta $22		  |
	0b1f a5 23      lda $23		  |
	0b21 65 36      adc $36		  |
	0b23 85 23      sta $23		  |
	0b25 a5 24      lda $24		  |
	0b27 65 37      adc $37		  |
	0b29 85 24      sta $24		--/
	0b2b ee 02 06   inc $0602	--* renable command interrupts
	0b2e 60         rts 		--* return

	Computes average velocity and instantaneous acceleration:

SUB3    0b2f a6 0b      ldx $0b		--\
L45     0b31 06 35      asl $35		  | Scale velocity by the number of 
	0b33 26 36      rol $36		  | ticks per cycle:
	0b35 26 37      rol $37		  | 
	0b37 ca         dex 		  | vEst << ticksPerSample
	0b38 d0 f7      bne L45     	--/
	0b3a 38         sec 		--\
	0b3b a5 35      lda $35		  |
	0b3d e5 32      sbc $32		  | Update the average velocity term.
	0b3f 85 65      sta $65		  | This is computed as a decaying
	0b41 a5 36      lda $36		  | average with time constant 
	0b43 e5 33      sbc $33		  | 'vAvgWin':
	0b45 85 66      sta $66		  |
	0b47 a5 37      lda $37		  | 
	0b49 e5 34      sbc $34		  | 
	0b4b 85 67      sta $67		  | 
	0b4d a4 10      ldy $10		  | temp = (vEst-vAvg)
	0b4f f0 0e      beq L47     	  |        
	0b51 a5 67      lda $67		  | if (vAvgWin != 0)
L46     0b53 c9 80      cmp #$80	  |    temp >> vAvgWin
	0b55 6a         ror 		  |
	0b56 66 66      ror $66		  |
	0b58 66 65      ror $65		  |
	0b5a 88         dey 		  |
	0b5b d0 f6      bne L46     	  |
	0b5d 85 67      sta $67		--/
L47     0b5f 18         clc 		--\
	0b60 a5 65      lda $65		  |
	0b62 65 32      adc $32		  |
	0b64 85 32      sta $32		  | 
	0b66 a5 66      lda $66		  | vAvg += temp
	0b68 65 33      adc $33		  |
	0b6a 85 33      sta $33		  |
	0b6c a5 67      lda $67		  |
	0b6e 65 34      adc $34		  |
	0b70 85 34      sta $34		--/
	0b72 38         sec 		--\
	0b73 a5 35      lda $35		  |
	0b75 e5 32      sbc $32		  |
	0b77 85 35      sta $35		  | Compute acceleration:
	0b79 a5 36      lda $36		  |
	0b7b e5 33      sbc $33		  | vEst -= vAvg
	0b7d 85 36      sta $36		  |
	0b7f a5 37      lda $37		  |
	0b81 e5 34      sbc $34		  |
	0b83 85 37      sta $37		--/
	0b85 a6 0f      ldx $0f		--\
	0b87 86 66      stx $66		  |
	0b89 38         sec 		  |
	0b8a a5 10      lda $10		  | if (vAvgWin-vEstScale > 0)
	0b8c e5 66      sbc $66		  |    vEst >> (vAvgWin-vEstScale)
	0b8e f0 13      beq L49		  |
	0b90 30 11      bmi L49		  |
	0b92 aa         tax 		  |
	0b93 f0 0e      beq L49		  |
	0b95 a5 37      lda $37		  |
L48     0b97 c9 80      cmp #$80	  |
	0b99 6a         ror 		  |
	0b9a 66 36      ror $36		  |
	0b9c 66 35      ror $35		  |
	0b9e ca         dex 		  |
	0b9f d0 f6      bne L48		  |
	0ba1 85 37      sta $37		--/
L49     0ba3 18         clc 		--\
	0ba4 a5 35      lda $35		  | vEst += vAvg
	0ba6 65 32      adc $32		  |
	0ba8 85 35      sta $35		  |
	0baa a5 36      lda $36		  |
	0bac 65 33      adc $33		  |
	0bae 85 36      sta $36		  |
	0bb0 a5 37      lda $37		  |
	0bb2 65 34      adc $34		  |
	0bb4 85 37      sta $37		--/
	0bb6 60         rts 		--* return

	Compares the 2 byte word pointed to by X with ABS(xErr):

SUB4    0bb7 a5 27      lda $27		--\ 
	0bb9 30 0b      bmi L50     	  | 
	0bbb b5 00      lda $00,x	  | 
	0bbd c5 26      cmp $26		  |
	0bbf b5 01      lda $01,x	  | if ( (X) >= ABS (xErr) )
	0bc1 e5 27      sbc $27		  |    set carry bit
	0bc3 4c cf 0b   jmp L51     	  | else
L50     0bc6 18         clc 		  |    clear carry bit
	0bc7 b5 00      lda $00,x	  |
	0bc9 65 26      adc $26		  |
	0bcb b5 01      lda $01,x	  |
	0bcd 65 27      adc $27		  |
L51     0bcf 60         rts 		--/

	On zero index interrupt:

L52     0bd0 ac 01 06   ldy $0601	--* Y = cnt [15-8]
	0bd3 ee 02 06   inc $0602	--* enable Command interrupts
	0bd6 48         pha 		--\
	0bd7 8a         txa 		  | push A and X
	0bd8 48         pha 		--/
	0bd9 84 5b      sty $5b		--* zindexEnc = cnt [15-8]
	0bdb a5 5e      lda $5e		--\
	0bdd 85 60      sta $60		  | oldZindexCnt = zindexCnt
	0bdf a5 5f      lda $5f		  |
	0be1 85 61      sta $61		--/
	0be3 a9 00      lda #$00	--\
	0be5 85 5e      sta $5e		  | zindexCnt = 0
	0be7 85 5f      sta $5f		--/
	0be9 a9 01      lda #$01	--\
	0beb 24 01      bit $01		  | if (LOOK_ZINDEX)
	0bed f0 64      beq L57     	  |    temp = rawEnc2 - lastRawEnc
	0bef a2 00      ldx #$00	  |	
	0bf1 86 65      stx $65		  |
	0bf3 38         sec 		  |
	0bf4 a5 20      lda $20		  |
	0bf6 e5 21      sbc $21		  |
	0bf8 85 66      sta $66		--/
	0bfa 10 02      bpl L53     	--\
	0bfc a2 ff      ldx #$ff	  |    sign extend temp to (67)
L53     0bfe 86 67      stx $67		--/
	0c00 a2 00      ldx #$00	--\
	0c02 38         sec 		  |    A = Y - rawEnc2
	0c03 98         tya 		  |    X = sign extend A
	0c04 e5 20      sbc $20		  |
	0c06 10 02      bpl L54     	  |
	0c08 a2 ff      ldx #$ff	--/
L54     0c0a 18         clc 		--\
	0c0b 65 66      adc $66		  |


	0c0d 85 66      sta $66		  |    temp = temp + (X, A)
	0c0f 8a         txa 		  |
	0c10 65 67      adc $67		  |
	0c12 85 67      sta $67		--/
	0c14 a4 06      ldy $06		--\
	0c16 f0 0d      beq L56     	  |
	0c18 a5 67      lda $67		  |    if (encoderScale != 0)
L55     0c1a c9 80      cmp #$80	  |       temp >> encoderScale
	0c1c 66 67      ror $67		  |
	0c1e 66 66      ror $66		  |
	0c20 66 65      ror $65		  |
	0c22 88         dey 		  |
	0c23 d0 f5      bne L55     	--/
L56     0c25 18         clc 		--\
	0c26 a5 22      lda $22		  |
	0c28 65 65      adc $65		  |
	0c2a a5 23      lda $23		  |    stopPosition = x + temp
	0c2c 65 66      adc $66		  |
	0c2e 85 03      sta $03		  |
	0c30 a5 24      lda $24		  |
	0c32 65 67      adc $67		  |
	0c34 85 04      sta $04		--/
	0c36 a5 03      lda $03		--\
	0c38 a4 04      ldy $04		  |    call stop (stopPosition)
	0c3a 20 25 0f   jsr SUB10   	--/
	0c3d ce 02 06   dec $0602	--*    disable command interrupts
	0c40 e6 01      inc $01		--*    set INDEX_FOUND
	0c42 a9 f7      lda #$f7	--\
	0c44 25 00      and $00		  |    clear LOST_SYNC
	0c46 85 00      sta $00		--/
	0c48 a9 03      lda #$03	--\    enable command interrupts and
	0c4a 0d 02 06   ora $0602	  |    request attention
	0c4d 8d 02 06   sta $0602	--/
	0c50 4c a1 0c   jmp L65     	--* else (not looking for index)
L57     0c53 a9 08      lda #$08	--\
	0c55 24 00      bit $00		  |    if (not LOST_SYNC)
	0c57 d0 48      bne L65     	  |       if (not opFlags 0x80)
	0c59 24 63      bit $63		  |	     set opFlags 0x80
	0c5b 30 08      bmi L58     	  |
	0c5d a9 80      lda #$80	  |
	0c5f 05 63      ora $63		  |
	0c61 85 63      sta $63		  |
	0c63 d0 3c      bne L65     	  |    else
L58     0c65 a2 01      ldx #$01	  |       X = 1
	0c67 38         sec 		  |	  Y = zindexEnc - zindexLast
	0c68 a5 5b      lda $5b		  |
	0c6a e5 5c      sbc $5c		  |
	0c6c a8         tay 		--/
	0c6d 38         sec 		--\	  A two cycle loop is used to 
	0c6e e5 05      sbc $05		  |       achieve the following:
L59     0c70 30 04      bmi L60     	  |       
	0c72 c9 05      cmp #$05	  |	  if ((ABS(Y-zSize) > 0x4) AND
	0c74 b0 07      bcs L61     	  |	      (ABS(Y+zSize) > 0x4) AND
L60     0c76 c9 fc      cmp #$fc	  |	      (ABS(Y) > 0x4 OR 
	0c78 30 03      bmi L61     	  |	       lostSyncCnt++ > 0x4) )
	0c7a 4c a1 0c   jmp L65     	  |	     signal LOST_SYNC
L61     0c7d ca         dex 		  |	  
	0c7e 30 07      bmi L62     	  |	  
	0c80 98         tya 		  |	  
	0c81 18         clc 		  |
	0c82 65 05      adc $05		  |
	0c84 4c 70 0c   jmp L59     	  |	  
L62     0c87 98         tya 		  |	  
	0c88 f0 17      beq L65     	  |	  
	0c8a 30 04      bmi L63     	  |	  
	0c8c c9 05      cmp #$05	  |	     
	0c8e b0 0c      bcs L64     	  |	     
L63     0c90 c9 fc      cmp #$fc	  |	  
	0c92 30 08      bmi L64     	  |	     
	0c94 e6 62      inc $62		  |
	0c96 a5 62      lda $62		  |	  
	0c98 c9 05      cmp #$05	  |
	0c9a 90 05      bcc L65     	  |
L64     0c9c a9 08      lda #$08	  |
	0c9e 20 44 0f   jsr SUB11   	--/
L65     0ca1 a5 5c      lda $5c		--\
	0ca3 85 5d      sta $5d		  | zindexOld = zindexLast
	0ca5 a5 5b      lda $5b		  | zindexLast = zindexEnc
	0ca7 85 5c      sta $5c		--/
	0ca9 4c 58 0f   jmp L91     	--* return from interrupt
	0cac 00         brk 

	...

	0cff 00         brk 

	CURRENT, POSITION>

	0d00 a5 01      lda $01		--\ if (not SERVO_ENABLE or 
	0d02 29 82      and #$82	  |     ZINDEX_FOUND)
	0d04 c9 80      cmp #$80	  |    do nothing
	0d06 d0 4d      bne L69     	--/

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

	0d08 a6 45      ldx $45		--\
	0d0a ad 00 04   lda $0400	  |
	0d0d 95 47      sta $47,x	  | load data into dataBuf, and the
	0d0f ad 03 04   lda $0403	  | command into the commandBuf.
	0d12 95 4b      sta $4b,x	  |
	0d14 94 4f      sty $4f,x	--/
	0d16 ca         dex 		--\
	0d17 10 02      bpl L66     	  | advance dataPtr
	0d19 a2 03      ldx #$03	  |
L66     0d1b 86 45      stx $45		--/
	0d1d e6 44      inc $44		--* commandRequest++
	0d1f 4c 58 0f   jmp L91     	--* return from interrupt

	READM>

	0d22 a6 02      ldx $02		--\ load memory word pointed to by
	0d24 b5 00      lda $00,x	  | memIndex into (A, Y)
	0d26 b4 01      ldy $01,x	--/
	0d28 e8         inx 		--\ 
	0d29 e8         inx 		  | memIndex = memIndex + 2
	0d2a 86 02      stx $02		--/
	0d2c 4c 42 0d   jmp L67     	--*

	JSTAT>

	0d2f ad 02 06   lda $0602	--\ clear ATTENTION_REQ in control 
	0d32 29 fd      and #$fd	  | register
	0d34 8d 02 06   sta $0602	--/
	0d37 a5 00      lda $00		--\ move jstat into (A, Y) for output
	0d39 a4 01      ldy $01		--/
	0d3b 4c 42 0d   jmp L67         --*

	READ>

	0d3e a5 23      lda $23		--\ load x into (A, Y)
	0d40 a4 24      ldy $24		--/ for output

L67     0d42 8d 00 04   sta $0400	--\ write (A, Y) out onto the bus
	0d45 8c 03 04   sty $0403	--/
	0d48 4c 58 0f   jmp L91     	--* return from interrupt

	READP>

	0d4b a2 64      ldx #$64	--\
L68     0d4d ca         dex 		  | wait a bit
	0d4e d0 fd      bne L68     	--/
	0d50 a5 01      lda $01		--\ load high byte of jstat into 0401
	0d52 8d 01 04   sta $0401	--/
	
	unused>

L69     0d55 ad 02 04   lda $0402	--* read data
	0d58 4c 58 0f   jmp L91     	--* return from interrupt

	position>

	0d5b 20 ea 0d   jsr SUB6    	--\ call SUB6 and return
	0d5e 4c bb 0d   jmp L71     	--/

	current>

	0d61 20 cb 0e   jsr SUB7    	--\ call SUB7 and return
	0d64 4c bb 0d   jmp L71     	--/
	
	gravity>

	0d67 4c bb 0d   jmp L71     	--* just return

	stopm, calib>

	0d6a a9 02      lda #$02	--\
	0d6c 24 01      bit $01		  | if (ZINDEX_FOUND)
	0d6e f0 06      beq L70     	  |    call SUB8
	0d70 20 f3 0e   jsr SUB8    	  | else
	0d73 4c bb 0d   jmp L71     	  |    call SUB9 (current data)
L70     0d76 b5 47      lda $47,x	  |
	0d78 b4 4b      ldy $4b,x	  |
	0d7a 20 21 0f   jsr SUB9    	--/
	0d7d 4c bb 0d   jmp L71     	--* return

	stopp>

	0d80 a5 23      lda $23		--\
	0d82 a4 24      ldy $24		  | call SUB9 (x)
	0d84 20 21 0f   jsr SUB9    	--/
	0d87 4c bb 0d   jmp L71     	--* return

	tolbnd>

	0d8a b5 47      lda $47,x	--\
	0d8c 85 57      sta $57		  | posTolerence = 
	0d8e b5 4b      lda $4b,x	  |    dataBuf[dataPtr]
	0d90 85 58      sta $58		--/
	0d92 4c bb 0d   jmp L71     

	unknown>

	0d95 a9 00      lda #$00	--\
	0d97 85 00      sta $00		  | Clear low byte of jstat
	0d99 85 62      sta $62		  | ATTENTION_REQ = false
	0d9b a9 fd      lda #$fd	  |
	0d9d 2d 02 06   and $0602	  |
	0da0 8d 02 06   sta $0602	--/
	0da3 4c bd 0d   jmp L72     	--* return

	intbnd>

	0da6 b5 47      lda $47,x	--\
	0da8 85 59      sta $59		  | intBand = dataBuf [dataPtr]
	0daa b5 4b      lda $4b,x	  |
	0dac 85 5a      sta $5a		--/
	0dae 4c bd 0d   jmp L72     	--* return

	setpar>

	0db1 b4 4b      ldy $4b,x	--\
	0db3 b5 47      lda $47,x	  | store parameter into location
	0db5 99 00 00   sta $0000,y	--/
	0db8 4c bd 0d   jmp L72     	--* return
	
	unused, jstat, readm, read, readp>

L71     0dbb a6 46      ldx $46		--\
L72     0dbd ca         dex 		  | Advance commandPtr
	0dbe 10 02      bpl L73     	  |
	0dc0 a2 03      ldx #$03	  |
L73     0dc2 86 46      stx $46		--/
	0dc4 c6 44      dec $44		--* --commandRequest
	0dc6 4c 58 0f   jmp L91     	--* return from interrupt

SUB5    0dc9 a2 fd      ldx #$fd	--\
	0dcb a9 00      lda #$00	  | integral = 0
L74     0dcd 95 17      sta $17,x	  | xdInc = 0
	0dcf 95 1c      sta $1c,x	  | vdInc = 0
	0dd1 95 2d      sta $2d,x	  | vd = 0
	0dd3 95 30      sta $30,x	  |
	0dd5 e8         inx 		  |
	0dd6 d0 f5      bne L74     	--/
	0dd8 a5 22      lda $22		--\
	0dda 85 1c      sta $1c		  | xd = x
	0ddc a5 23      lda $23		  |
	0dde 85 1d      sta $1d		  |
	0de0 a5 24      lda $24		  |
	0de2 85 1e      sta $1e		--/
	0de4 a2 8c      ldx #$8c	--\
L75     0de6 ca         dex 		  | wait a little bit ...
	0de7 d0 fd      bne L75     	--/
	0de9 60         rts 		--* return

	Subroutine to process a new position request

SUB6    0dea a5 01      lda $01		--\ 
	0dec 29 82      and #$82	  | if (not SERVO_ENABLE or 
	0dee c9 80      cmp #$80	  |	ZINDEX_FOUND),
	0df0 f0 03      beq L76     	  |    return
	0df2 4c ca 0e   jmp L86     	--/
L76     0df5 a9 00      lda #$00	--\
	0df7 85 1c      sta $1c		  | clear low order bytes of xd,
	0df9 85 19      sta $19		  |    xdInc, and vdInc
	0dfb 85 2a      sta $2a		--/
	0dfd a5 30      lda $30		--\
	0dff 85 66      sta $66		  | temp = xDelta 
	0e01 a5 31      lda $31		  |
	0e03 85 67      sta $67		--/
	0e05 38         sec 		--\
	0e06 b5 47      lda $47,x	  | xDelta = dataBuf[dataPtr] - xd;
	0e08 e5 1d      sbc $1d		  |
	0e0a 85 1a      sta $1a		  |
	0e0c 85 30      sta $30		  | prepare tick increment for 
	0e0e b5 4b      lda $4b,x	  |    divison
	0e10 e5 1e      sbc $1e		  | 
	0e12 85 31      sta $31		--/
	0e14 a4 0b      ldy $0b		--\
L77     0e16 c9 80      cmp #$80	  |
	0e18 6a         ror 		  | 
	0e19 66 1a      ror $1a		  | xdInc = xDelta/ticksPerSample;
	0e1b 66 19      ror $19		  |
	0e1d 88         dey 		  |
	0e1e d0 f6      bne L77     	  |
	0e20 85 1b      sta $1b		--/
	0e22 a5 0c      lda $0c		--\ tickCnt = ticksPerSample;
	0e24 85 18      sta $18		--/
	0e26 38         sec 		--\
	0e27 a5 30      lda $30		  |
	0e29 e5 66      sbc $66		  | 
	0e2b 85 3c      sta $3c		  | accelEst = xDelta - 
	0e2d a5 31      lda $31		  |	       previous xDelta
	0e2f e5 67      sbc $67		  |
	0e31 85 3d      sta $3d		--/
	0e33 30 0d      bmi L78     	--\ 
	0e35 a5 09      lda $09		  | 
	0e37 c5 3c      cmp $3c		  | 
	0e39 a5 0a      lda $0a		  | 
	0e3b e5 3d      sbc $3d		  |
	0e3d b0 11      bcs L79     	  |
	0e3f 4c 6d 0e   jmp L81           |
L78     0e42 18         clc 		  |
	0e43 a5 09      lda $09		  |    
	0e45 65 3c      adc $3c		  |	  
	0e47 a5 0a      lda $0a		  | if (ABS(accelEst)< accelThreshold)
	0e49 65 3d      adc $3d		  |  {  
	0e4b b0 03      bcs L79     	  |
	0e4d 4c 6d 0e   jmp L81     	--/
L79     0e50 a5 64      lda $64		--\    if (accelCnt == 0 OR
	0e52 f0 04      beq L80     	  |        accelCnt-- == 0)
	0e54 c6 64      dec $64		  |     {
	0e56 d0 33      bne L83     	--/       
L80     0e58 a5 63      lda $63		--\ 
	0e5a 29 df      and #$df	  |       clear opFlags ACCELERATING
	0e5c 85 63      sta $63		--/
	0e5e a9 20      lda #$20	--\ 
	0e60 24 01      bit $01		  |    
	0e62 d0 27      bne L83     	  |       if (jstat 13 clear)
	0e64 a5 00      lda $00		  |          clear jstat 6
	0e66 29 bf      and #$bf	  |     }
	0e68 85 00      sta $00		  |  }
	0e6a 4c 8b 0e   jmp L83     	--/ else
L81     0e6d a9 20      lda #$20	--\  { if (jstat 13 clear)
	0e6f 24 01      bit $01		  |     { set opFlags ACCELERATING
	0e71 d0 13      bne L82     	--/       set jstat 6
	0e73 a5 63      lda $63	        --\       accelCnt = 0x10
	0e75 09 20      ora #$20	  |    
	0e77 85 63      sta $63		--/
	0e79 a5 00      lda $00		--\ 
	0e7b 09 40      ora #$40	  |    
	0e7d 85 00      sta $00		--/
	0e7f a9 10      lda #$10	--\    
	0e81 85 64      sta $64		--/     }
	0e83 4c 8b 0e   jmp L83     	--\    else
L82     0e86 a9 40      lda #$40	  |     { signal code 6
	0e88 20 44 0f   jsr SUB11   	--/     }
L83     0e8b 38         sec 		--\  }
	0e8c a9 00      lda #$00	  |
	0e8e e5 2d      sbc $2d		  | 
	0e90 85 2a      sta $2a		  | vdInc = xDelta -
	0e92 a5 30      lda $30		  |    vd
	0e94 e5 2e      sbc $2e		  |
	0e96 85 2b      sta $2b		  | 
	0e98 a5 31      lda $31		  | 
	0e9a e5 2f      sbc $2f		  |
	0e9c 85 2c      sta $2c		--/
	0e9e a6 0b      ldx $0b		--\
	0ea0 a5 2c      lda $2c		  | 
L84     0ea2 c9 80      cmp #$80	  |
	0ea4 6a         ror 		  |
	0ea5 66 2b      ror $2b		  | vdInc >> tickPerSample
	0ea7 66 2a      ror $2a		  |
	0ea9 ca         dex 		  |
	0eaa d0 f6      bne L84     	  |
	0eac 85 2c      sta $2c		--/
	0eae a5 63      lda $63		--\
	0eb0 09 40      ora #$40	  | set opFlag SERVOING
	0eb2 85 63      sta $63		--/
	0eb4 ad 02 06   lda $0602	--\
	0eb7 29 db      and #$db	  | clear control register bits 5 & 2
	0eb9 8d 02 06   sta $0602	--/
	0ebc a6 12      ldx $12		--\
	0ebe f0 0a      beq L86     	  | if (accelEstScale != 0)
	0ec0 a5 3c      lda $3c		  |    accelEst << accelEstScale
L85     0ec2 0a         asl 		  |
	0ec3 26 3d      rol $3d		  |
	0ec5 ca         dex 		  |
	0ec6 d0 fa      bne L85     	  |
	0ec8 85 3c      sta $3c		--/
L86     0eca 60         rts 		--* return

	Routine to process a set current request 

SUB7    0ecb b4 47      ldy $47,x	--\
	0ecd b5 4b      lda $4b,x	  | DACvalue = dataBuf[dataPtr]
	0ecf 85 40      sta $40		  |
	0ed1 84 3f      sty $3f		--/
	0ed3 a2 00      ldx #$00	--\
	0ed5 a5 40      lda $40		  |
	0ed7 10 02      bpl L87     	  | sign extend DACvalue
	0ed9 a2 ff      ldx #$ff	  |
L87     0edb 86 41      stx $41		--/
	0edd a6 40      ldx $40		--\ send the current to the DAC
	0edf 20 4c 0a   jsr SUB1    	--/
	0ee2 a5 63      lda $63		--\
	0ee4 29 bf      and #$bf	  | clear opFlags SERVOING
	0ee6 85 63      sta $63		--/
	0ee8 ad 02 06   lda $0602	--\
	0eeb 29 db      and #$db	  | clear controlRegister bits 2 & 5
	0eed 09 18      ora #$18	  | set controlRegister bits 3 & 4
	0eef 8d 02 06   sta $0602	--/
	0ef2 60         rts 		--* return

SUB8    0ef3 38         sec 		--\
	0ef4 b5 47      lda $47,x	  |
	0ef6 a8         tay 		  | 
	0ef7 e5 03      sbc $03		  | temp = dataBuf[datap] -
	0ef9 85 66      sta $66		  |    stopPosition
	0efb 84 03      sty $03		  | 
	0efd b5 4b      lda $4b,x	  | stopPosition = 
	0eff a8         tay 		  |    dataBuf[datap]
	0f00 e5 04      sbc $04		  |
	0f02 85 67      sta $67		  |
	0f04 84 04      sty $04		--/
	0f06 18         clc 		--\
	0f07 a5 66      lda $66		  |
	0f09 65 23      adc $23		  | x =
	0f0b 85 23      sta $23		  |    x + temp
	0f0d a5 67      lda $67		  |
	0f0f 65 24      adc $24		  |
	0f11 85 24      sta $24		--/
	0f13 a5 01      lda $01		--\
	0f15 29 fc      and #$fc	  | clear LOOK_ZINDEX and ZINDEX_FOUND
	0f17 85 01      sta $01		--/
	0f19 a5 03      lda $03		--\
	0f1b a4 04      ldy $04		  | call stop(stopPosition)
	0f1d 20 25 0f   jsr SUB10   	--/
	0f20 60         rts 		--* return

	This is a "stop motion" routine

SUB9    0f21 85 23      sta $23		--\ [Y, A] = x
	0f23 84 24      sty $24		--/

SUB10   0f25 85 1d      sta $1d		--\ xd = [Y, A]
	0f27 84 1e      sty $1e	 	--/
	0f29 a9 59      lda #$59	--\ load control register with 0x59
	0f2b 8d 02 06   sta $0602	--/
	0f2e a5 63      lda $63		--\
	0f30 09 40      ora #$40	  | set opFlags SERVOING
	0f32 85 63      sta $63		--/
	0f34 a9 00      lda #$00	--\ clear low byte of xd
	0f36 85 1c      sta $1c		--/ 
	0f38 a2 02      ldx #$02	--\
L88     0f3a 95 19      sta $19,x	  | xdInc = 0
	0f3c 95 2a      sta $2a,x	  | vdInc = 0
	0f3e 95 2d      sta $2d,x	  | vd = 0
	0f40 ca         dex 		  | 
	0f41 10 f7      bpl L88     	--/
	0f43 60         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.

SUB11   0f44 24 00      bit $00		--\
	0f46 f0 03      beq L89     	  | if (flag not set 
	0f48 4c 57 0f   jmp L90     	  |     in low byte of jstat)
L89     0f4b 05 00      ora $00		  |    set the flag
	0f4d 85 00      sta $00		  |    set ATTN REQ
	0f4f a9 02      lda #$02	  |
	0f51 0d 02 06   ora $0602	  |
	0f54 8d 02 06   sta $0602	--/
L90     0f57 60         rts 		--* return

	Return from interrupt sequence:

L91     0f58 68         pla 		--\
	0f59 aa         tax 		  |
	0f5a 68         pla 		  | Pop X, Y, A, and return from
	0f5b a8         tay 		  |    interrupt
	0f5c 68         pla 		  |
	0f5d 40         rti 		--/


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

	Offset	Command

	 00	CURRENT
	 08	STOPM
	 08	GRAVITY
	 08	SETPAR
	 08	UNKNOWN
	 55 	unused
	 2f	JSTAT
	 22	READM
	 00	POSITION
	 08	TOLBND
	 08	CALIB
	 08	INTBND
	 08	STOPP
	 55 	unused
	 3e     READ
	 4b	READP

	 61	CURRENT
	 6a	STOPM
	 67	GRAVITY
	 b1 	SETPAR
	 95	UNKNOWN
	 bb	unused
	 bb	JSTAT
	 bb	READM
	 5b	POSITION
	 8a	TOLBND
	 6a	CALIB
	 a6 	INTBND
	 80	STOPP
	 bb	unused
	 bb	READ
	 bb	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
	
	12         00         
	0e	   0a 
	0d	   05 
	17         38         
	54         0d 
	56 	   0d   
	06 	   02      
	0c         40       
	0b         05 
	11         05 
	13         01 
	0f         03         
	10	   05      
	57         14         
	58         00         
	59 	   46 
	5a         00         
	07         f4         
	08         01 
	09         c8         
	0a         00         
	80	   00         
	82         00        
	86	   00      
	81	   0f      
	83         ff

	0fb2 ff         ??? 
	0fb3 00         brk 

	...

	0ff9 00         brk 
	0ffa 41 57      eor ($57,x)
	0ffc 00         brk 
	0ffd 08         php 
	0ffe 08         php 
	0fff 08         php 

		     MEMORY MAP FOR THE PUMA MARK 3 MICROPROCESSORS


	00 X	jstat(2): joint control status register
	01 X	
	02 X	memIndex(1): used by external commands to reference memory
	03 X	stopPosition(2): used to store the reference point for
	04 X	   when the joint is brought to a halt
	05 X    zSize(1): number of encoder counts between zero indices
	06 X 02 encoderScale(1): scale factor for encoder counts
	07 X f4 posEnvelope(2): maximum desirable encoder count error
	08 X 01
	09 X c8 accelThreshold(2): Joint is considered to be accelerating if
	0A X 00    the change in position request velocities exceeds this
	0B X 05	log2ticks(1): log 2 of number of ticks per cycle
	0C X 40	ticksPerSample(1): number of ticks per cycle period
	0D X 05 kGain(1): gain of proportional and integral terms (log 2 form)
	0E X 0a integralGain(1): integral term inverse gain (log 2 form)
	0F X 03 vEstScale(1): averaging constant to est. velocity from vAvg
		   and encoder deltas, stored as fraction (log 2) of vAvgWin
	10 X 05 vAvgWin(1): size of window for decaying velocity average
		   (log 2 form)
	11 X 05 vGain(1): velocity gain (log 2 form)
	12 X 00 accelEstScale(1): scale for accel. estimate (log 2 form)
	13 X 01 oGain(1): overall output gain (log 2 form)
	14 X	integral(3): accumlated integral control term
	15 X
	16 X
	17 X 38	tickRate(1): time count between tick interrupts
	18 X	tickCnt(1): Number of ticks left in present cycle
	19 X	xdInc(3): per-cycle change in position setpoint
	1A X
	1B X
	1C X	xd(3): interpolated position setpoint for each tick
	1D X
	1E X	
	1F X    rawEnc1(1): 1st sample of 8 bit encoder value.

	20 X	rawEnc2(1): 2nd sample of 8 bit encoder value.
	21 X	lastRawEnc(1): Previous value of rawEnc1
	22 X    x(3): Current robot position (updated each tick).
	23 X	
	24 X
	25 X    xErr(3): Position error, used in computing integral term
	26 X
	27 X
	28 X	xErrSave(2): Saved value of xErr, which is not used
	29 X
	2A X	vdInc(3): per-cycle change in the velocity setpoint
	2B X
	2C X
	2D X	vd(3): desired velocity, interpolated for each tick
	2E X
	2F X

	30 X	xDelta(2): desired position change for the current cycle
	31 X
	32 X    vAvg(3): a decaying time average of the velocity
	33 X
	34 X
	35 X    vEst(4): initially set to encoder change between ticks. Is
	36 X       later massaged into an estimate of the current velocity
	37 X       by averaging it in with vAvg, and finally is combined with
	38 X	   vd to produce the velocity error term
	39 X    vErrSave(3): used to store the per-tick velocity error.
	3A X	   This value does not appear to be used.
	3B X
	3C X    accelEst(2): per-cycle estimate of the desired acceleration
	3D X
	3E X 	DACvalue(4): current setpoint for DAC
	3F X
	40 X
	41 X
	42 X    DACtimeout(2): timeout counter for DAC output saturation
	43 X
	44 X	commandRequest(1): Number of outstanding input commands
	45 X	dataPtr(1): pointer to beginning of data buffer
	46 X	commandPtr(1): pointer to beginning of command buffer
	47 X	dataBuf(8): buffer for storing command data
	48 X
	49 X
	4A X
	4B X
	4C X
	4D X
	4E X
	4F X	commandBuf(4): buffer for storing commands

	50 X
	51 X
	52 X
	53 X	jumpAddress1(2): For computing jump table addresses
	54 X 0d
	55 X	jumpAddress2(2): For computing jump table addresses
	56 X 0d
	57 X 14	posTolerence(2): Positioning tolerence
	58 X 00
	59 X 46	intBand(2): Integration band (not apparently used)
	5A X 00
	5B X    zindexEnc(1): Raw encoder value at present zero index
	5C X    zindexLast(1): Raw encoder value at last zero index
	5D X    zindexOld(1): Old value of zindexLast (value not used)
	5E X	zindexCnt(2): Number of encoder counts since last zero index
	5F X

	60 X	oldZindexCnt(2): old value of zindexCnt (value not used)
	61 X
	62 X    lostSyncCnt(1): counter used in handling loss of encoder sync
	63 X	opFlags(1): Operational flags
	64 X	accelCnt(1): no. of cycles in accelTreshold refresh period 
	65 X    temp(3): Scratch computing area
	66 X
	67 X
	68 X	time(1): Counter that is bumped once per tick. Value not used.
	69 X    vEstSave(3): used to store the velocity estimate 'vEst'.
	6A X	   The stored value is not used explicitly.
	6B X	   
	6C
	6D
	6E
	6F

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

	80   00
	81   0f
	82   00
	83   ff
	84
	85
	86   00
	87
	88
	89
	8A
	8B
	8C
	8D
	8E
	8F


				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 pos_tol
