'------------------------------------------------------------------
'Useful Utility Routines
'
'utility.bs2    Roger Arrick    www.robotics.com
'
'Date: 3/11/99
'Edit: 1
'
'
'------------------------------------------------------------------
'Variable Declarations
'
direction var byte		'Direction. '1'=fwd, '0'=rev.
motor var byte			'motor #. '1' - '4'
speed var byte			'speed variable. '0' - '6'
distance var word			'motor distance/position variable.
d var word				'distance for travel comparison.
char var byte			'get chars from coprocessor.
'
'------------------------------------------------------------------
'Constant Declarations
'
net con 8				'coprocessor network pin.
baud con 396			'coprocessor network baud rate.
speaker con 9			'speaker pin.
'
'------------------------------------------------------------------



'------------------------------------------------------------------
'Sound Good = two high pitched beeps.
'
soundg:
	freqout speaker,150,2000
	pause 10
sound1:
	'call here for only one beep.
	freqout speaker,150,2000
	low speaker
	return


'------------------------------------------------------------------
'Sound Bad = one low, then one lower pitch beeps.
'
soundb:
	freqout speaker,400,900
	freqout speaker,600,600
	low speaker
	return


'------------------------------------------------------------------
'Steer left.
'
steerl:
	serout net,baud,["!1R1FF"]	'RC servo #1, position=FF.
	serin net,baud,[char]		'Get "A" back.

steerf:
	pause 400				'Wait for servo to turn

	'Steering motor off.
	serout net,baud,["!1R100"]	'RC Servo #1, position=00 (off).
	serin net,baud,[char]		'Get "A".
	return


'------------------------------------------------------------------
'Steer center.
'
steerc:
	serout net,baud,["!1R180"]	'RC servo #1, position=80.
	serin net,baud,[char]		'Get "A" back.
	goto steerf				'Finish up.


'------------------------------------------------------------------
'Steer right.
'
steerr:
	serout net,baud,["!1R101"]	'RC servo #1, position=01.
	serin net,baud,[char]		'Get "A" back.
	goto steerf				'Finish up.


'------------------------------------------------------------------
'Turn 15 degrees right.  Wait until done.  At current speed.
'
drive15r:
	distance=4				'Amount of turn.
	goto driver				'Turn right.


'------------------------------------------------------------------
'Turn 15 degrees left.  Wait until done.  At current speed.
'
drive15l:
	distance=4				'Amount of turn.
	goto drivel				'Turn left.


'------------------------------------------------------------------
'Turn 45 degrees right.  Wait until done.  At current speed.
'
drive45r:
	distance=13				'Amount of turn.
	goto driver				'Turn right.


'------------------------------------------------------------------
'Turn 45 degrees left.  Wait until done.  At current speed.
'
drive45l:
	distance=13				'Amount of turn.
	goto drivel				'Turn left.


'------------------------------------------------------------------
'Turn 90 degrees right.  Wait until done.  At current speed.
'
drive90r:
	distance=27				'Amount of turn.
	goto driver				'Turn right.


'------------------------------------------------------------------
'Turn 90 degrees left.  Wait until done.  At current speed.
'
drive90l:
	distance=27				'Amount of turn.
	goto drivel				'Turn left.


'------------------------------------------------------------------
'Turn 180 degrees right.  Wait until done.  At current speed.
'
drive180r:
	gosub drive90r
	gosub drive90r
	return


'------------------------------------------------------------------
'Turn right. Distance=amount of turn.  Wait until done.  At current speed.
'
driver:
	gosub steerl			'Wheels left.
	direction=0				'Reverse.
	gosub drivew
	gosub steerr			'Wheels right.
	direction=1				'Forward.
	gosub drivew
	gosub steerc			'Steering straight.
	return


'------------------------------------------------------------------
'Turn left. Distance=amount of turn.  Wait until done.  At current speed.
'
drivel:
	gosub steerr			'Wheels left.
	direction=0				'Reverse.
	gosub drivew
	gosub steerl			'Wheels right.
	direction=1				'Forward.
	gosub drivew
	gosub steerc			'Steering straight.
	return


'------------------------------------------------------------------
'Control the drive motor and wait until distance is traveled.
'direction '1'=forward or '0'=backwards
'speed = '0','1','2','3','4','5','6'  (0=off,6=full speed)
'distance = '0000' to 'FFFF' distance to travel
'
drivew:
	speed = speed | "0"
	direction = direction | "0"
	serout net,baud,["!1M1", direction, speed, hex4 distance]
	serin net,baud,[char]			'Get "A" back.
drivew1:
	'wait till done.
	pause 50
	serout net,baud,["!1E1"]		'Ask for encoder count.
	serin net,baud,[hex4 d]			'Get 4 hex bytes.
	if d < distance then drivew1		'Loop until there.
	pause 500					'Delay to prevent crash.
	return


'------------------------------------------------------------------
'Control the drive motor without waiting until distance it traveled.
'Direction '1'(forward) or '0'(backwards)
'speed '0','1','2','3','4','5','6'  (0=off,6=full speed)
'distance '0000' to 'FFFF' distance to travel
'
drive:
	speed = speed | "0"
	direction = direction | "0"
	serout net,baud,["!1M1", direction, speed, hex4 distance]
	serin net,baud,[char]			'Get "A" back.
	return


'------------------------------------------------------------------
'Stop motor
'
drives:
	serout net,baud,["!1M1100001"]	'Need a 1 in distance here.
	serin net,baud,[char]			'Get "A" back.
	pause 500					'Delay.
	return


'------------------------------------------------------------------
'Control RC servo motors.
'mtr (Motor #) '1', '2', '3', '4'.   '1'=steering motor.
'dis (distance/position)  = '00' to 'FF', where '00' = relax
'
rcservo:
	serout net,baud,["!1R",motor,hex2 distance]  'Send command to co.
	serin net,baud,[char]			'Get "A".
	return



'------------------------------------------------------------------
'  End of Utilities
'------------------------------------------------------------------