'
' Test Rover Program
' (c)2004 Eric Lundquist
' Free to use for non-commercial purposes. 
'
' Basic collision avoidance utilizing an IRPD infrared sensor.
' Controller is RC40.  Robot is a CR01.  All by CompuBotics.
'
'Pin assignments
' 0 = IRPD Left Sensor (PortB.0)/0=detect, 1=nothing detected
' 1 = Right servo (PortB.1)/Continuous Rotation Servo pulsed from 1.0ms-2.0ms
' 2 = Left Servo (PortB.2)/Continuous Rotation servo pulsed from 1.0ms-2.0ms
' 3 = IRPD Right Sensor (PortB.3)/0=detect, 1=nothing detected
' 4 = IRPD Range Select (PortB.4)/0=long range, 1=short range
'
' HISTORY:
' June 6, 2004 - Initial release - E.Lundquist
'
#include "rc40.inc"

' Constant Declarations
FOREVER		CON	1	'Loop Control

MS1			CON	500		'number of 2us ticks in 1ms
MS15		CON 750		'number of 2us ticks in 1.5ms
MS2			CON	1000	'number of 2us ticks in 2ms
LEFT_IR		CON	0		'pin number of left irpd sensor
SERVO1		CON	1		'Pin number of Servo 1
SERVO2		CON	2		'Pin number of Servo 2
RIGHT_IR	CON	3		'pin number of right irpd sensor
RANGE		CON	4		'pin number of ir range select 0=long 1=short)
SPEED_INCREMENT CON ((MS2 - MS1) / 20)	'ticks per speed unit

SERVO_SETTLE_TIME	CON	20	'ms required between servo pulses

' Global Variable Declarations
left_speed	VAR	WORD	'Current Speed of Left Servo (-10 to +10)
right_speed	VAR	WORD	'Current Speed of Right Servo (-10 to +10)
tmp			VAR	WORD	'Temp Holding Value
tmp1 		VAR WORD	'more temp space
rpulse		VAR WORD	'computed pulse length in 2us units
lpulse		VAR WORD
rndseed		VAR WORD	'Random number seed

' Main Program Start
Main:
	debug "Test Rover v1.1", CR, "(c)2004 Eric Lundquist", CR
	OUTPUT SERVO1	'Right Servo Enable
	OUTPUT SERVO2	'Left Servo Enable
	OUTPUT RANGE	' 0=short range scan
	HIGH RANGE		'set to short range scan
	INPUT LEFT_IR	'Left IR sen
	INPUT RIGHT_IR	'Right IR Sensor
	
	' Initialize Current Speed
	left_speed = 0
	right_speed = 0
	rndseed = TMR0		'set initial random number seed
	
	while (FOREVER)
		'Get bumpers into lower 2 bits
		tmp = PORTB.bit0 + (PORTB.bit3 << 1)
		'0 = both left and right detected
		'1 = right detected
		'2 = left detected
		'3 = all clear. full speed ahead!
		branch tmp, [FRONTBUMP, RIGHTBUMP, LEFTBUMP, NOBUMP]
		debug "weird bump val:", hex tmp, CR
		goto ENDLOOP
NOBUMP:
		'debug "."
		'Full speed ahead!
		gosub INCLEFT
		gosub INCRIGHT
		goto ENDLOOP
RIGHTBUMP:
		'debug "R"
		'Turn left
		gosub DECLEFT
		gosub INCRIGHT
		goto ENDLOOP
LEFTBUMP:
		'debug "L"
		'Turn right
		gosub DECRIGHT
		gosub INCLEFT
		goto ENDLOOP
FRONTBUMP:
		'debug "F"
		'Back up
		gosub DECRIGHT
		gosub DECLEFT
		gosub PULSEMOTORS
		'Skew left or right randomly to get out of this
		RANDOM rndseed
		tmp = rndseed // 3
		branch tmp, [RIGHTBUMP, LEFTBUMP, FRONTBUMP]
ENDLOOP:
		gosub PULSEMOTORS
	wend
'
' Increase Speed of left motor
INCLEFT:
	left_speed = left_speed + 1
	'Check if negative
	if left_speed > $8000 then return
	'Make sure we don't go faster than the max
	if left_speed > 10 then 
		left_speed = 10
	endif
	return
'
' Increase speed of right motor
INCRIGHT:
	right_speed = right_speed + 1
	'Check if negative
	if right_speed > $8000 then return
	'Make sure we dont go faster than max
	if right_speed > 10 then 
		right_speed = 10
	endif
	return
'
' Slow down left motor
DECLEFT:
	left_speed = left_speed - 1
	if ABS(left_speed) > 10 then
		left_speed = 0-10
	endif
	return
'
' Slow down right motor
DECRIGHT:
	right_speed = right_speed - 1
	if ABS(right_speed) > 10 then 
		right_speed = 0-10
	endif
	return
'
' Drive the motors according to speed values
PULSEMOTORS:
	tmp1 = 0-right_speed	'invert right side so that +values mean forward
	'Check for negative speed
	'Compute number of 2us ticks
	if tmp1 > $8000 then
		rpulse = MS15 - ((0-tmp1) * SPEED_INCREMENT)
	else
		rpulse = MS15 + (tmp1 * SPEED_INCREMENT)
	endif
	pulsout SERVO1, rpulse
	pause SERVO_SETTLE_TIME
	
	'Check for negative(reverse) speed
	'Compute number of 2us ticks
	if left_speed > $8000 then
		lpulse = MS15 - ((0-left_speed) * SPEED_INCREMENT)
	else
		lpulse = MS15 + (left_speed * SPEED_INCREMENT)
	endif
	pulsout SERVO2, lpulse	
	pause SERVO_SETTLE_TIME
	return
'