*
*  Lab4
*
*  Purpose: Make the car bounce of the wall given
*           sensor input.  If the right sensor is
*           triggered, go left.  If the left sensor
*           is triggered, go right;  otherwise,
*           go straight.
*
*  Author: Heather D. Pfeiffer
*
* Setup some useful labels
STACK           equ	  $00ff
EEPROM	        equ	  $f800
RESET	        equ	  $fffe
FORWARD	        equ       $aa
BACKWARD	equ       $a0
LEFT            equ       $a2
RIGHT           equ       $a8
SWOFF		equ	  $ff
SWLEFT		equ       $fe
SWRIGHT		equ	  $fb
SWITCH	        equ	  $1003
MOTORS	        equ	  $1004

	
* Program code for bouncing off the walls
		org	  EEPROM
start
		lds	  #STACK       * initialize the stack
dins
		ldab	  SWITCH       * read the digital inputs
		cmpb	  #SWOFF       * see if any sensors are pressed

* check if turning; if not, go forward; otherwise, which way
		bne	  turn
		ldaa      #FORWARD     * go forward
		bra       cont
turn                        
		cmpb      #SWRIGHT     * see if turn left
		bne       rturn
		ldaa	  #LEFT
	        bra       cont
rturn
		ldaa      #RIGHT       * turn right [default]
cont
		psha
		jsr	  onmotors
		ins
	        bra       dins

***
* Procedure onmotors
*
* Purpose:	to turn on the motors in correct direction.
*               Note: backs up if going left or right.
*
* Input:	motors value:	forward, left, right
*
***
onmotors	tsx
		ldaa    2,x
		ldab    #BACKWARD
		cmpa    #FORWARD     * if forward;  do direction loop
		beq     secloop
		ldx     #25000       * if turn;  go backwards for
onloop1         stab    MOTORS       *   ~1/4 second first
		dex
		bne     onloop1
secloop		ldx     #50000       * do direction for ~1/2 second
onloop		staa    MOTORS
	        dex
		bne	onloop
		rts
	 
*** Setup the reset vector
		org     RESET
	        fdb     start