'{$STAMP BS2}
' Program		: ROVER
'
' Description	: "Very simple" Rover program for the BOE-BOT
'			  Demonstrates the use of the Compubotics IRPD100 Sensor
'			  to detect short and long-range obstacles and react accordingly
'			  
'			  Provided as a demonstration only - I'm no authority on Stamp programming!
'
' Pin Usage	
'--------------------------------
'	12	Right servo
'	13	Left Servo
'	0	Left sensor
'	1	Right sensor
'	2	Range selector
'
' Compubotics
' DougE@compubotics.com
' Released to the public domain
'

rightServo 		con 	12
leftServo 		con 	13
leftIRPD		con	0
rightIRPD		con	1
rangeSelect		con 	2
longRange		con	0
shortRange		con	1			

speedStop		con 	0
speedCenter		con	750
speedSlow		con	125
speedCruise 	con	220

leftSpeed		var 	word 
rightSpeed		var	word

active		con	0
inactive		con	1

		
leftIRPDshort 	var	byte
rightIRPDshort 	var 	byte
leftIRPDlong	var	byte
rightIRPDlong	var	byte

'----------------------------
' initialization
'----------------------------
init

	output rangeSelect
   	low rightServo
    	low leftServo
	leftIRPDshort  = inactive
	leftIRPDlong   = inactive
	rightIRPDshort = inactive
	rightIRPDlong  = inactive

	' set long range mode by default
	low rangeSelect

	' wait a couple of seconds
	pause 2000

'----------------------------
' main processing loop
'----------------------------
main:

	'check the sensors
	gosub checkIRPD

	'make proper course adjustments
	gosub navigate

	'activate servos
	gosub pulseServos

	' we need to waste about 7 ms
	pause 7

	goto main


'----------------------------------
' navigate based on sensor readings
'----------------------------------
navigate:


	' first, check for short-range obstruction
	if leftIRPDshort = active AND rightIRPDshort = active then rotateLeft

	' obstruction close left
	if leftIRPDshort = active then hardRight

	' obstruction close right
	if rightIRPDshort = active then hardLeft
	
	' something there, far left
	if leftIRPDlong = active then turnRight

	' something there, far right
	if rightIRPDlong = active then turnLeft

	' clear ahead
	leftSpeed = speedCruise
	rightSpeed = speedCruise

	return


rotateLeft:

	leftSpeed = -speedSlow
	rightSpeed = speedSlow
	return 


rotateRight:

	leftSpeed = speedSlow
	rightSpeed = -speedSlow
	return

turnLeft:

	leftSpeed = speedSlow
	rightSpeed = speedCruise
	return

turnRight

	leftSpeed = speedCruise
	rightSpeed = speedSlow
	return

hardLeft:
	
	leftSpeed = speedStop
	rightSpeed = speedCruise
	return

hardRight:
	
	leftSpeed = speedCruise
	rightSpeed = speedStop
	return



'----------------------------
' check the IRPD sensors
'----------------------------
checkIRPD:
	
	' we're always in long-range mode on entry here

	' get long range reading
	leftIRPDlong  = in0
	rightIRPDlong = in1

	' switch to short-range mode
	high rangeSelect

	' wait 10ms for change to take effect
	pause 10
	
	' get short-range reading
	leftIRPDshort  = in0
	rightIRPDshort = in1

	' switch back to long-range
	low rangeSelect

	return

'-------------------------------
' pulse the servos using the
' currently active speed values
'-------------------------------
pulseServos:

	pulsout rightServo, (speedCenter-rightSpeed)
	pulsout leftServo, (speedCenter+leftSpeed)

	return