'  Behavior2 - With Buttons
'  Implement Behavior 2 (the Photovore) in PBASIC.  
'  Button Control Added to make the application 
'   more controllable.  
'  Button 1 (|) = Start Behavior (does this Automatically)
'  Button 2 (||) = Stop Behavior and Run from Remote
'
'
'  Myke Predko
'
'  Copyright (C) 2001 McGraw-Hill
'
' { $STAMP BS2 }


'  Variables
i   var byte
j   var byte


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

	pause 100				'  Wait for Robot to Get Setup 
	RobotData = RobotLEDOn		'  Flash LED Twice
	gosub RobotSend
	pause 500
	RobotData = RobotLEDOff
	gosub RobotSend
	pause 500
	RobotData = RobotLEDOn
	gosub RobotSend
	pause 500
	RobotData = RobotLEDOff
	gosub RobotSend

Behavior2Loop:				'  Read in the Current
	RobotData = RobotButton		'  Any Buttons There?  
	gosub RobotSendReceive
	if (RobotData = 1) then Behavior2Start
	if (RobotData = 2) then Behavior2End
						'  If nothing or Button3
Behavior2Start				'   Just Carry On
	RobotData = RobotLEDOff		'  Turn off LED
	gosub RobotSend
	pause 75				'  Delay before Starting Again
	RobotData = RobotCDSL		'   Light Levels at Both
	gosub RobotSendReceive		'   CDS Cells
	i = RobotData			'**    i = LeftCDS
	pause 75
	RobotData = RobotCDSR		'**    j = RightCDS
	gosub RobotSendReceive		'**    j = RightCDS
	j = RobotData			'**    j = RightCDS
	pause 75

  	if (i = j) then Behavior2Forward	'  Straight if Equal
  	if (i > j) then Behavior2Right   	'  Turn towards smaller
							'   CDS Cell Value
Behavior2Left:				'  Left CDS Cell 
	RobotData = RobotLeft		'**    Robot(TurnLeft, 80ms)
	gosub RobotSend			'**    Robot(TurnLeft, 80ms)
	pause 75				'**    Robot(TurnLeft, 80ms)
	RobotData = RobotStop		'**    Robot(TurnLeft, 80ms)
	gosub RobotSend			'**    Robot(TurnLeft, 80ms)
	goto Behavior2Forward

Behavior2Right:				'  Right CDS Cell
	RobotData = RobotRight		'**    Robot(TurnRight, 80ms)
	gosub RobotSend			'**    Robot(TurnRight, 80ms)
	pause 75				'**    Robot(TurnRight, 80ms)
	RobotData = RobotStop		'**    Robot(TurnRigh, 80ms)
	gosub RobotSend			'**    Robot(TurnRigh, 80ms)

Behavior2Forward:				'  Finished Turn, go Forward
	pause 75
	RobotData = RobotWhiskers	'  Look for Collisions
	gosub RobotSendReceive
	if (RobotData <> 0) then Behavior2End
	pause 75
	RobotData = RobotForward	'**    Robot(Forward, 80msecs)
	gosub RobotSend			'**    Robot(Forward, 80msecs)
	pause 75				'**    Robot(Forward, 80msecs)
	RobotData = RobotStop		'**    Robot(Forward, 80msecs)
	gosub RobotSend			'**    Robot(Forward, 80msecs)
	pause 75				'**    Robot(TurnLeft, 80ms)
	RobotData = RobotWhiskers	'  Look for Collisions
	gosub RobotSendReceive
	if (RobotData = 0) then Behavior2Loop

Behavior2End:				'  At Light Source, Stop
	pause 75				'  Delay before Starting Again
	RobotData = RobotLEDOn		'  Turn on LED
	gosub RobotSend
	pause 75				'  Delay before Starting Again
	RobotData = RobotButton		'  Any Buttons There?  
	gosub RobotSendReceive
	if (RobotData = 1) then Behavior2Start
	goto Behavior2End			'  No Start, Just Loop Around



'  Robot Interface Code Follows:
'  
'  Myke Predko
'
'  Copyright (C) 2001 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
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