; Disassembly from 0x0800 to 0x0fff

	0800 d8         cld 		--* Clear decimal mode
	0801 a9 ff      lda #$ff	--\
L0:	0803 a2 7f      ldx #$7f	  | Test memory locations 00 - 7f,
L1:	0805 95 00      sta $00,x	  | first with 0xff and then with 
	0807 d5 00      cmp $00,x	  | 0. If memory is bad, signal
	0809 d0 0a      bne L2      	  | failure. 
	080b ca         dex 		  |
	080c 10 f7      bpl L1      	  | Memory will be cleared when this
	080e a8         tay 		  | is done.
	080f f0 09      beq L3      	  |
	0811 c8         iny 		  |
	0812 98         tya 		  |
	0813 f0 ee      beq L0      	  |
L2:	0815 a9 01      lda #$01	  |
	0817 20 49 0f   jsr SUB11   	--/
L3:	081a a2 7f      ldx #$7f	--\ Initialize the stack to 0x7f
	081c 9a         txs 		--/
	081d a0 00      ldy #$00	--\
	081f f0 07      beq L5      	  |
L4:	0821 c8         iny 		  | Initialize memory using the 
	0822 b9 df 0f   lda $0fdf,y	  | address - value table at address 
	0825 95 00      sta $00,x	  | 0xfdf.
	0827 c8         iny 		  |
L5:	0828 be df 0f   ldx $0fdf,y	  |
	082b e0 ff      cpx #$ff	  |
	082d d0 f2      bne L4      	--/
	082f ad 00 06   lda $0600	--\
	0832 85 20      sta $20		  | rawEnc2, lastRawEnc = cnt [7-0]
	0834 85 21      sta $21		--/
	0836 a5 23      lda $23		--\
	0838 a4 24      ldy $24		  | STOP at x
	083a 20 26 0f   jsr SUB9    	--/
	083d ad 01 06   lda $0601
	0840 85 5c      sta $5c
	0842 a5 17      lda $17		--\ PIO_timer = tickRate
	0844 85 9e      sta $9e		--/
	0846 58         cli 		--* Enable PIO interrupts
L6:	0847 4c 47 08   jmp L6		--* Wait for some action

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

	084a 48         pha 		--\
	084b 98         tya 		  |
	084c 48         pha 		  | Push A, Y, X
	084d 8a         txa 		  |
	084e 48         pha 		--/
	084f ad 03 06   lda $0603	--\
	0852 4a         lsr 		  |
	0853 4a         lsr 		  | Read the joint command off the bus
	0854 4a         lsr 		  | and jump to the appropriate 
	0855 29 0f      and #$0f	  | handler as indexed in the NMI
	0857 a8         tay 		  | dispatch table
	0858 b9 bf 0f   lda $0fbf,y	  |
	085b 85 53      sta $53		  |
	085d 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).

	0860 48         pha 		--* Push A
	0861 ce 02 06   dec $0602	--* Disable Command interrupts
	0864 98         tya 		--* A = Y
	0865 24 85      bit $85		--\ if (zero index interrupt)
	0867 50 03      bvc L7      	  |    goto L52
	0869 4c aa 0b   jmp L50     	--/

	On timer interrupt:

L7:	086c a4 17      ldy $17		--\ PIO_timer = tickRate
	086e 84 9e      sty $9e		--/
	0870 c6 18      dec $18		--* tickCnt = tickCnt - 1
	0872 ac 00 06   ldy $0600	--\
	0875 ee 02 06   inc $0602	  | Reenable command interrupts
	0878 48         pha 		  |
	0879 8a         txa 		  | push A, X
	087a 48         pha 		  |
	087b 84 1f      sty $1f		  | rawEnc1 = cnt [7-0]
	087d 20 e0 0a   jsr SUB2    	  | compute current encoder count
	0880 20 4c 0b   jsr SUB3        --/ compute velocity control term
	0883 a9 40      lda #$40	--\
	0885 24 60      bit $60		  | if (opFlags SERVOING)
	0887 d0 06      bne L8            |    do position control
	0889 20 cf 0d   jsr SUB4    	  | else
	088c 4c 92 08   jmp L9      	  |    do current control
L8:	088f 20 ac 08   jsr SUB0    	--/
L9:	0892 e6 68      inc $68		--* time++
	0894 ac 00 06   ldy $0600	--\ rawEnc2 = cnt [7-0]
	0897 84 20      sty $20		--/
	0899 a5 44      lda $44		--\
	089b d0 03      bne L10     	  | if (commandRequest != 0)
	089d 4c b9 0f   jmp L107    	  |    lookup the appropriate routine
L10:	08a0 a6 46      ldx $46		  |    and jump to it
	08a2 b4 4f      ldy $4f,x	  | else
	08a4 b9 cf 0f   lda $0fcf,y	  |    goto interrupt return
	08a7 85 55      sta $55		  |
	08a9 6c 55 00   jmp ($0055)	--/

	Main position servo control routine:

SUB0:	08ac a2 1c      ldx #$1c	--\
	08ae a0 19      ldy #$19	  | xd += xdInc;
	08b0 20 66 0f   jsr SUB12   	--/
	08b3 a2 2d      ldx #$2d	--\
	08b5 a0 2a      ldy #$2a	  | vd += vdInc;
	08b7 20 66 0f   jsr SUB12   	--/
	08ba a5 66      lda $66		
	08bc f0 15      beq L13     	
	08be a5 1b      lda $1b		
	08c0 d0 0d      bne L11     	
	08c2 18         clc 		
	08c3 a5 65      lda $65		
	08c5 65 19      adc $19		
	08c7 85 65      sta $65		
	08c9 a5 66      lda $66		
	08cb 65 1a      adc $1a		
	08cd 30 02      bmi L12     	
L11:	08cf a9 00      lda #$00	
L12:	08d1 85 66      sta $66		
L13:	08d3 a5 18      lda $18		--\
	08d5 d0 0e      bne L14     	  | 
	08d7 a9 00      lda #$00	  | if (tickCnt == 0)
	08d9 85 19      sta $19		  |  { /* then we are 'there': */
	08db 85 1a      sta $1a		  |    xdInc = 0;
	08dd 85 1b      sta $1b		  |    vdInc = 0;
	08df 85 2a      sta $2a		  |  }
	08e1 85 2b      sta $2b		  | 
	08e3 85 2c      sta $2c		--/
L14:	08e5 a2 00      ldx #$00	--\
	08e7 38         sec 		  | 
	08e8 a5 22      lda $22		  | 
	08ea e5 1c      sbc $1c		  | 
	08ec 85 25      sta $25		  | 
	08ee 85 3e      sta $3e		  | xErr = xErrSave = x - xd;
	08f0 a5 23      lda $23		  | DACvalue = xErr;
	08f2 e5 1d      sbc $1d		  | 
	08f4 85 26      sta $26		  | 
	08f6 85 28      sta $28		  | 
	08f8 85 3f      sta $3f		  | 
	08fa a5 24      lda $24		  | 
	08fc e5 1e      sbc $1e		  | 
	08fe 85 27      sta $27		  | 
	0900 85 29      sta $29		  | 
	0902 85 40      sta $40		--/
	0904 10 01      bpl L15     	--\
	0906 ca         dex 		  | sign extend high byte of DACvalue
