*****************************************************************************
* Servo-11a.asm
* Assembly code for Sumo11 Mini-sumo robot
* Dan Gates
* Feb 22th 2003
*****************************************************************************
*
* 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 1 for FWD (right green LED), and a 0 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
*
* Port E is used as follows:
* Bits 0-4 = No Connection
* Bit 5 = Right line sensor
* Bit 6 = Left line sensor
* Bit 7 = No Connection
*
* Objective: After power on, delay for 5 seconds. Move randomly across board
* while monitoring line sensor. If line is detected at any time back up, turn
* and continue.
******************************************************************************
OPTION		equ	$1039
ADCTL		equ	$1030
ADR1		equ	$1031
ADR2		equ	$1032
ADR3		equ	$1033
ADR4		equ	$1034
LEDS		equ	$1003
RLINE		equ	$10
LLINE		equ	$10
CENTR		equ	2975
CCW		equ	3975
CW		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_Analog	;power up the charge pump for analog conversion

		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

		jsr	Init_Servos	; start up servo pulsetrain
		
*5 sec delay
		ldy	#$01bb		;set up delay outer loop size
		jsr	delay		;delay for 5 seconds
		
*main loop
main		ldab	RLINE		;read analog value at AN5
		jsr	ad_readr
		cmpb	#$40
		bhs	bcknlft		;if result is 80 or higher then branch to bcknlft
		
		ldaa	LLINE		;read analog value at AN6
		jsr	ad_readl
		cmpa	#$40
		bhs	bcknrt		;if result is 80 or higher then branch to bcknrt
forward		ldx	#CW		;else go forward
		ldy	#CCW
		bsr	Servo
		ldaa	#%11000001
		staa	$1003		
		bra	main			


*operations

bcknlft		ldx	#CCW
		ldy	#CW
		bsr	Servo		;set servos to reverse
		ldaa	#%11000010
		staa	$1003		;show wheel direction on LEDs
		ldy	#$0020		;set backup delay
		bsr	delay
		ldx	#CW
		ldy	#CW
		bsr	Servo		;set servos to turn right
		ldaa	#%11000000
		staa	$1003		;show wheel direction on LEDs
		ldy	#$0040		;set turn delay
		bsr	delay
		bra	forward		;return to main loop
		
bcknrt		ldx	#CCW		
		ldy	#CW
		bsr	Servo		;set servos to reverse
		ldaa	#%11000010	
		staa	$1003		;show wheel direction on LEDs
		ldy	#$0020		;set backup delay
		bsr	delay
		ldx	#CCW
		ldy	#CCW
		bsr	Servo		;set servos to turn left
		ldaa	#%11000011
		staa	$1003		;show wheel direction on LEDs
		ldy	#$0040		;set turn delay
		bsr	delay
		bra	forward		;return to main loop
		
				
*****************************
* Output servo timer values *
*****************************
Servo		stx	$1018	; Timer 2 (port A6) - Right servo
		sty	$101A	; Timer 3 (port A5) - Left servo
		rts
		
*********************************
* Right ad_read: A/D subroutine *
* Pass desired A/D channel in A *
* Value returned in A           *
*********************************
ad_readl	staa	ADCTL	;start the conversion
adrloop		ldaa	ADCTL	;get the status
		bpl	adrloop	;wait until complete, bit 7 is set
		ldaa	ADR2	;return the result in A
		rts
		
*********************************
* Left ad_read: A/D subroutine  *
* Pass desired A/D channel in B *
* Value returned in B           *
*********************************
ad_readr	stab	ADCTL	;start the conversion
adlloop		ldab	ADCTL	;get the status
		bpl	adrloop	;wait until complete, bit 7 is set
		ldab	ADR3	;return the result in B
		rts

************************************
* delay: delay subroutine          *
* Pass outer loop value in Y       *
************************************
delay		ldx	#$0fff		;inner delay loop
delay2		dex
		bne	delay2
		dey
		bne	delay
		rts
		
*************************
* A/D Power up Routine  *
*************************
Init_Analog     ldaa	OPTION		;set ADPU to enable A/D
		oraa	#%10000000
		staa	OPTION
		ldy	#$0A
		bsr	delay
		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