' fritz.bs2 - by David Sharp, August 2001

sonar_servo_pin	con	4
sonar_init_pin	con	5
sonar_echo_pin	con	6
sonar_scan_side	var	bit	' 0 = left, 1 = right
sonar_depth		var	word
sonar_map		var	byte(3)
distance_factor	con	74	' 74 for inches

servo_pulses	con	11	' was 15
pause_time		con	50
servo_center	con	800
servo_distance	con	250
servo_left		con	servo_center - servo_distance
servo_right		con	servo_center + servo_distance
i			var	byte
look_destination	var	word

safe_distance	con	10	' robot won't approach anything closer
too_close		con	6	' ack, danger!!
long_distance	con	20	' no biggie

touch_right		con	7	' collision sensor on right side
touch_left		con	8	' collision sensor on left side

pause 2000
look_destination = servo_center
gosub look
pause 1000
look_destination = servo_right
gosub look

start
	gosub scan_sonar

	gosub avoid_collisions
	gosub mv_forward
	pause pause_time

	debug cls, ? sonar_map(0)
	debug ? sonar_map(1)
	debug ? sonar_map(2)
goto start

scan_sonar:
	gosub sonar_get_depth
	sonar_map(sonar_scan_side) = sonar_depth

	look_destination = servo_center
	gosub look
	gosub sonar_get_depth
	sonar_map(1) = sonar_depth

	look_destination = servo_left + (sonar_scan_side * 
(servo_distance * 2))
	gosub look
	gosub sonar_get_depth
	sonar_map(sonar_scan_side * 2) = sonar_depth
	sonar_scan_side = sonar_scan_side + 1
return

look:
	for i = 0 to servo_pulses
		pulsout sonar_servo_pin, look_destination
		pause 10
	next
return

sonar_get_depth:
	pulsout sonar_init_pin, 5
	output sonar_init_pin ' dummy op
	rctime sonar_echo_pin, 1, sonar_depth
	'pulsin sonar_echo_pin, 1, sonar_depth
	sonar_depth = sonar_depth / distance_factor
	pause 10
return

mv_forward:
'	debug "move forward", cr
	high 0
	low 1
	high 2
	low 3
return

mv_reverse:
'	debug "move reverse", cr
	low 0
	high 1
	low 2
	high 3
return


turn_left_fast:
	low 0
	high 1
	high 2
	low 3
return


turn_right_fast:
	high 0
	low 1
	low 2
	high 3
return


turn_left:
	low 0
	low 1
	high 2
	low 3
return


turn_right:
	high 0
	low 1
	low 2
	low 3
return


mv_stop:
	low 0
	low 1
	low 2
	low 3
return


avoid_collisions:
'	if touch_left = 1 then collide_left
'	if touch_right = 1 then collide_right
	if in7 = 1 then collide_right
	if in8 = 1 then collide_left

	gosub scan_sonar

	if sonar_map(1) < too_close then escape
	if sonar_map(0) < too_close then escape
	if sonar_map(2) < too_close then escape

	if sonar_map(1) < safe_distance then barrier
	if sonar_map(0) < safe_distance then barrier
	if sonar_map(2) < safe_distance then barrier

	if sonar_map(1) < long_distance then coast_turn
	if sonar_map(0) < long_distance then coast_turn
	if sonar_map(2) < long_distance then coast_turn

	return
	collide_left:
		if touch_right = 1 then collide_both
		gosub mv_stop
		pause pause_time
		gosub mv_reverse
		pause pause_time * 5
		gosub turn_right_fast
		pause pause_time * 5
		gosub mv_stop
	goto avoid_collisions
	collide_right:
		if touch_left = 1 then collide_both
		gosub mv_stop
		pause pause_time
		gosub mv_reverse
		pause pause_time * 10
		gosub turn_left_fast
		pause pause_time * 10
		gosub mv_stop
	goto avoid_collisions
	collide_both:
		gosub mv_stop
		pause pause_time
		gosub mv_reverse
		pause pause_time * 10
	goto avoid_collisions
	barrier:
		gosub mv_stop
		pause pause_time
''		gosub scan_sonar
		if sonar_map(0) < sonar_map(2) then avoid_left
		'else avoid right:
			debug "turn left!", cr
			gosub turn_left_fast
			pause pause_time * 4
			gosub mv_stop
			goto avoid_collisions
		avoid_left:
			debug "turn right!", cr
			gosub turn_right_fast
			pause pause_time * 4
			gosub mv_stop
		goto avoid_collisions
	'done
	escape:
		gosub mv_stop
		pause pause_time * 3
		gosub mv_reverse
		pause pause_time * 3
		gosub mv_stop
''		gosub scan_sonar
	goto avoid_collisions
	coast_turn:
''		gosub scan_sonar
		if sonar_map(0) < sonar_map(2) then coast_left
		'else coast right
			gosub turn_left
			pause pause_time * 3
			gosub mv_forward
			goto avoid_collisions
		coast_left:
			gosub turn_right
			pause pause_time * 3
			gosub mv_forward
		goto avoid_collisions
	'done
'return