'BII routine to drive BoE-Bot Parallax pinout
'DLC revision 11/10/99
'
'Generic values
i	var	byte			'loop counter, whatever
tmp	var	word
seed	var	word			'random number seed

'These are for the servo routines
LEFT	con	15			'left wheel port
RIGHT	con	3			'right wheel port
SACT	con	2			'times through act routine
drive	var	word			'wheel command combo
ldrive	var	drive.byte1		'left wheel command
rdrive 	var	drive.byte0		'right wheel command
aDur    var     byte			'duration of pulse left

'Servo drive commands
fd	con	$6432			'forward
rv	con	$3264			'reverse
st	con	$4b4b			'stop
tr	con	$644b			'turn right
tl	con	$4b32			'turn left
rr	con	$6464			'rotate right
rl	con	$3232			'rotate left

'wander values
wlstate var	byte			'shared byte
wstate	var	byte			'FSM status
wDir	var	word			'wander value
wDur	var	byte			'wander duration

'light looker vars and constants
LLIGHT	con	11			'left photosensor
RLIGHT	con	7			'right photosensor
pleft	var	word			'left value
pright	var	word			'right value
lstate	var	byte 			'FSM state
lDur	var	byte			'how long to go there
lDir	var	word			'direction to turn
LMARG	con	20			'difference margin

'IRPD vars and constants
ileft	var	in9			'IR LED outputs
iright	var	in0			' i=(in6 *2) + in5
IEN	con	5			'enable for 555
ilast	var	byte			'what did it see last?

'sleep cricket noise
SPKR	con	4			'speaker output

'bumper vars and constants
bumper	var	in12			'bumper io pin
bstate	var	byte			'FSM state
bDir	var	word			'bumper direction holder
bDur	var	byte			'duration in that direction

'dbug:
'    high IEN
'    count 6,100,tmp			'setting 555 for 38-40KHz
'    debug dec5 tmp*10,cr
'goto dbug

'set up for running
wstate =0				'initial wander state
lstate =0				'initial lightlook state
ilast =0				'clear IRPD readings

main:
    gosub wander
    gosub lightlook
    gosub avoid
'    gosub bumpck
    gosub gosleep
    gosub act
'debug "left= ",DEC pleft," right= ",DEC pright,cr
goto main

'===========================
'    Behaviours
'===========================

wander:					'randomly wander around
    branch wstate,[wcDir,wcDur]		'state 2 immed. follows
    wDur = wDur - 1
    if wDur > 0 then wDone1
        drive = wDir			'get direction
        wstate = 0			'reset state
wDone1:					'completed
    return
wcDir:					'choose direction
    random seed				'random direction
    i = seed & %111			'mask off for 0-7 only
    lookup i,[fd,fd,fd,tr,fd,fd,fd,tl],wDir'chose direction
    wstate = 1				'next state
    return
wcDur:					'choose duration
    random seed 			'random direction and duration
    wDur = (seed & %111111) + 20	'mask for 64 choices
    wstate = 2				'next state
    return

act:					'moves servo motors
    if aDur > 0 then aDec		'already doing one, got here
	aDur = SACT			'times through this one
        pulsout LEFT,ldrive * 10
	pulsout RIGHT,rdrive * 10
aDec:					'decrement stuff
        aDur = aDur - 1
aDone:
return

lightlook:				'look for light
    low LLIGHT				'set up for sensors
    low RLIGHT				'branch takes 200us min

    branch lstate,[lread1,lread2,lcomp1,lcomp2]
    lDur = lDur - 1			'state 4 decr duration
    drive = lDir			'in correct direction
    if lDur > 0 then lDone1		'still decrementing
        lstate = 0			'restart FSM
lDone1:					'completed state 0
    return
lread1:					'state 0 read left sensor
    rctime LLIGHT,0,pleft		'get left light reading
    lstate = 1				'go to next state
    return
lread2:					'state 1 read right sensor
    rctime RLIGHT,0,pright		'get right light reading
    tmp = pright >> 1			'compensation for mismatch
    pright = pright + tmp
    lstate = 2				'go to next state
    return
lcomp1:					'state 2 first compare
    tmp = pleft +LMARG			'set threshhold
    if tmp > pright then lDone2		'left not past threshhold
        lDir = tr			'bright to left, turn away
        lDur = 30			'for a little while
        lstate = 4			'go to decr state
        return
lDone2:	
    lstate = 3				'go to second compare state
    return				'completed
lcomp2:					'state 3 second compare
    tmp = pright +LMARG			'set threshhold
    if tmp > pleft then lDone3		'right not past threshhold
        lDir = tl			'bright to right, turn away
        lDur = 30			'for a while
        lstate = 4			'go to decr state
        return				
lDone3:					'completed
    lstate = 0				'none past threshhold
    return

avoid:					'IRPD routine
    High IEN				'enable 555
    i=0
    i = ileft * 2 + iright		'2=left, 1=right, 0=both
    low IEN				'disable 555
    if ilast = i then ickit
        goto iDone
ickit:					'choose new direction
    lookup i,[rr,tr,tl,drive],tmp	'set drive here
    drive = tmp    
    i = 0				'clear history
iDone:					'completed
    ilast = i
return

goSleep:				'go to sleep in the dark
'This depends on the last values from lightlook, this saves time
   if pleft < 150 AND pright < 150 then slDone
        drive = st			'nap time
        random seed			'random cricket noises
        seed = seed >>8
        tmp = seed & %1111
        if tmp < 14 then slDone
            freqout SPKR,20,2000
            freqout SPKR,20,3000
            freqout SPKR,20,2000
	    freqout SPKR,20,3000
slDone:					'finished
return

bumpck:					'Bumper reaction FSM
    branch bstate,[blook,bbup,bta]	'states 0-2, 3 immed. follows
    drive = bDir			'use current direction
    bDur = bDur - 1			'decrement duration
    if bDur > 0 then bDone1		'not done yet
        bstate = 0			'reset FSM
bDone1:					'completed
    return
blook:					'state 0 look at bumper
    if bumper = 1 then bDone2		'nothing happened
        bstate = 1			'go to next state
        bDir = rv			'backup now
	bDur = 100			'for a while
bDone2:					'completed
    return
bbup:					'state 1, backup for a while
    drive = bDir			'set direction
    bDur = bDur - 1			'decrement it
    if bDur > 40 then bDone3		'still backing up
        bstate = 2			'next state
        bDir = rl			'rotate away
bDone3:					'completed
    return
bta:					'state 2 turning away
    drive = bDir			'set direction
    bDur = bDur - 1			'dec count
    if bDur > 0 then bDone4		'not done yet
        bstate = 0			'start over at state 0
bDone4:					'completed
    return

end