L15:	0907 86 41      stx $41		--/
	0909 a5 66      lda $66		
	090b f0 15      beq L16     	
	090d 18         clc 		
	090e 65 26      adc $26		
	0910 85 26      sta $26		
	0912 85 3f      sta $3f		
	0914 a5 27      lda $27		
	0916 69 ff      adc #$ff	
	0918 85 27      sta $27		
	091a 85 40      sta $40		
	091c a5 41      lda $41		
	091e 69 ff      adc #$ff	
	0920 85 41      sta $41		
L16:	0922 a5 2e      lda $2e		
	0924 85 63      sta $63		
	0926 a5 2f      lda $2f		
	0928 85 64      sta $64		
	092a 10 0d      bpl L17     	
	092c 38         sec 		
	092d a9 00      lda #$00	
	092f e5 63      sbc $63		
	0931 85 63      sta $63		
	0933 a9 00      lda #$00	
	0935 e5 64      sbc $64		
	0937 85 64      sta $64		
L17:	0939 38         sec 		
	093a a9 20      lda #$20	
	093c e5 63      sbc $63		
	093e 85 63      sta $63		
	0940 a9 03      lda #$03	
	0942 e5 64      sbc $64		
	0944 10 0b      bpl L18     	
	0946 a5 07      lda $07		
	0948 85 63      sta $63		
	094a a5 08      lda $08		
	094c 85 63      sta $63		
	094e 4c 64 09   jmp L19     	
L18:	0951 18         clc 		
	0952 6a         ror 		
	0953 66 63      ror $63		
	0955 85 64      sta $64		
	0957 38         sec 		
	0958 a5 07      lda $07		
	095a e5 63      sbc $63		
	095c 85 63      sta $63		
	095e a5 08      lda $08		
	0960 e5 64      sbc $64		
	0962 85 64      sta $64		
L19:	0964 a2 63      ldx #$63	--\ if (ABS(xErr) > temp)
	0966 20 df 0e   jsr SUB7    	  |    if (bit 13 of jstat is set OR
	0969 b0 11      bcs L21     	  |        opFlags ACCELERATING clear)
	096b a9 20      lda #$20	  |       signal OUT_OF_ENVELOPE
	096d 24 01      bit $01		  | 
	096f d0 06      bne L20     	  | 
	0971 a9 20      lda #$20	  | 
	0973 24 60      bit $60		  | 
	0975 d0 05      bne L21     	  | 
L20:	0977 a9 04      lda #$04	  | 
	0979 20 49 0f   jsr SUB11   	--/
L21:	097c a2 57      ldx #$57	--\ if (ABS(xErr) <= posTolerence)
	097e 20 df 0e   jsr SUB7    	  |    clear control reg TOLERENCE;
	0981 b0 08      bcs L22     	  | else
	0983 ad 02 06   lda $0602	  |    set control reg TOLERENCE;
	0986 29 7f      and #$7f	  | 
	0988 4c 90 09   jmp L23     	  | 
L22:	098b ad 02 06   lda $0602	  | 
	098e 09 80      ora #$80	  | 
L23:	0990 8d 02 06   sta $0602	--/
	0993 24 01      bit $01		--\
	0995 70 1a      bvs L25     	  | 
	0997 a6 35      ldx $35		  | 
	0999 30 0a      bmi L24     	  | 
	099b d0 14      bne L25     	  | 
	099d a5 34      lda $34		  | 
	099f c9 04      cmp #$04	  | 
	09a1 10 0e      bpl L25     	  | 
	09a3 30 3f      bmi L28     	  | 
L24:	09a5 e8         inx 		  | Compute integration control term
	09a6 d0 09      bne L25     	  | if necessary:
	09a8 38         sec 		  | 
	09a9 a9 00      lda #$00	  | if (INTEGRATION is set OR
	09ab e5 34      sbc $34		  |     ABS(vAvg) >= 0x4)
	09ad c9 04      cmp #$04	  |  {
	09af 30 33      bmi L28     	--/
