'------------------------------------------------------------------
'wander.bs2   3/28/99  Arrick Robotics  www.robotics.com
'
'This program is a simple object detection/avoidance routine.
'It causes ARobot to drive forward until a front whisker is
'activated.  It will then reverse and turn away from the object
'then continue forward.
'
'------------------------------------------------------------------
'Main Processor Pin Usage
'
'P0  left whisker (active low)
'P1  right whisker (active low)
'P2  
'P3  
'P4  
'P5  
'P6  
'P7
'P8  network I/O
'P9  speaker
'P10 red LED
'P11 green LED
'P12 jumper #1
'P13 jumper #2
'P14 button #1 Green
'P15 button #2 Red
'
'------------------------------------------------------------------
'Controller Board I/O Usage.
'
'RC servo 1 = steering
'RC servo 2 = unused
'RC servo 3 = unused
'RC servo 4 = unused
'DC motor 1 = main drive motor
'DC motor 2 = unused
'
'------------------------------------------------------------------
'Variable Declarations
'
i var byte				'counter.
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.
char1 var byte			'get chars from coprocessor.
char2 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.
redled con 10			'red led pin.
grnled con 11			'green led pin.
'
'------------------------------------------------------------------


'******************************************************************
'Start program.
'******************************************************************
'
start:
	gosub reset			'Initialize everything.
start1:
	if in14=0 then start2	'Wait for green button to start program.
	goto start1

	'Start program.
start2:
	gosub soundg		'Good sound.
	pause 1000			'Short delay.
	goto eventloop		'Start responding to events.


'------------------------------------------------------------------
'Reset - initialize variables, motors, etc.
'
reset:
	pause 250				'Short break.
	low speaker				'Speaker off.
	high redled				'turn off red led.
	high grnled				'turn off green led.
	distance=1
	direction=1				'Drive direction.
	speed=4				'Drive motor speed.
	gosub drives			'stop drive motor.
	gosub steerl
	gosub steerr
	gosub steerc			'steering center.
	gosub soundg			'good sound.
	return


'------------------------------------------------------------------
'Event loop - Checks whiskers and responds.
'
eventloop:

	distance=65000			'Drive for a long time.
	direction=1
	gosub drive				

e1:
	'Left whisker?
	if in0=1 then e2
	gosub lwhisker

e2:
	'Right whisker?
	if in1=1 then e3
	gosub rwhisker

e3:
	'Stop button?
	if in15=1 then e4
	goto start

e4:
	'You can put more events here.

	goto e1				'Continue event loop.


'------------------------------------------------------------------
'EVENT - Left whisker.
'
lwhisker:
	gosub drives			'stop drive motor.
	gosub soundb			'Bad sound.
	gosub steerl			'Wheels left.
	distance=25
	direction=0				'Reverse.
	gosub drivew
	if in0=1 then lwhisker1		'Whisker stuck?
	gosub problem
lwhisker1:
	gosub steerc			'Wheels straight.
	distance=65000			'Drive for a long time.
	direction=1
	gosub drive
	return


'------------------------------------------------------------------
'EVENT - right whisker.
'
rwhisker:

	gosub drives			'stop drive motor.
	gosub soundb			'Bad sound.
	gosub steerr			'Wheels right.
	distance=25
	direction=0				'Reverse.
	gosub drivew
	if in1=1 then rwhisker1		'Whisker stuck?
	gosub problem
rwhisker1:
	gosub steerc			'Wheels straight.
	distance=65000			'Drive for a long time.
	direction=1
	gosub drive
	return


'******************************************************************
'Utilities
'******************************************************************


'------------------------------------------------------------------
'Problem: trapped, whisker stuck, etc.
'
'Make sounds, wait for green button to continue.
'
problem:
	low redled				'Red led on.
	high grnled				'Green led off
	gosub soundb			'Bad sound.
	gosub soundb
	gosub soundb
problem1:
	if in14=1 then problem2		'Wait for green button
	return
problem2:
	pause 100
	toggle redled			'Flash red led
	goto problem1


'******************************************************************
'Sounds
'******************************************************************


'------------------------------------------------------------------
'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


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


'******************************************************************
'Steering Subroutines
'******************************************************************


'------------------------------------------------------------------
'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.


'******************************************************************
'Drive Motor Subroutines
'******************************************************************


'------------------------------------------------------------------
'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


'------------------------------------------------------------------
'  End of Program
'------------------------------------------------------------------