* 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