'************************************************************************
' IRBOT
'
' Object avoiding robot that uses my home-made IR sensor, a bump
' switch and two CDS photocells to detect light.  The IR is debounced by 
' several readings to eliminate false echoes hopefully. 
' Operation:
' Output a 'low' on port 5 to enable the left looking IR LED
' Output a 'low' on port 6 to enable the right looking IR LED
' Look for a consistant 'low' on port 7 to detect an obstruction
' A 'low' on pin 2 signifies that IRBOT ran into something 
' Port 4 is the right light sensor, low values mean lots of light
' Port 3 is the left light sensor, same as above
' Port 1 controls the right motor
' Port 0 controls the left motor
' 
' IRBOT can be set to just avoid obstacles or be photophobic (light 
' avoiding) or phototrophic (light seeking).  To set the various modes,
' do the following:
'  To just avoid obstacles, just turn it on in normal or bright light
'  To be photophobic, cover the left photocell for two seconds when turned on
'  To be phototrophic, cover the right photocell for two seconds on turnon
' 
' IRBOT will now choose semi-randomly the backup mode when it is either
' light sensitive, not just avoiding.
'
' DLC 10/9/98
'************************************************************************

SYMBOL side = b1
SYMBOL mot_ctl = w2
SYMBOL mot_l = b4
SYMBOL mot_r = b5
SYMBOL timer = b7
SYMBOL fulsec = 15      '50 = about 1 second
SYMBOL longsec = 50
SYMBOL sright = b8      'light sensors
SYMBOL sleft = b9
SYMBOL literight = 3
SYMBOL liteleft = 4
SYMBOL temp1 = b10
SYMBOL temp2 = b11
SYMBOL scale = 40       'new factor 10/5
SYMBOL DARK  = 254      'Its dark
SYMBOL BRIGHT = 25      'Its bright
SYMBOL modeflag = b6    'flag for mode determination

SYMBOL F = $6464        'Go forward
SYMBOL R = $9664        'Turn Right     
SYMBOL L = $6496        'Turn Left
SYMBOL BR = $C896       'Back Right
SYMBOL BL = $96C8       'Back Left
SYMBOL B = $C8C8        'Straight Back
SYMBOL PB = $64C8       'Pivot back
SYMBOL RB = $C864       'The other way
SYMBOL SS = $9696       'stand still

SYMBOL RMOT = 1         'Right Motor
SYMBOL LMOT = 0         'Left Motor
SYMBOL LEFT = 5         'Left LED
SYMBOL RIGHT = 6        'Right LED

START:  
	pause 2000      	   'Just wait 2 seconds
	gosub lookout		   'check for mode 
	if b0 = 1 then bphobic     'be photophobic
	if b0 = 2 then btropic     'be phototropic
	modeflag = 0               'just avoid stuff
	goto testit
bphobic:
	modeflag = 1                    
	goto testit
btropic:
	modeflag = 2
testit:
	b0 = 0                      	'sense flag
	gosub lookout                   'Look at IR sensors
	if pin2 = 0 then oops           'bump switch
	if b0 <> 0 OR modeflag = 0 then brnch        'avoid first
	pot liteleft,scale,sleft        'look for darkness
	pot literight,scale,sright      'look for darkness
	sleft = sleft -1                'convert 0 to 255
	sright = sright -1
	temp1 = sleft/sright * 100      'bug in Stamp makes me do this
	temp2 = sright/sleft
	temp1 = temp1 + temp2
	if modeflag = 1 then phobic     'be photophobic
tropic:
	if temp1 >= 200 then gright                     'darker to left
	if temp1 >= 2 AND temp1 < 100 then gleft        'darker to right
	if sright <= BRIGHT AND sleft <= BRIGHT then goforit'Nice and bright
	if sright >= DARK AND sleft >= DARK then oops   'go back to light!
phobic:
	if temp1 >= 200 then gleft                      'darker to left
	if temp1 >= 2 AND temp1 < 100 then gright       'darker to right
	if sright <= BRIGHT AND sleft <= BRIGHT then oops 'WAY too bright
	if sright >= DARK AND sleft >= DARK then stand  'stay in the dark
	goto brnch
gleft:
	b0 = 2
	goto brnch
gright:
	b0 = 1
	goto brnch
goforit:
	b0 = 0
	goto brnch
stand:
	pause 5000
oops:
	b0 = 4

brnch:
	lookup b0,(F,R,L,BR,PB),mot_ctl           'choose path
	if b0  > 2 then long                      'backing up
	timer = fulsec                            'normal timer
	goto loopit
long:
	timer = longsec                		  'a real second
loopit:
	for b2 = 1 to timer             'Turns motors on 
		pulsout RMOT,mot_r
		pulsout LMOT,mot_l
		pause 15
	next
	goto testit

'Check IR sensor
lookout: 'Look left
	high RIGHT
	low LEFT
	side =1
	goto dbounce
rstart:  'Look right
	high LEFT
	low RIGHT
	side = 2

dbounce:
	b2 = pin7
	for b3 = 1 to 3                 'Take four readings to debounce
	    pause 1          
	    b2 = b2 + pin7
	next
	if b2 = 0 then detected         'got a repeatable transition
	if side = 1 then rstart         'this was left side, now do right
finished:
	return
detected:
	b0 = b0 + side            	'add left and right reading
	if side = 1 then rstart         'now do the right side
	if b0 = 3 AND modeflag <> 0 then rbckup
	goto finished                   
rbckup:
	b3 = sright & 1                 'semi random backup mode
	b0 = b0 + b3
	goto finished