L25:	09b1 a5 16      lda $16		--\
	09b3 30 10      bmi L26     	  | 
	09b5 a9 ff      lda #$ff	  |    if ( ABS(integral) < 0x1fff OR
	09b7 c5 15      cmp $15		  |         sgn(xErr) != sgn(integral)
	09b9 a9 1f      lda #$1f	  | 
	09bb e5 16      sbc $16		  | 
	09bd b0 17      bcs L27     	  |    { scale integration term and
	09bf a5 27      lda $27		  | 	 add it in:
	09c1 30 13      bmi L27     	  | 
	09c3 10 1f      bpl L28     	  | 
L26:	09c5 18         clc 		  | 
	09c6 a9 ff      lda #$ff	  | 
	09c8 65 15      adc $15		  | 
	09ca a9 1f      lda #$1f	  | 
	09cc 65 16      adc $16		  | 
	09ce b0 06      bcs L27     	  | 
	09d0 a5 27      lda $27		  | 
	09d2 10 02      bpl L27     	  | 
	09d4 30 0e      bmi L28    	--/
L27:	09d6 a4 0e      ldy $0e		--\
	09d8 a2 25      ldx #$25	  | xErr >> integralGain
	09da 20 aa 0f   jsr SUB14   	--/
	09dd a2 14      ldx #$14	--\
	09df a0 25      ldy #$25	  | integral += xErr
	09e1 20 66 0f   jsr SUB12   	--/
L28:	09e4 a2 3e      ldx #$3e	--\
	09e6 a0 00      ldy #$00	  | 
	09e8 a5 16      lda $16		  | 
	09ea 10 01      bpl L29     	  | 
	09ec 88         dey 		  | 
L29:	09ed 84 63      sty $63		  | DACvalue += integral
	09ef a0 14      ldy #$14	  | 
	09f1 20 66 0f   jsr SUB12   	  |
	09f4 a5 41      lda $41		  | 
	09f6 65 63      adc $63		  | 
	09f8 85 41      sta $41		--/
	09fa a2 3e      ldx #$3e	--\
	09fc a4 0d      ldy $0d		  | DACvalue << kGain
	09fe 18         clc 		  | 
	09ff 20 7d 0f   jsr SUB13   	--/
	0a02 38         sec 		--\
	0a03 a5 36      lda $36		  | 
	0a05 e5 2d      sbc $2d		  | vEst -= vd;
	0a07 85 36      sta $36		  | vErrSave = vEst;
	0a09 a5 37      lda $37		  | 
	0a0b e5 2e      sbc $2e		  | 
	0a0d 85 37      sta $37		  | 
	0a0f 85 3a      sta $3a		  | 
	0a11 a5 38      lda $38		  | 
	0a13 e5 2f      sbc $2f		  | 
	0a15 85 38      sta $38		  | 
	0a17 85 3b      sta $3b		--/
	0a19 a2 00      ldx #$00	--\
	0a1b a5 38      lda $38		  | sign extend vEst to high byte
	0a1d 10 02      bpl L30     	  | 
	0a1f a2 ff      ldx #$ff	  | 
L30:	0a21 86 39      stx $39		--/
	0a23 a2 36      ldx #$36	--\
	0a25 a4 11      ldy $11		  | vEst <<= vGain
	0a27 18         clc 		  | 
	0a28 20 7d 0f   jsr SUB13   	--/
	0a2b a2 3e      ldx #$3e	--\
	0a2d a0 36      ldy #$36	  | 
	0a2f 20 66 0f   jsr SUB12   	  | DACvalue += vEst;
	0a32 a5 41      lda $41		  | 
	0a34 65 39      adc $39		  | 
	0a36 85 41      sta $41		--/
	0a38 a2 00      ldx #$00	--\
	0a3a a5 3d      lda $3d		  | sign extend accelEst into temp
	0a3c 10 01      bpl L31     	  |    for next subtraction
	0a3e ca         dex 		  | 
L31:	0a3f 86 63      stx $63		--/
	0a41 38         sec 		--\
	0a42 a5 3f      lda $3f		  | 
	0a44 e5 3c      sbc $3c		  | DACvalue = DACvalue - accelEst
	0a46 85 3f      sta $3f		  | 
	0a48 a5 40      lda $40		  | 
	0a4a e5 3d      sbc $3d		  | 
	0a4c 85 40      sta $40		  | 
	0a4e a5 41      lda $41		  | 
	0a50 e5 63      sbc $63		  | 
	0a52 85 41      sta $41		--/
	0a54 a2 3e      ldx #$3e	--\
	0a56 a4 13      ldy $13		  | DACvalue << oGain
	0a58 38         sec 		  | 
	0a59 20 7d 0f   jsr SUB13   	--/
	0a5c a4 3f      ldy $3f		--\
	0a5e a6 40      ldx $40		  | output DAC values
	0a60 20 64 0a   jsr SUB1    	--/
	0a63 60         rts 		--* return

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

SUB1:	0a64 8a         txa 		--\ Clip signal value if necessary:
	0a65 2a         rol 		  |
	0a66 a5 41      lda $41		  | The signal value is stored in
	0a68 69 00      adc #$00	  | [2f, X, Y].
	0a6a f0 06      beq L32     	  |
	0a6c a5 41      lda $41		  | if (signal value >= 0x7fe)
	0a6e 10 0f      bpl L33     	  |    [X, Y] = 0x7fe
	0a70 30 1c      bmi L35     	  | else if (signal value < -0x800)
L32:	0a72 a5 41      lda $41		  |    [X, Y] = -0x800
	0a74 30 0f      bmi L34     	  |
	0a76 38         sec 		  |
	0a77 98         tya 		  |
	0a78 e9 fe      sbc #$fe	  |
	0a7a 8a         txa 		  |
	0a7b e9 07      sbc #$07	  |
	0a7d 30 26      bmi L37     	  |
L33:	0a7f a0 fe      ldy #$fe	  |
	0a81 a2 07      ldx #$07	  |
	0a83 d0 0d      bne L36     	  |
L34:	0a85 38         sec 		  |
	0a86 98         tya 		  |
	0a87 e9 00      sbc #$00	  |
	0a89 8a         txa 		  |
	0a8a e9 f8      sbc #$f8	  |
	0a8c 10 17      bpl L37     	  |
L35:	0a8e a0 00      ldy #$00	  |
	0a90 a2 f8      ldx #$f8	--/
L36:	0a92 e6 42      inc $42		--\ if (signal value was clipped)
	0a94 d0 15      bne L38     	  |    DACtimeout++
	0a96 e6 43      inc $43		  |    if (DACtimeout > 0x700)
	0a98 a9 07      lda #$07	  |       signal DAC_SATURATE
	0a9a c5 43      cmp $43		  | 
	0a9c d0 0d      bne L38     	  | 
	0a9e a9 10      lda #$10	  | 
	0aa0 20 49 0f   jsr SUB11   	  | 
	0aa3 d0 06      bne L38     	  | else
L37:	0aa5 a9 00      lda #$00	  |    DACtimeout = 0
	0aa7 85 42      sta $42		  | 
	0aa9 85 43      sta $43		--/
L38:	0aab a5 00      lda $00		
	0aad f0 15      beq L40     	
	0aaf 8a         txa 		
	0ab0 30 0a      bmi L39     	
	0ab2 e0 03      cpx #$03	
	0ab4 30 0e      bmi L40     	
	0ab6 a2 03      ldx #$03	
	0ab8 a0 00      ldy #$00	
	0aba d0 08      bne L40     	
L39:	0abc e0 0d      cpx #$0d	
	0abe 30 04      bmi L40     	
	0ac0 a2 0c      ldx #$0c	
	0ac2 a0 ff      ldy #$ff	
L40:	0ac4 84 63      sty $63		--\
	0ac6 8a         txa 		  | All 12 bits of the DAC signal are
	0ac7 06 63      asl $63		  | in [X, Y].
	0ac9 2a         rol 		  |
	0aca 06 63      asl $63		  | Shift the bits around so that
	0acc 2a         rol 		  | X contains the high order 8 bits,
	0acd 06 63      asl $63		  | and the A contains the low order
	0acf 2a         rol 		  | 4 bits.
	0ad0 06 63      asl $63		  |
	0ad2 2a         rol 		  |
	0ad3 aa         tax 		  | 
	0ad4 98         tya 		--/
	0ad5 45 80      eor $80		--\ write X and A into the DAC 
	0ad7 29 0f      and #$0f	  | registers 82 and 80 such that the
	0ad9 45 80      eor $80		  | high 4 bits of 80 are preserved.
	0adb 85 80      sta $80		  |
	0add 86 82      stx $82		--/
	0adf 60         rts 		--* return

	Subroutine to compute the current encoder value:
					
SUB2:	0ae0 a2 00      ldx #$00	--\ clear low byte of vEst
	0ae2 86 36      stx $36		--/
	0ae4 38         sec 		--\
	0ae5 a5 20      lda $20		  | temp = rawEnc2 - lastRawEnc
	0ae7 e5 21      sbc $21		  | Calculate 'vEst' using the
	0ae9 85 63      sta $63		  | last raw encoder count, and the 
	0aeb 10 02      bpl L41     	  | intermediate count 'rawEnc2':
	0aed a2 ff      ldx #$ff	  | 
L41:	0aef 86 64      stx $64		  | vEst =  (rawEnc2 - lastRawEnc)
	0af1 a2 00      ldx #$00	  |	       + (rawEnc1 - rawEnc2)
	0af3 38         sec 		  | 
	0af4 a5 1f      lda $1f		  | (Using the extra sample 'rawEnc2' 
	0af6 e5 20      sbc $20		  | increases the maximum observable
	0af8 10 02      bpl L42     	  | difference from 128 to 256.)
	0afa a2 ff      ldx #$ff	  | 
L42:	0afc 18         clc 		  | 
	0afd 65 63      adc $63		  | 
	0aff 85 37      sta $37		  | 
	0b01 8a         txa 		  | 
	0b02 65 64      adc $64		  | 
	0b04 85 38      sta $38		--/ 
	0b06 18         clc 		--\
	0b07 a5 5d      lda $5d		  | zindexCnt += vEst;
	0b09 65 37      adc $37		  | 
	0b0b 85 5d      sta $5d		  | 
	0b0d a5 5e      lda $5e		  | 
	0b0f 65 38      adc $38		  | 
	0b11 85 5e      sta $5e		--/
	0b13 30 0c      bmi L43     	--\
	0b15 a9 00      lda #$00	  | if (vEst >= 0)
	0b17 c5 5d      cmp $5d		  |    if (ABS(zindexCnt) > 0x1a00)
	0b19 a9 1a      lda #$1a	  |	  signal NO_ZERO_INDEX
	0b1b e5 5e      sbc $5e		  | 
	0b1d b0 12      bcs L45     	  | 
	0b1f 90 0b      bcc L44     	  | 
L43:	0b21 18         clc 		  | 
	0b22 a9 00      lda #$00	  | 
	0b24 65 5d      adc $5d		  | 
	0b26 a9 1a      lda #$1a	  | 
	0b28 65 5e      adc $5e		  | 
	0b2a b0 05      bcs L45     	  | 
L44:	0b2c a9 02      lda #$02	  | 
	0b2e 20 49 0f   jsr SUB11   	--/
L45:	0b31 a4 06      ldy $06		--\
	0b33 f0 05      beq L46     	  | if (encoderScale != 0)
	0b35 a2 36      ldx #$36	  |    vEst >> encoderScale
	0b37 20 aa 0f   jsr SUB14   	--/
L46:	0b3a a5 1f      lda $1f		--\ lastRawEnc = rawEnc1
	0b3c 85 21      sta $21		--/
	0b3e ce 02 06   dec $0602	--* disable command interrupts
	0b41 a2 22      ldx #$22	--\
	0b43 a0 36      ldy #$36	  | x += vEst;
	0b45 20 66 0f   jsr SUB12   	--/
	0b48 ee 02 06   inc $0602	--* renable command interrupts
	0b4b 60         rts 		--* return
					
SUB3:	0b4c a6 0b      ldx $0b		--\
L47:	0b4e 06 36      asl $36		  | Scale velocity by the number of 
	0b50 26 37      rol $37		  | ticks per cycle:
	0b52 26 38      rol $38		  | 
	0b54 ca         dex 		  | vEst << ticksPerSample;
	0b55 d0 f7      bne L47     	--/
	0b57 38         sec 		--\
	0b58 a5 36      lda $36		  | 
	0b5a e5 33      sbc $33		  | Update the average velocity term.
	0b5c 85 62      sta $62		  | This is computed as a decaying
	0b5e a5 37      lda $37		  | average with time constant 
	0b60 e5 34      sbc $34		  | 'vAvgWin':
	0b62 85 63      sta $63		  | 
	0b64 a5 38      lda $38		  | temp = (vEst-vAvg)
	0b66 e5 35      sbc $35		  | if (vAvgWin != 0)
	0b68 85 64      sta $64		  |    temp >> vAvgWin
	0b6a a4 10      ldy $10		  | 
	0b6c f0 05      beq L48     	  | 
	0b6e a2 62      ldx #$62	  |
	0b70 20 aa 0f   jsr SUB14   	--/
L48:	0b73 a2 33      ldx #$33	--\
	0b75 a0 62      ldy #$62	  | vAvg += temp
	0b77 20 66 0f   jsr SUB12   	--/
	0b7a 38         sec 		--\
	0b7b a5 36      lda $36		  | 
	0b7d e5 33      sbc $33		  | Compute acceleration:
	0b7f 85 36      sta $36		  | 
	0b81 a5 37      lda $37		  | vEst -= vAvg
	0b83 e5 34      sbc $34		  | 
	0b85 85 37      sta $37		  | 
	0b87 a5 38      lda $38		  | 
	0b89 e5 35      sbc $35		  | 
	0b8b 85 38      sta $38		--/
	0b8d a4 0f      ldy $0f		--\
	0b8f 84 63      sty $63		  | 
	0b91 38         sec 		  | if (vAvgWin-vEstScale > 0)
	0b92 a5 10      lda $10		  |    vEst >> (vAvgWin-vEstScale)
	0b94 e5 63      sbc $63		  | 
	0b96 f0 0a      beq L49     	  | 
	0b98 30 08      bmi L49     	  | 
	0b9a a8         tay 		  | 
	0b9b f0 05      beq L49     	  | 
	0b9d a2 36      ldx #$36	  | 
	0b9f 20 aa 0f   jsr SUB14   	--/
L49:	0ba2 a2 36      ldx #$36	--\
	0ba4 a0 33      ldy #$33	  | vEst += vAvg;
	0ba6 20 66 0f   jsr SUB12   	--/
	0ba9 60         rts 		--* return

	On zero index interrupt:

L50:	0baa ac 01 06   ldy $0601	--* Y = cnt [15-8]
	0bad ee 02 06   inc $0602	--* enable Command interrupts
	0bb0 48         pha 		--\
	0bb1 8a         txa 		  | push A and X
	0bb2 48         pha 		--/
	0bb3 84 5b      sty $5b		--* zindexEnc = cnt [15-8]
	0bb5 98         tya 		
	0bb6 a9 00      lda #$00	--\
	0bb8 85 5d      sta $5d		  | zindexCnt = 0;
	0bba 85 5e      sta $5e		--/
	0bbc a9 04      lda #$04	--\
	0bbe 24 60      bit $60		
	0bc0 d0 02      bne L51     	
	0bc2 30 3f      bmi L55     	
L51:	0bc4 a5 5c      lda $5c		
	0bc6 38         sec 		
	0bc7 e5 5b      sbc $5b		
	0bc9 f0 11      beq L52     	
	0bcb c9 01      cmp #$01	
	0bcd f0 0d      beq L52     	
	0bcf c9 ff      cmp #$ff	
	0bd1 f0 09      beq L52     	
	0bd3 a9 80      lda #$80	
	0bd5 05 60      ora $60		
	0bd7 85 60      sta $60		
	0bd9 4c c6 0c   jmp L71     	
L52:	0bdc a9 04      lda #$04	
	0bde 24 60      bit $60		
	0be0 d0 1c      bne L54     	
	0be2 a5 66      lda $66		
	0be4 d0 10      bne L53     	
	0be6 a5 01      lda $01		
	0be8 29 03      and #$03	
	0bea d0 0a      bne L53     	
	0bec a9 00      lda #$00	
	0bee 85 65      sta $65		
	0bf0 a9 f4      lda #$f4	
	0bf2 85 66      sta $66		
	0bf4 d0 0d      bne L55     	
L53:	0bf6 a9 04      lda #$04	
	0bf8 05 60      ora $60		
	0bfa 85 60      sta $60		
	0bfc d0 05      bne L55     	
L54:	0bfe a9 08      lda #$08	
	0c00 20 49 0f   jsr SUB11   	
L55:	0c03 a9 01      lda #$01	--\
	0c05 24 01      bit $01		  | if (LOOK_ZINDEX)
	0c07 f0 55      beq L59     	  |    temp = rawEnc2 - lastRawEnc
	0c09 a2 00      ldx #$00	  | 
	0c0b 86 62      stx $62		  | 
	0c0d 38         sec 		  | 
	0c0e a5 20      lda $20		  | 
	0c10 e5 21      sbc $21		  | 
	0c12 85 63      sta $63		--/
	0c14 10 02      bpl L56     	--\
	0c16 a2 ff      ldx #$ff	  |    sign extend temp to (67)
L56:	0c18 86 64      stx $64		--/
	0c1a a2 00      ldx #$00	--\
	0c1c 38         sec 		  |    A = Y - rawEnc2
	0c1d 98         tya 		  |    X = sign extend A
	0c1e e5 20      sbc $20		  | 
	0c20 10 02      bpl L57     	  | 
	0c22 a2 ff      ldx #$ff	--/
L57:	0c24 18         clc 		--\
	0c25 65 63      adc $63		  | 
	0c27 85 63      sta $63		  |    temp = temp + (X, A)
	0c29 8a         txa 		  | 
	0c2a 65 64      adc $64		  | 
	0c2c 85 64      sta $64		--/
	0c2e a4 06      ldy $06		--\
	0c30 f0 05      beq L58     	  | if (encoderScale != 0)
	0c32 a2 62      ldx #$62	  |    temp >> encoderScale;
	0c34 20 aa 0f   jsr SUB14   	--/
L58:	0c37 18         clc 		--\
	0c38 a5 22      lda $22		  | 
	0c3a 65 62      adc $62		  | 
	0c3c a5 23      lda $23		  |    stopPosition = x + temp
	0c3e 65 63      adc $63		  | 
	0c40 85 03      sta $03		  | 
	0c42 a5 24      lda $24		  | 
	0c44 65 64      adc $64		  | 
	0c46 85 04      sta $04		--/
	0c48 a8         tay 		--\
	0c49 a5 03      lda $03		  |    call stop(stopPosition)
	0c4b 20 2a 0f   jsr SUB10   	--/
	0c4e ce 02 06   dec $0602	--*    disable command interrupts
	0c51 e6 01      inc $01		--*    set INDEX_FOUND
	0c53 a9 03      lda #$03	--\    enable command interrupts and
	0c55 0d 02 06   ora $0602	  |    request attention
	0c58 8d 02 06   sta $0602	--/
	0c5b 4c c6 0c   jmp L71     	--* else (not looking for index)
L59:	0c5e a9 08      lda #$08	--\
	0c60 24 00      bit $00		  |  { if (not LOST_SYNC)
	0c62 d0 62      bne L71     	  |     { if (opFlags & 0x80)
	0c64 24 60      bit $60		  |        { X = 1;
	0c66 10 5e      bpl L71     	  | 	     Y = zindexEnc-zindexLast;
	0c68 a2 01      ldx #$01	  | 	   }
	0c6a 38         sec 		  |     }
	0c6b a5 5b      lda $5b		  | 
	0c6d e5 5c      sbc $5c		  | 
	0c6f a8         tay 		--/
	0c70 38         sec 		
	0c71 e5 05      sbc $05		
L60:	0c73 30 04      bmi L61     	
	0c75 c9 09      cmp #$09	
	0c77 b0 04      bcs L62     	
L61:	0c79 c9 f8      cmp #$f8	
	0c7b 10 17      bpl L65     	
L62:	0c7d ca         dex 		
	0c7e 30 07      bmi L63     	
	0c80 98         tya 		
	0c81 18         clc 		
	0c82 65 05      adc $05		
	0c84 4c 73 0c   jmp L60     	
L63:	0c87 98         tya 		
	0c88 f0 0a      beq L65     	
	0c8a 30 04      bmi L64     	
	0c8c c9 09      cmp #$09	
	0c8e b0 24      bcs L69     	
L64:	0c90 c9 f8      cmp #$f8	
	0c92 30 20      bmi L69     	
L65:	0c94 f0 11      beq L67     	
	0c96 18         clc 		
	0c97 65 5f      adc $5f		
	0c99 85 5f      sta $5f		
	0c9b 30 04      bmi L66     	
	0c9d c9 0a      cmp #$0a	
	0c9f b0 13      bcs L69     	
L66:	0ca1 c9 f5      cmp #$f5	
	0ca3 30 0f      bmi L69     	
	0ca5 10 04      bpl L68     	
L67:	0ca7 a9 00      lda #$00	
	0ca9 85 5f      sta $5f		
L68:	0cab a5 60      lda $60		
	0cad 29 eb      and #$eb	
	0caf 85 60      sta $60		
	0cb1 4c c6 0c   jmp L71     	
L69:	0cb4 a2 08      ldx #$08	
	0cb6 a9 10      lda #$10	
	0cb8 24 60      bit $60		
	0cba d0 06      bne L70     	
	0cbc 05 60      ora $60		
	0cbe 85 60      sta $60		
	0cc0 d0 08      bne L72     	
L70:	0cc2 8a         txa 		
	0cc3 20 49 0f   jsr SUB11   	
L71:	0cc6 a5 5b      lda $5b		--\ zindexLast = zindexEnc
	0cc8 85 5c      sta $5c		--/
L72:	0cca 4c b9 0f   jmp L107    	--* return from interrupt
					
	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 4c      bne L76		--/
					
	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 L73     	  | advance dataPtr
	0d19 a2 03      ldx #$03	  |
L73:	0d1b 86 45      stx $45		--/
	0d1d e6 44      inc $44		--* commandRequest++
	0d1f 4c b9 0f   jmp L107	--* 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 d0 13      bne L74     	--*
					
	JSTAT>				
					
	0d2e ad 02 06   lda $0602	--\ clear ATTENTION_REQ in control 
	0d31 29 fd      and #$fd	  | register
	0d33 8d 02 06   sta $0602	--/
	0d36 a5 00      lda $00		--\ move jstat into (A, Y) for output
	0d38 a4 01      ldy $01		--/
	0d3a 4c 41 0d   jmp L74     	--*
					
	READ>				
					
	0d3d a5 23      lda $23		--\ load x into (A, Y)
	0d3f a4 24      ldy $24		--/ for output

L74:	0d41 8d 00 04   sta $0400	--\ write (A, Y) out onto the bus
	0d44 8c 03 04   sty $0403	--/
	0d47 4c b9 0f   jmp L107	--* return from interrupt
					
	READP>				
					
	0d4a a2 64      ldx #$64	--\
L75:	0d4c ca         dex 		  | wait a bit
	0d4d d0 fd      bne L75     	--/
	0d4f a5 01      lda $01		--\ load high byte of jstat into 0401
	0d51 8d 01 04   sta $0401	--/
					
	UNUSED>				
					
L76:	0d54 ad 02 04   lda $0402	--* read data
	0d57 4c b9 0f   jmp L107	--* return from interrupt
					
	position>			
					
	0d5a 20 f0 0d   jsr SUB5    	--\ call SUB5 and return
	0d5d 4c b6 0d   jmp L79     	--/
					
	current>			
					
	0d60 20 b8 0e   jsr SUB6    	--\ call SUB6 and return
	0d63 d0 51      bne L79     	--/
					
	gravity>			
					
	0d65 4c b6 0d   jmp L79     	--* just return
					
	stopm,calib>			
					
	0d68 a9 02      lda #$02	--\
	0d6a 24 01      bit $01		  | if (ZINDEX_FOUND)
	0d6c f0 05      beq L77     	  |    call SUB8
	0d6e 20 f8 0e   jsr SUB8    	  | else
	0d71 30 43      bmi L79     	  |    call SUB9 (current data)
L77:	0d73 b5 47      lda $47,x	  | 
	0d75 b4 4b      ldy $4b,x	  | 
	0d77 20 26 0f   jsr SUB9    	--/
	0d7a 30 3a      bmi L79     	--* return
					
	stopp>				
					
	0d7c a5 23      lda $23		--\
	0d7e a4 24      ldy $24		  | call SUB9(x)
	0d80 20 26 0f   jsr SUB9    	--/
	0d83 30 31      bmi L79     	--* return
					
	tolbnd>				
					
	0d85 b5 47      lda $47,x	--\
	0d87 85 57      sta $57		  | posTolerence = dataBuf[dataPtr]
	0d89 b5 4b      lda $4b,x	  |
	0d8b 85 58      sta $58		--/
	0d8d 4c b6 0d   jmp L79     	--* return
					
	unknown>			
					
	0d90 a9 00      lda #$00	--\
	0d92 85 00      sta $00		  | Clear low byte of jstat
	0d94 85 5f      sta $5f		  | ATTENTION_REQ = false
	0d96 a9 fd      lda #$fd	  | 
	0d98 2d 02 06   and $0602	  | 
	0d9b 8d 02 06   sta $0602	--/
	0d9e 4c b6 0d   jmp L79     	--* return
					
	intbnd>				
					
	0da1 b5 47      lda $47,x	--\
	0da3 85 59      sta $59		  | intBand = dataBuf[dataPtr]
	0da5 b5 4b      lda $4b,x	  | 
	0da7 85 5a      sta $5a		--/
	0da9 4c b6 0d   jmp L79     	--* return
					
	setpar>				
					
L78:	0dac b4 4b      ldy $4b,x	--\
	0dae b5 47      lda $47,x	  | store parameter into location
	0db0 99 00 00   sta $0000,y	--/
	0db3 4c b6 0d   jmp L79     	--* return
					
	unused,jstat,readm,read>	
					
L79:	0db6 a6 46      ldx $46		
	0db8 ca         dex 		
	0db9 8a         txa 		
	0dba 29 03      and #$03	
	0dbc 85 46      sta $46		
	0dbe aa         tax 		
	0dbf c6 44      dec $44		
	0dc1 f0 09      beq L80     	
	0dc3 b4 4f      ldy $4f,x	
	0dc5 b9 cf 0f   lda $0fcf,y	
	0dc8 c9 ac      cmp #$ac	
	0dca f0 e0      beq L78     	
L80:	0dcc 4c b9 0f   jmp L107    	--* return from interrupt;
					
SUB4:	0dcf a2 fd      ldx #$fd	--\
	0dd1 a9 00      lda #$00	  | integral = 0
L81:	0dd3 95 17      sta $17,x	  | xdInc = 0
	0dd5 95 1c      sta $1c,x	  | vdInc = 0
	0dd7 95 2d      sta $2d,x	  | vd = 0
	0dd9 95 30      sta $30,x	  | 
	0ddb e8         inx 		  | 
	0ddc d0 f5      bne L81     	--/
	0dde a5 22      lda $22		--\
	0de0 85 1c      sta $1c		  | xd = x
	0de2 a5 23      lda $23		  | 
	0de4 85 1d      sta $1d		  | 
	0de6 a5 24      lda $24		  | 
	0de8 85 1e      sta $1e		--/
	0dea a2 8c      ldx #$8c	--\
L82:	0dec ca         dex 		  | wait a little bit ...
	0ded d0 fd      bne L82     	--/
	0def 60         rts 		--* return

	Subroutine to process a new position request
					
SUB5:	0df0 a5 01      lda $01		--\
	0df2 29 82      and #$82	  | if (not SERVO_ENABLE or 
	0df4 c9 80      cmp #$80	  |	ZINDEX_FOUND),
	0df6 f0 03      beq L83     	  |    return
	0df8 4c b7 0e   jmp L94     	--/
L83:	0dfb a5 31      lda $31		--\
	0dfd 85 63      sta $63		  | temp = xDelta
	0dff a5 32      lda $32		  | 
	0e01 85 64      sta $64		--/
	0e03 38         sec 		--\
	0e04 a9 00      lda #$00	  | 
	0e06 e5 1c      sbc $1c		  | xdInc = dataBuf[dataPtr] - xd;
	0e08 85 19      sta $19		  | 
	0e0a 85 30      sta $30		  | 
	0e0c b5 47      lda $47,x	  | 
	0e0e e5 1d      sbc $1d		  | 
	0e10 85 1a      sta $1a		  | 
	0e12 85 31      sta $31		  | 
	0e14 b5 4b      lda $4b,x	  | 
	0e16 e5 1e      sbc $1e		  | 
	0e18 85 1b      sta $1b		  | 
	0e1a 85 32      sta $32		--/
	0e1c a4 0b      ldy $0b		--\
	0e1e a2 19      ldx #$19	  | xdInc >>= ticksPerSample
	0e20 20 aa 0f   jsr SUB14   	--/
	0e23 a5 0c      lda $0c		--\ tickCnt = ticksPerSample;
	0e25 85 18      sta $18		--/
	0e27 38         sec 		--\
	0e28 a5 31      lda $31		  | 
	0e2a e5 63      sbc $63		  | accelEst = xDelta - temp;
	0e2c 85 3c      sta $3c		  | 
	0e2e a5 32      lda $32		  | 
	0e30 e5 64      sbc $64		  | 
	0e32 85 3d      sta $3d		--/
	0e34 30 0c      bmi L84     	--\
	0e36 a5 09      lda $09		  | 
	0e38 c5 3c      cmp $3c		  | 
	0e3a a5 0a      lda $0a		  | 
	0e3c e5 3d      sbc $3d		  | 
	0e3e b0 0d      bcs L85     	  | 
	0e40 90 1c      bcc L87     	  | 
L84:	0e42 18         clc 		  | 
	0e43 a5 09      lda $09		  | 
	0e45 65 3c      adc $3c		  | 
	0e47 a5 0a      lda $0a		  | 
	0e49 65 3d      adc $3d		  | if (ABS(accelEst)< accelThreshold)
	0e4b 90 11      bcc L87     	--/
L85:	0e4d a5 61      lda $61		--\
	0e4f f0 04      beq L86     	  |  { if (accelCnt == 0 || 
	0e51 c6 61      dec $61		  |        accelCnt-- == 0)
	0e53 d0 20      bne L89     	  |     { clear opFlags ACCELERATING;
L86:	0e55 a5 60      lda $60		  |     }
	0e57 29 df      and #$df	  | 
	0e59 85 60      sta $60		  |  }
	0e5b 4c 75 0e   jmp L89     	--/
L87:	0e5e a9 20      lda #$20	--\
	0e60 24 01      bit $01		  | else
	0e62 d0 0c      bne L88     	  |  { if (jstat 13 clear)
	0e64 a5 60      lda $60		  |     { set opFlags ACCELERATING;
	0e66 09 20      ora #$20	  |       accelCnt = 0x10;
	0e68 85 60      sta $60		  |     }
	0e6a a9 10      lda #$10	  |    else
	0e6c 85 61      sta $61		  |     { signal code 6;
	0e6e d0 05      bne L89     	  |     }
L88:	0e70 a9 40      lda #$40	  |  }
	0e72 20 49 0f   jsr SUB11   	--/
L89:	0e75 a2 00      ldx #$00	--\
	0e77 a0 03      ldy #$03	  | 
	0e79 38         sec 		  |
L90:	0e7a b5 30      lda $30,x	  | 
	0e7c f5 2d      sbc $2d,x	  | 
	0e7e 95 2a      sta $2a,x	  | vdInc = xDelta - vd;
	0e80 e8         inx 		  | 
	0e81 88         dey 		  | 
	0e82 d0 f6      bne L90     	--/
	0e84 a4 0b      ldy $0b		--\
	0e86 a2 2a      ldx #$2a	  | vdInc >>= ticksPerSample;
	0e88 20 aa 0f   jsr SUB14   	--/
	0e8b a5 60      lda $60		--\
	0e8d 09 40      ora #$40	  | set opFlag SERVOING
	0e8f 85 60      sta $60		--/
	0e91 ad 02 06   lda $0602	--\
	0e94 29 db      and #$db	  | clear control register bits 5 & 2
	0e96 8d 02 06   sta $0602	--/
	0e99 a6 12      ldx $12		--\
	0e9b f0 1a      beq L94     	  | if (accelEstScale != 0)
	0e9d 30 0c      bmi L92     	  |  { if (accelEstScale > 0)
	0e9f a5 3c      lda $3c		  |     { accelEst <<= accelEstScale;
L91:	0ea1 0a         asl 		  |     }
	0ea2 26 3d      rol $3d		  |    else
	0ea4 ca         dex 		  |     { accelEst >>= accelEstScale;
	0ea5 d0 fa      bne L91     	  |     }
	0ea7 85 3c      sta $3c		  |  }
	0ea9 f0 0c      beq L94     	  | 
L92:	0eab a5 3d      lda $3d		  | 
L93:	0ead c9 80      cmp #$80	  | 
	0eaf 6a         ror 		  | 
	0eb0 66 3c      ror $3c		  | 
	0eb2 e8         inx 		  | 
	0eb3 d0 f8      bne L93     	  | 
	0eb5 85 3d      sta $3d		--/
L94:	0eb7 60         rts 		--* return
					
	Routine to process a set current request 

SUB6:	0eb8 b4 47      ldy $47,x	--\
	0eba b5 4b      lda $4b,x	  | DACvalue = dataBuf[dataPtr]
	0ebc 85 40      sta $40		  | 
	0ebe 84 3f      sty $3f		--/
	0ec0 a2 00      ldx #$00	--\
	0ec2 a5 40      lda $40		  | 
	0ec4 10 01      bpl L95     	  | sign extend DACvalue
	0ec6 ca         dex 		  |
L95:	0ec7 86 41      stx $41		--/
	0ec9 a6 40      ldx $40		--\ send the current to the DAC
	0ecb 20 64 0a   jsr SUB1    	--/
	0ece a5 60      lda $60		--\
	0ed0 29 bf      and #$bf	  | clear opFlag SERVOING
	0ed2 85 60      sta $60		--/
	0ed4 ad 02 06   lda $0602	--\
	0ed7 29 db      and #$db	  | clear controlRegister bits 2 & 5
	0ed9 09 18      ora #$18	  | set controlRegister bits 3 & 4
	0edb 8d 02 06   sta $0602	--/
	0ede 60         rts 		--* return
	
       Compares the 2 byte word pointed to by X with ABS(xErr):
				
SUB7:	0edf a5 27      lda $27		--\
	0ee1 30 0b      bmi L96     	  | 
	0ee3 b5 00      lda $00,x	  | 
	0ee5 c5 26      cmp $26		  | 
	0ee7 b5 01      lda $01,x	  | if ( (X) >= ABS (xErr) )
	0ee9 e5 27      sbc $27		  |    set carry bit
	0eeb 4c f7 0e   jmp L97     	  | else
L96:	0eee 18         clc 		  |    clear carry bit
	0eef b5 00      lda $00,x	  | 
	0ef1 65 26      adc $26		  | 
	0ef3 b5 01      lda $01,x	  | 
	0ef5 65 27      adc $27		  | 
L97:	0ef7 60         rts 		--/
					
SUB8:	0ef8 38         sec 		--\
	0ef9 b5 47      lda $47,x	  | 
	0efb a8         tay 		  | temp = dataBuf[datap] -
	0efc e5 03      sbc $03		  |        stopPosition
	0efe 85 63      sta $63		  | 
	0f00 84 03	sty $03		  | stopPosition = dataBuf[datap]
	0f02 b5 4b      lda $4b,x	  | 
	0f04 a8         tay 		  | 
	0f05 e5 04      sbc $04		  | 
	0f07 85 64      sta $64		  | 
	0f09 84 04      sty $04		--/
	0f0b 18         clc 		--\
	0f0c a5 63      lda $63		  | 
	0f0e 65 23      adc $23		  | x += temp;
	0f10 85 23      sta $23		  | 
	0f12 a5 64      lda $64		  | 
	0f14 65 24      adc $24		  | 
	0f16 85 24      sta $24		--/
	0f18 a5 01      lda $01		--\
	0f1a 29 fc      and #$fc	  | clear LOOK_ZINDEX and ZINDEX_FOUND
	0f1c 85 01      sta $01		--/
	0f1e a5 03      lda $03		--\
	0f20 a4 04      ldy $04		  | call stop(stopPosition)
	0f22 20 2a 0f   jsr SUB10   	--/
	0f25 60         rts 		--* return

        This is a "stop motion" routine
					
SUB9:	0f26 85 23      sta $23		--\ [Y, A] = x
	0f28 84 24      sty $24		--/
					
SUB10:	0f2a 85 1d      sta $1d		--\ xd = [Y, A]
	0f2c 84 1e      sty $1e		--/
	0f2e a9 59      lda #$59	--\ load control register with 0x59
	0f30 8d 02 06   sta $0602	--/
	0f33 a5 60      lda $60		--\
	0f35 09 40      ora #$40	  | set opFlags SERVOING;
	0f37 85 60      sta $60		--/
	0f39 a9 00      lda #$00	--\ clear low byte of xd;
	0f3b 85 1c      sta $1c		--/
	0f3d a2 02      ldx #$02	--\
L98:	0f3f 95 19      sta $19,x	  | xdInc = 0;
	0f41 95 2a      sta $2a,x	  | vdInc = 0;
	0f43 95 2d      sta $2d,x	  | vd = 0;
	0f45 ca         dex 		  | 
	0f46 10 f7      bpl L98     	--/
	0f48 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:	0f49 24 00      bit $00		--\
	0f4b d0 18      bne L100    	  | 
	0f4d 05 00      ora $00		  | if (flag not set in low byte 
	0f4f 85 00      sta $00		  |     of jstat)
	0f51 a9 20      lda #$20	  |  { jstat |= flag;
	0f53 24 60      bit $60		  |    if (opFlags & ACCELERATING)
	0f55 f0 06      beq L99     	  |     { jstat |= 0x40;
	0f57 a9 40      lda #$40	  |     }
	0f59 05 00      ora $00		  |    set ATTN REQ;
	0f5b 85 00      sta $00		  |  }
L99:	0f5d a9 02      lda #$02	  | 
	0f5f 0d 02 06   ora $0602	  | 
	0f62 8d 02 06   sta $0602	--/
L100:	0f65 60         rts 		--* return

	Do a 3-byte add of the integer pointed to by (x) to the
	integer pointed to by (y), and store the result in the
	area pointed to by (x)
					
SUB12:	0f66 18         clc 		--\
	0f67 b5 00      lda $00,x	  |
	0f69 79 00 00   adc $0000,y	  |
	0f6c 95 00      sta $00,x	  |
	0f6e b5 01      lda $01,x	  |
	0f70 79 01 00   adc $0001,y	  | (x) += (y)
	0f73 95 01      sta $01,x	  |
	0f75 b5 02      lda $02,x	  |
	0f77 79 02 00   adc $0002,y	  |
	0f7a 95 02      sta $02,x	  |
	0f7c 60         rts 		--/
					
	Do a 4-byte shift of (x) by y. If the carry bit is set,
	negate y before proceeding. If y is positive, shift left; 
	otherwise, shift right.

SUB13:	0f7d f0 2a      beq L105    	--\
	0f7f 90 07      bcc L101    	  | if (y == 0)
	0f81 a9 00      lda #$00	  |    return;
	0f83 84 63      sty $63		  |  
	0f85 e5 63      sbc $63		  | if (carry is set)
	0f87 a8         tay 		--/    y = -y
L101:	0f88 30 0f      bmi L103    	--\
	0f8a b5 00      lda $00,x	  | if (y > 0)
L102:	0f8c 0a         asl 		  |    (x) <<= y
	0f8d 36 01      rol $01,x	  | else
	0f8f 36 02      rol $02,x	  |    (x) >>= y
	0f91 36 03      rol $03,x	  |    
	0f93 88         dey 		  | 
	0f94 d0 f6      bne L102    	  | 
	0f96 95 00      sta $00,x	  |
	0f98 60         rts		  |
L103:	0f99 b5 03      lda $03,x	  |
L104:	0f9b c9 80      cmp #$80	  |
	0f9d 6a         ror 		  | 
	0f9e 76 02      ror $02,x	  | 
	0fa0 76 01      ror $01,x	  | 
	0fa2 76 00      ror $00,x	  | 
	0fa4 c8         iny 		  | 
	0fa5 d0 f4      bne L104    	  | 
	0fa7 95 03      sta $03,x	  | 
L105:	0fa9 60         rts 		--/
					
	Do a 3-byte right shift of (x) by Y

SUB14:	0faa b5 02      lda $02,x	--\
L106:	0fac c9 80      cmp #$80	  | 
	0fae 6a         ror 		  | 
	0faf 76 01      ror $01,x	  | (x) >>= y
	0fb1 76 00      ror $00,x	  | 
	0fb3 88         dey 		  | 
	0fb4 d0 f6      bne L106    	  | 
	0fb6 95 02      sta $02,x	  | 
	0fb8 60         rts 		--/

        Return from interrupt sequence:

L107:	0fb9 68         pla 		--\
	0fba aa         tax 		  | 
	0fbb 68         pla 		  | Pop X, Y, A, and return from
	0fbc a8         tay 		  |    interrupt
	0fbd 68         pla 		  | 
	0fbe 40         rti 		--/
					
	NMI interrupt dispatch table:	
	Entry point for a particular joint command = 0x0D00 + offset
					
	   Offset  Command		
					
	0fbf 00	   CURRENT		
	0fc0 08	   STOPM		
	0fc1 08	   GRAVITY		
	0fc2 08    SETPAR		
	0fc3 08    UNKNOWN		
	0fc4 54    UNUSED		
	0fc5 2e    JSTAT		
	0fc6 22    READM		
	0fc7 00    POSITION		
	0fc8 08    TOLBND		
	0fc9 08    CALIB		
	0fca 08    INTBND		
	0fcb 08    STOPP		
	0fcc 54    unused		
	0fcd 3d    READ			
	0fce 4a    READP		
					
        Second level command dispatch table
					
	0fcf 60	   current		
	0fd0 68    stopm		
	0fd1 65    gravity		
	0fd2 ac    setpar		
	0fd3 90    unknown		
	0fd4 b6    unused		
	0fd5 b6    jstat		
	0fd6 b6    readm		
	0fd7 5a    position		
	0fd8 85    tolbnd		
	0fd9 68    calib		
	0fda a1    intbnd		
	0fdb 7c    stopp		
	0fdc b6    unused		
	0fdd b6    read			
	0fde b6    readp		
					
	0ffa 4a 08	NMI vector	
	0ffc 00 08	RESET vector	
	0ffe 60 08	IRQ vector	
					
				 MEMORY MAP
					
					
	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	
	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	kGain(1): gain of proportional and integral terms (log 2 form)
	0E X	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	vGain(1): velocity gain (log 2 form)	
        12 X 00 accelEstScale(1): scale for accel. estimate (log 2 form)
	13 X	oGain(1): overall output gain (log 2 form)	
	14 X	integral(3): accumulated 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 terms
	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	
	33 X	vAvg(3): a decaying time average of the velocity	
	34 X	
	35 X	
	36 X	vEst(4): initially set to encoder change between ticks. Is
	37 X	   later massaged into an estimate of the current velocity
	38 X	   by averaging it in with vAvg, and finally is combined with
??	39 X	   vd to produce the velocity error term
	3A X	vErrSave(2): used to store the per-tick velocity error.	
	3B X	   This value does not appear to be used.
	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	zindexCnt(2): Number of encoder counts since last zero index
	5E X	
??	5F X	lostSyncCnt(1): counter used in handling loss of encoder sync

	60 X	opFlags(1): Operational flags	
	61 X    accelCnt(1): no. of cycles in accelTreshold refresh period
	62 X	temp(3): Scratch computing space	
	63 X
	64 X	
??	65	
??	66	
	67
	68 X	time(1): Counter that is bumped once per tick. Value not used.
	69	
	6A	
	6B	
	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


                                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
