{{{
'  Follow Me - Uses modified Behavior 1
'   In PBASIC
'
' { $STAMP BS2 }


'  Variables
i   var bit				'**    i = Left Eye
j   var bit				'**    j = Right Eye

i=1
j=1

'  Mainline
	high SC				'  Set the I/O Bits As O/P
	high SD				'   and High
	high LED			'  Turn OFF LED
	pause 100			'  Wait for Robot to Get Setup
	RobotData = RobotStop		'  Stop Random Movement
	gosub RobotSend

	pause 100			'  Wait for Robot to Get Setup
	low LED				'  Flash LED Twice
	pause 200
	high LED
	pause 200
	low LED
	pause 200
	high LED



MainLoop:				'  Read in the Current
	gosub S_CheckSensors
	gosub S_RotateLeft
	gosub S_RotateRight
	goto MainLoop
	end

S_RotateLeft:				'  Left Eye and seek LED
	if (~i & j) then RotateLeft
	return
    RotateLeft:
'	DEBUG "RotateLeft",CR
	RobotData = RobotLeft		'**    Robot(TurnLeft, 80ms)
	gosub RobotSend			'**    Robot(TurnLeft, 80ms)
	pause 100			'**    Robot(TurnLeft, 10ms)
	return
goto MainLoop

S_RotateRight:				'  Right Eye
	if (~j & i) then RotateRight
	return
    RotateRight:
'	DEBUG "RotateRight",CR
	RobotData = RobotRight		'**    Robot(TurnRight, 80ms)
	gosub RobotSend			'**    Robot(TurnRight, 80ms)
	pause 100			'**    Robot(TurnRight, 10ms)
	return
goto MainLoop

S_CheckSensors:				'  eye exam
	j = IN3				'**    i = Right Eye
	i = IN0				'**    j = Left Eye
	return
goto MainLoop

	end



'  SumoBot Interface Code Follows
'
'  Myke Predko
'
'  Copyright (C) 2001 & 2002 McGraw-Hill
'
'  Robot Commands
RobotStop     con  0			'  Stop the Robot
Behavior1     con  1			'  Random Movement
Behavior2     con  2			'  Photovore
Behavior3     con  3			'  Photophobe
Behavior4     con  4			'  Wall Hugger/Maze Solver
RobotForward  con  5			'  Move Forward for 200 msecs
RobotReverse  con  6			'  Move Reverse for 200 msecs
RobotLeft     con  7			'  Turn Left for 200 msecs
RobotRight    con  8			'  Turn Right for 200 msecs
RobotLEDOn    con  9			'  Turn on the Robot's LED
RobotLEDOff   con 10			'  Turn off the Robot's LED
RobotPWM0     con 11			'  PWM = 0% Duty Cycle
RobotPWM1     con 12			'  PWM = 1st "Notch"
RobotPWM2     con 13			'  PWM = 2nd "Notch"
RobotPWM3     con 14			'  PWM = 3rd "Notch"
RobotPWM4     con 15			'  PWM = 100% Duty Cycle
RobotPWM      con 16			'  Return the Current PWM Value
RobotState    con 17			'  Return the Executing State
RobotWhiskers con 18			'  Return State of the "Whiskers"
					'   Bit 0 - Left "Whisker"
					'   Bit 1 - Right "Whisker"
RobotCDSL     con 19			'  Return Value of Left CDS Cell
RobotCDSR     con 20			'  Return Value of Right CDS Cell
RobotButton   con 21			'  Return the Last Remote Button Press
					'   0 - No Buttons Pressed
					'   1 - Leftmost Button Pressed
					'   2 - Middle Button Pressed
					'   3 - Rightmost Button Pressed
					'  After "RobotButton" Operation,
					'   Button Save is Cleared

'  Robot Interface Pins
LED con 11				'  LED, Negative Active On
RIR con 12				'  Right Infra-Red Detector
LIR con 13				'  Left Infra-Red Detector
SC con 14				'  Define the I/O Pins
SD con 15

'  Robot Interface Variables
RobotData var byte			'  Data Byte to Send to/Receive
					'   from Robot

'  Robot Operation Subroutines
RobotSend				'  Send the Byte in "RobotData"
	low SC				'  Hold Low for 1 msec before
	pause 1				'   Shifting in Data
	shiftout SD, SC, LSBFIRST, [RobotData]
	high SC
	return

RobotSendReceive			'  Send the Byte in "RobotData"
	low SC				'  Hold Low for 1 msec before
	pause 1				'   Shifting in Data
	shiftout SD, SC, LSBFIRST, [RobotData]
	pause 1				'  Wait for Operation to Complete
	shiftin SD, SC, LSBPOST, [RobotData]
	high SC
	return
}}}