'
'  ready04.bas
'
'  This code supports the motors, the lights, and the keyboard.
'


include  "regs11.lib"


'
'  Motor control constants, used by the RTI ISR code to support the
'  motors.  These constants assume the motors connect as follows:
'
'  PB0     1 means right motor goes forward, 0 means no motion
'  PB1     1 means right motor reverses, 0 means no motion
'  PB2     1 means left motor goes forward, 0 means no motion
'  PB3     1 means left motor reverses, 0 means no motion
'
'  Note that having PB0 and PB1 both high, or PB2 and PB3 both high, is
'  an illegal state, and might damage the drive electronics.
'

const  RMTR_FWD = $01
const  RMTR_REV = $02
const  LMTR_FWD = $04
const  LMTR_REV = $08
const  RMTR_STOP = 0
const  LMTR_STOP = 0

const  DEBOUNCE = 10
keytable:
datab	0, 'W', 'F', 0, 'H', 'L', 'B', 0, 'R', '?', 'G', 0, 'X'

declare  reset
declare  rmtr
declare  lmtr
declare  rpwm
declare  lpwm
declare  t
declare  k
declare  wait
declare  n
declare  newkey
declare  oldkey
declare  keywait
declare  keyrow
declare  scnkey
declare  char


'
'  RTI interrupt service routine
'
'  This ISR supports down-counting timers, such as WAIT.  It also
'  supports PWM of both motors, through the RPWM and LPWM pattern
'  variables and the two motor control variables, RMTR and LMTR.
'
'  This routine assumes port b controls the motors.  See the constant
'  declarations above for pin assignments.
'

interrupt  $fff0				' RTI ISR
pokeb tflg2, $40				' rearm rti
if wait <> 0
	wait = wait - 1
endif
if keywait <> 0
	keywait = keywait - 1
endif
t = 0
if rpwm and 1 = 1
	t = rmtr
endif
if lpwm and 1 = 1
	t = t or lmtr
endif
pokeb portb, ((peekb(portb) and $f0) or t)
rpwm = rroll(rpwm)
lpwm = rroll(lpwm)
end



'
'  SetMotor      change the direction of the current motion
'
'  Invocation:  gosub SetMotor, LeftDir, RightDir
'
'  Note:  This routine momentarily disables interrupts while it changes
'  the low-level motor variables lmtr and rmtr.
'

SetMotor:
interrupts off
rmtr = pull()
lmtr = pull()
interrupts on
return



ScanKeys:
keyrow = $30						' mask to pull one row low
for n=0 to 2						' for all three rows...
	pokeb  portc, ((peekb(portc) and $c0) or keyrow)	' pull a row low
	scnkey = peekb(portc) and $07	' read the cols
	if scnkey <> 7					' if one col is low...
		exit						' leave the loop now
	endif
	keyrow = lshft(keyrow) + $08	' move to next row
	keyrow = keyrow and $38			' keep within proper 3 bits
next
pokeb  portc, (peekb(portc) or $38)	' leave with all rows high
scnkey = scnkey xor 7				' invert result bits
if scnkey = 0						' if 0, no key was pressed
	n = 0							' show as return value
else
	n = (n * 4) + scnkey			' convert to unique value
endif
return  n




GetKey:
newkey = usr(ScanKeys)				' check the keys
if newkey = 0						' if nothing pressed...
	oldkey = 0						' mark this as a released key
	return  0						' show it and leave
endif
keywait = DEBOUNCE					' get debounce delay
do
loop until keywait = 0				' wait it out
if usr(ScanKeys) = newkey			' if the same key is down...
	if newkey <> oldkey				' and different from last time...
		oldkey = newkey				' record the new key
	else							' this is same key as last time...
		newkey = 0					' only show key on first press
	endif
else								' key changed between scans; noise
	newkey = 0						' show nothing pressed
	oldkey = 0						' erase memory also
endif
if newkey <> 0
	printx newkey
endif
return    peekb(addr(keytable) + newkey)	' convert to ASCII




WaitLoop:
wait = pull() * 250
do
	k = usr(GetKey)
	if k = 'X'
		gosub SetMotor, LMTR_STOP, RMTR_STOP
		reset = 1
	endif
loop until wait = 0
return




main:
rpwm = $5555
lpwm = $5555
gosub  SetMotor, LMTR_STOP, RMTR_STOP
pokeb  baud, $30					' 9600 baud
pokeb  sccr2, $0c					' turn on rcvr and xmtr
print
print  "ready04"
print

pokeb  portb, 0						' all port b lines low
pokeb  ddrc, $f8					' low 3 bits are inputs for keypad
pokeb  tflg2, %01000000
pokeb  tmsk2, %01000000

oldkey = 0							' init the ScanKey variable
wait = 0
interrupts on

reset = 1
do
	if reset <> 0
		do
			k = usr(GetKey)
		loop until k = 'G'
		reset = 0
	endif

	interrupts off
	lpwm = $5555
	rpwm = $5555
	interrupts on
	gosub  SetMotor, LMTR_FWD, RMTR_FWD
	gosub  WaitLoop, 3

	if reset = 0
		interrupts off
		lpwm = $eeee
		rpwm = $1111
		interrupts on
		gosub  WaitLoop, 2
	endif

	if reset = 0
		interrupts off
		lpwm = $1111
		rpwm = $eeee
		interrupts on
		gosub  WaitLoop, 2
	endif

loop
end