' -----[ Title ]-----------------------------------------------------------
'
' File... obstacle.bas
' Purpose... RoboBoard controlled robot finds its way out of a single exit arena.
' Author.... Chuck Hellebuyck
' Started... October 15, 2002
' Updated... 

' -----[ Program Description ]---------------------------------------------
'
' This article describes how I used three Sharp infrared detectors as the eyes of my robot to
' successfully navigate its way out of circular arena with only one entrance/exit. 

' Hardware Connections:
'	
' AX0		Sharp GP2D12
' P0		Sharp GP2D15 Left
' P1		Sharp GP2D15 Right
' P8		Left Servomotor
' P9		Right Servomotor
' P14		Left LED
' P13		Center LED
' P12		Right LED		       
' 
'-----[ Constants ]------------------------------------------------------------

LLED		con	P14		' Left LED indicator
CLED		con	P13		' Center LED indicator
RLED		con	P12		' Right LED indicator
LSERV	con	P8		' Left Servomotor
RSERV	con	P9		' Right Servomotor

'-----[ Variables ]------------------------------------------------------------

LSENS	var		IN0		' Left IRD sensor
RSENS	var		IN1		' Right IRD sensor
scanrange 	var 	word		' A/D result variable
floating 	var 	long		' Floating point math result storage
val 		var 	word		' Table conversion result storage
x		var		byte	' General purpose variable

'----[ Initialization ] -----------------------------------------------------------

init
setpullups pu_on	' Turn on portB pull-up resistors
low LLED			' All LEDs off to start
low CLED
low RLED

'----[ Main Loop ] -----------------------------------------------------------

main
	adin ax0,3,AD_RON, val
	
	if val < 450 and lsens = 0 and rsens = 0 then
	goto move
	
	elseif val > 450 
	goto rev
	
	elseif lsens = 1 
	goto lft
	
	elseif rsens = 1
	goto rht
	
	endif
	
	goto main
	
rev
	high LLED
	high CLED
	high RLED
	for x = 1 to 5
	servo lserv, -1200, 1	' Drive left wheel in reverse
	servo rserv, 1200, 1	' Drive right wheel in reverse
	next

	servo rserv, 1200, 3
	
	goto main			' Back to top to get new A/D sample

move
	low LLED				' Left LED off
	low CLED				' Center LED off
	low RLED				' Right LED off
	for x = 1 to 5			' Control forward movement
	servo LSERV, 1200, 1	' Turn left wheel
	servo RSERV, -1200, 1	' Turn right wheel
	next

	goto main
	
lft
	high LLED				' Left LED on
	low CLED				' Center LED off
	low RLED				' Right LED off
	servo LSERV, 1200, 2	' Turn robot left, away from obstacle  

	goto main
	
rht
	low LLED				' Left LED off
	low CLED				' Center LED off
	high RLED				' Right LED on
	servo RSERV, -1200, 2	' Turn robot right, away from obstacle

	goto main				' Get new Center IRD sample