* sumotest.asm
* Assembly code for sumo-11 servo centering and motor port test.
* Dan Gates
* Feb 20th 2003
*
*Objective: Set a center frequency so servo's can be calibrated
*and send out a FWD/REV pattern to the motor ports.
*
* Port A is used as follows:
* Bit 0 = Left Servo pulse out
* Bit 1 = Right Servo pulse out
* Bits 2-7 = No Connection
*
* Servo Center Frequency is 2975
*
* Port C is used as follows:
* Bit 0 = Left motor direction a 1 for FWD (left green LED), and a 0 for REV (left red LED)
* Bit 1 = Right motor direction, a 0 for FWD (right green LED), and a 1 for REV (right red LED)
* Bits 2-5 = No Connection
* Bit 6 = Left motor Enable/Disable 1 = On, 0 = Off
* Bit 7 = Right motor Enable/Disable 1 = On, 0 = Off
*
******************************************************************************

CENTR		equ	2975
*FWD		equ	3975
*BCK		equ	1975

*start of the EEPROM

	org	$b600
  
begin:		ldaa	#4		;disable COP in CONFIG register
		staa	$103f
		lds	#$ff		;stack pointer = top of ram

		jsr	init_servos	; start up servo pulsetrain
		
		ldaa	#%11111111	;set Port C direction mask (0=input, 1=output)
		staa	$1007		;store mask to Port C Data Direction register
		
		ldaa	#0		;shut off all outputs
		staa	$1003

		
*main loop
main:		ldx	#CENTR
		ldy	#CENTR
		jsr	servo
Loop		ldaa	#%01000000
		staa	$1003
		ldaa	#$ff
		jsr	delay
		ldaa	#%01000001
		staa	$1003
		ldaa	#$ff
		jsr	delay
		ldaa	#%10000000
		staa	$1003
		ldaa	#$ff
		jsr	delay
		ldaa	#%10000010
		staa	$1003
		ldaa	#$ff
		jsr	delay
		bra	Loop
		
		
				
*****************************
* Output servo timer values *
*****************************
servo		stx	$1018	; Timer 2 (port A6) - Right servo
		sty	$101A	; Timer 3 (port A5) - Left servo
		rts
		


********************************************
* Start up servo pulse train for PA5 and 6 *
********************************************
init_servos	ldaa	#%01010000
		staa	$1020	; set timers 2 & 3 to toggle at count equal
		ldaa	#%01100000
		staa	$100C	; set timers 2 & 3 to set on OC1 overflow
		staa	$100D	; specify data states for timers 2 & 3
		rts



*************
*Delay Loop *
*************
delay:		ldab	#$ff		;inner delay loop
delay2:		decb
		bne	delay2
		deca
		bne	delay
		rts