* memory segments
RAM     equ     0
STACK   equ     $ff
DEVICES equ     $1000
EEPROM  equ     $f800

* interrupt vectors
RTI	equ	$fff0
RESET   equ     $fffe

* rti mask and timing
TMSK2	equ	$24
RTII	equ	%01000000
TFLG2	equ	$25
RTIF	equ	%01000000
PACTL	equ	$26
RTR01   equ     %00000011

* constants for motors
MOTORS  equ     $04

MOT1EN  equ     %00010000
MOT2EN  equ     %00100000
MOT3EN  equ     %01000000
MOT4EN  equ     %10000000

MOT1DIR equ     %00000001
MOT2DIR equ     %00000010
MOT3DIR equ     %00000100
MOT4DIR equ     %00001000

* data area
        org     RAM

* code area
        org     EEPROM

start
* initialization code
        ldx     #DEVICES
	ldaa	#RTR01
	staa	PACTL,x		* initialize Real Time rate
	bset	TMSK2,x #RTII	* initialize Real Time interrupts
	bset	TFLG2,x #RTIF
    
        lds     #STACK

* Enable all the motors so we'll be able to watch the RTI results
        ldaa    #MOT1EN|MOT2EN|MOT3EN|MOT4EN
        staa    MOTORS,x

* Enable interrupts.
        cli

* main loop
mloop
        bra     mloop

***
* Procedure onmotors
*
* Purpose:	to turn on the motors in correct direction.
*               Note: backs up if going left or right.
*
* Input:	motors value:	forward, stop, left, right
*
***
onmotors	pshx
	        tsx
	        ldy     0,x
		ldaa    4,x
		ldx     #50000       * do direction for ~1/2 second
onloop		staa    MOTORS,y
	        dex
		bne	onloop
	        pulx
		rts

* RTI interrupt handler
rtihan  ldaa    MOTORS,x

* If I don't have all the direction bits set, add one to the motor port.
* If I do, clear them all (so we don't get a carry into the enable bits)

        brset   MOTORS,x #MOT1DIR|MOT2DIR|MOT3DIR|MOT4DIR clearem
	inca
        staa    MOTORS,x
        bra     retcode

clearem bclr    MOTORS,x #MOT1DIR|MOT2DIR|MOT3DIR|MOT4DIR
	ldaa    MOTORS,x

retcode psha
	jsr     onmotors
	ins
	bset	TFLG2,x	#RTIF   * turn interrupts back on
	rti

* interrupt vectors
        org     RTI
        fdb     rtihan

        org     RESET
        fdb     start