' Copyright 2002 Pololu Corporation.
'
' F R O N T   B U M P E R   R O B O T
' This program demonstrates how to use the Pololu micro dual serial motor 
' controller with a BASIC Stamp II. A basic circuit is mounted on a LEGO
' chassis with differential drive and two front bumpers.
'
' The program makes the robot drive forward until a bumper hits something,
' at which point the robot backs away and turns from the obstacle.  For 
' more details, see http://www.pololu.com/projects/prj0002/


' *** Disclaimer ***
' This program is intended solely as an example of how to use
' Pololu products.  Although we try our best to test the functionality of
' sample code, it is provided "as is", and we provide no guarantees that it 
' is error free or follows good programming practices.  Please be careful 
' whenever you are building or playing with robots and electronics.  If you
' have any comments or questions, please contact support@pololu.com.


'{$stamp BS2} 'Let the editor know this code is for a Basic Stamp 2

'---------***I/O lines
RBUMP		var	IN6	'right front bumper input (0=hit something)
LBUMP		var	IN7	'left front bumper input
MC_RESET	con	8	'Pololu micro dual serial motor controller reset line
MC_SOUT	con	10	'serial line to motor controller

dirs	=	(1<<MC_RESET) | (1<<MC_SOUT)	'set output pins (rest are input pins)


'--------***Variables
SPEED		var	byte	'speed of robot driving straight and of faster wheels when turning
SLOWSPEED	var	byte	'speed of slower wheels when turning backwards
TURNTIME	var	byte	'random time to spin in place after backing away from obstacle


'---------***Motor numbrs/directns for Pololu micro dual serial motor controller commands
LFWD		con	0	 
LBAK		con	1		
RFWD		con	2	
RBAK		con	3




'--------***The Program

	high 	MC_SOUT		'serial line idle state
	low	MC_RESET		'reset motor controller
	high	MC_RESET
	
	SPEED	= 127		'speeds can range from 0 to 127
	SLOWSPEED = 20	
	TURNTIME = 37

twiddling:	'Press a bumper to get the bot moving
	if RBUMP = 0 then go
	if LBUMP = 0 then go
	goto twiddling	

go:	'Wait a second so that bumpers aren't pushed when you first enter loop.
	pause 1000	


loop:	'Go forward till bump something
	serout MC_SOUT, 32, [$80, 0, LFWD, SPEED]	'Left and right motors forward at SPEED
	serout MC_SOUT, 32, [$80, 0, RFWD, SPEED]	'32 indicates 8 bits, no parity, non-inverted, 
								'	buad rate 19200
	if (RBUMP = 0) then rbumped			'If bumped, turn backward in appropo direction
	if (LBUMP = 0) then lbumped
	goto	loop	


rbumped: 'Turn backward right, then spin left in place for a random time
	serout MC_SOUT, 32, [$80, 0, LBAK, SPEED]		'Turn backward for 1 sec
	serout MC_SOUT, 32, [$80, 0, RBAK, SLOWSPEED]
	pause 1000

	serout MC_SOUT, 32, [$80, 0, LBAK, SPEED]	'Spin in place for random time, TURNTIME
	serout MC_SOUT, 32, [$80, 0, RFWD, SPEED]
	random TURNTIME
	pause (TURNTIME*5) + 250			'pause between 0.25 and 1.5 seconds

	goto loop


lbumped: 'Turn backward left, then spin right in place for a random time
	serout MC_SOUT, 32, [$80, 0, LBAK, SLOWSPEED]	
	serout MC_SOUT, 32, [$80, 0, RBAK, SPEED]
	pause 1000

	serout MC_SOUT, 32, [$80, 0, LFWD, SPEED]
	serout MC_SOUT, 32, [$80, 0, RBAK, SPEED]
	random TURNTIME
	pause (TURNTIME*5) + 250

	goto loop