ORG	$B600	; start of EEPROM
*
*	Read A/D from port E, pins 0-3.  
*	Set 4 servo positions based on A/D data
*
	ldx	#$1000		; needed for the brclr command
	ldaa	#%10010000	; Power up A/D with clock delay
	staa	$39,x
	bsr	Init_4_Servos
Main:
	ldaa	#%00010000	; single scan, multi-mode, pins e0-3
	ldx	#$1000		; needed for the brclr command
	staa	$30,x		; write starts conversion
Conversion_not_done
	brclr	$30,x $80 Conversion_not_done

	ldy	#$1018
	ldaa	#4
A2D_Loop:
	psha
	ldaa	$31,x		; get value from pin E0, E1, E2, or E3
*
*	Servo high byte = A2d value / 2
*	Servo low byte 00
*
	clrb
	lsrd
	lsrd
	lsrd
	lsrd
	addd	#$0700
	std	$00,y
	inx
	iny
	iny
	pula
	deca
	bne	A2D_Loop
*
	bra	Main		; let's do it again
*
*=======================================================================
*	The robot device-driver code is from here on down...
*=======================================================================
*  OK, what we want to do is to get a PWM signal going for a
*  servo motor.  The free-running counter will cycle at
*  500nS * 2^16 = 32.768 mS, which will work nicely
*  (Assuming 8 MHz Xtal).  We want a range from 1 mS to 2 ms
*  high, and the rest low.  Comparing to the counter values,
*  starting at $0000, 1 mS later we have $07D0, 1.5 mS at
*  $0BB8, and 2 mS at $0FA0 (decimal 2000, 3000, and 4000)
*  If we set the servo output(s) high at $0000 of the free-
*  running counter, we can use the 4 timer compare registers
*  to control bits 3, 4, 5, & 6 on port A.  Use the desired
*  value in the timer compare and configure the compare to
*  toggle the port bit.  This will happen automatically;
*  the port bits are set high when the free-running counter
*  rolls over, using OC1.  No interrupts are needed.
*
Init_4_Servos:
   psha
   pshx
   ldx     #$1000          ; set index to control registers
   ldaa    #%01010101
   staa    $20,x   ; set timers 2, 3, 4 & 5 to toggle at count equal
   ldaa    #%01111000
   staa    $0C,x   ; set timers 2, 3, 4 & 5 to set on OC1 overflow
   staa    $0D,x   ; specify data states for timers 2, 3, 4, & 5.
   pulx
   pula
   rts