'----------------------------------------------------------------------
'wanderbx.bas		5/22/00	Arrick Robotics
'
'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.  This prgram has been ported to the bx-24.
'This program follows very closely to the original.
'
'------------------------------------------------------------------
'Main Processor Pin Usage
'
'Pin5  left whisker (active low)
'Pin6  right whisker (active low)
'Pin7  
'Pin8  
'Pin9  
'Pin10  
'Pin11  
'Pin12
'Pin13  network I/O
'Pin14  speaker
'P15 red LED
'P16 green LED
'P17 jumper #1
'P18 jumper #2
'P19 button #1 Green
'P20 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
'
dim direction as byte		'Direction. '1'=fwd, '0'=rev.
dim motor as byte			'motor #. '1' - '4'
dim speed as byte			'speed variable. '0' - '6'
dim distance as new unsignedinteger	'motor distance/position variable.
dim d as new unsignedinteger		'distance for travel comparison.
dim char as byte			'get chars from coprocessor.

dim outbuffer(1 To 40) as byte	'define buffers for com port.
dim inbuffer(1 To 40) as byte
'
'------------------------------------------------------------------
'Constant Declarations
'
const net as byte = 13		'coprocessor network pin.
const speaker as byte = 14		'speaker pin.
const redled as byte = 15		'red led pin.
const grnled as byte = 16		'green led pin.
const button1 as byte = 19		'button 1 pin.
const button2 as byte = 20		'button 2 pin.
const whisker1 as byte = 5		'whisker 1 pin.
const whisker2 as byte = 6		'whisker 2 pin.
'
'------------------------------------------------------------------

'**********************************************************
'Start program.
'**********************************************************
'
sub main()
start:
	call reset			'Initialize everything.
start1:
	if getpin(button1)=1 then	'Wait for green button to start program.
		goto start1
	end if

	'Start program.
start2:
	call soundg			'Good sound.
	call sleep(1.0)			'Short delay.
	goto eventloop			'Start responding to events.


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

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

e1:
	'Left whisker?
	if getpin(whisker1)=0 then
		call lwhisker
	end if

e2:
	'Right whisker?
	if getpin(whisker2)=0 then
		call rwhisker
	end if

e3:
	'Stop button?
	if getpin(button2)=0 then
		goto start
	end if

e4:
	'You can put more events here.

	goto e1				'Continue event loop.

end sub


'------------------------------------------------------------------
'Reset - initialize variables, motors, etc.
'
sub reset()
	call sleep(0.25)			'Short break.

							'setup com port.
	call openqueue(outbuffer, 40)		'open buffers for com port.
	call openqueue(inbuffer, 40)
	call definecom3(net,net,bx0000_1000)		'define com port.
	call opencom(3, 2400, inbuffer, outbuffer)	'open com port.

	call putpin(speaker, 0)
	call putpin(redled,1)			'turn off red led.
	call putpin(grnled,1)			'turn off green led.
	distance=1
	direction=1				'motor direction.
	speed=4					'motor speed.	
	call drives				'stop motor.
	call steerl
	call steerr
	call steerc				'center steering.
	call soundg				'good sound.
end sub


'------------------------------------------------------------------
'EVENT - Left whisker.
'
sub lwhisker()
	call drives				'stop drive motor.
	call soundb				'bad sound.
	call steerl				'wheels left.
	distance=25
	direction=0
	call drivew
	if getpin(whisker1)=0 then		'whisker stuck?
		call problem
	end if
	call steerc				'wheels straight.
	distance=65000				'drive for a long distance.
	direction=1
	call drive
end sub


'------------------------------------------------------------------
'EVENT - right whisker.
'
sub rwhisker()
	call drives				'stop drive motor.
	call soundb				'bad sound.
	call steerr				'wheels right.
	distance=25
	direction=0
	call drivew
	if getpin(whisker2)=0 then		'whisker stuck?
		call problem
	end if
	call steerc				'wheels straight.
	distance=65000				'drive for a long distance.
	direction=1
	call drive
end sub


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


'------------------------------------------------------------------
'Problem: trapped, whisker stuck, etc.
'
'Make sounds, wait for green button to continue.
'
sub problem()
dim toggle as boolean

	toggle = true			
	call putpin(redled,0)		'Red led on.
	call putpin(grnled,1)		'Green led off
	call soundb			'Bad sound.
	call soundb
	call soundb
problem1:
	if getpin(button1)=0 then 	'Wait for green button.
		goto problem2
	end if
	call sleep(0.1)			'pause.
	if toggle=true then		'Flash red led.
		call putpin(redled,1)
		toggle=false
	else
		call putpin(redled,0)
		toggle = true
	end if
	goto problem1			'loop until button is pressed.

problem2:
	call putpin(redled, 1)		'turn off red led.

end sub


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


'------------------------------------------------------------------
'Good = two high pitched beeps.
'
sub soundg()
	call freqout(speaker,2000,2000,150)		'2 beeps.
	call freqout(speaker,2000,2000,150)
	call putpin(speaker,0)				'turn off speaker.
end sub


'------------------------------------------------------------------
'Bad = one low, then one lower pitch beeps.
'
sub soundb()
	call freqout(speaker,900,900,400)		'2 beeps.
	call freqout(speaker,600,600,600)
	call putpin(speaker,0)				'turn off speaker.
end sub


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


'------------------------------------------------------------------
'Steer left.
'
sub steerl()
dim trash as string

	call putqueuestr(outbuffer,"!1R1FF")		'RC servo #1, position=FF.
	call getqueue(inbuffer,trash,6)		'get out command.
	call getqueue(inbuffer,char,1)		'get out "A"

	call sleep(0.4)					'wait for servo to turn.

	call putqueuestr(outbuffer,"!1R100")		'RC servo #1, position=OFF.
	call getqueue(inbuffer,trash,6)		'get out command.
	call getqueue(inbuffer,char,1)		'get out "A"

end sub


'------------------------------------------------------------------
'Steer center.
'
sub steerc()
dim trash as string

	call putqueuestr(outbuffer,"!1R180")		'RC servo #1, position=80.
	call getqueue(inbuffer,trash,6)		'get out command.
	call getqueue(inbuffer,char,1)		'get out "A"

	call sleep(0.4)					'wait for servo to turn.

	call putqueuestr(outbuffer,"!1R100")		'RC servo #1, position=OFF.
	call getqueue(inbuffer,trash,6)		'get out command.
	call getqueue(inbuffer,char,1)		'get out "A"

end sub


'------------------------------------------------------------------
'Steer right.
'
sub steerr()
dim trash as string

	call putqueuestr(outbuffer,"!1R101")		'RC servo #1, position=01.
	call getqueue(inbuffer,trash,6)		'get out command.
	call getqueue(inbuffer,char,1)		'get out "A"

	call sleep(0.4)					'wait for servo to turn.

	call putqueuestr(outbuffer,"!1R100")		'RC servo #1, position=OFF.
	call getqueue(inbuffer,trash,6)		'get out command.
	call getqueue(inbuffer,char,1)		'get out "A"

end sub


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


'------------------------------------------------------------------
'Turn right. Distance=amount of turn.  Wait until done.  At current speed.
'
sub driver()
	call steerl				'wheels left.
	direction=0				'reverse.
	call drivew
	call steerr				'wheels right.
	direction=1				'forward.
	call steerc				'steering straight.
end sub


'------------------------------------------------------------------
'Turn left. Distance=amount of turn.  Wait until done.  At current speed.
'
sub drivel()
	call steerr				'wheels right.
	direction=0				'reverse.
	call drivew
	call steerl				'wheels left.
	direction=1				'forward.
	call steerc				'steering straight.
end sub


'------------------------------------------------------------------
'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
'
sub drivew()
dim disstr as string * 4
dim trash as string

	speed = speed or asc("0")		'turn into ascii char.
	direction = direction or asc("0")	'turn into ascii char.
	disstr = inttohex(distance)		'convert distance to string.
	call putqueuestr(outbuffer,"!1M1" & chr(direction) & chr(speed) & disstr)
	call getqueue(inbuffer, trash, 10)	'clear command out of buffer.
	call getqueue(inbuffer, char, 1)	'Get "A" back.
drivew1:						'wait till done moving.
	call sleep(0.05)
	call putqueuestr(outbuffer,"!1E1")	'Ask for encoder count.
	call getqueue(inbuffer, trash, 4)	'clear out command.
	call getqueue(inbuffer, disstr, 4)	'get distance.
	d = cuint(hextoint(disstr))		'convert string to integer.
	if d < distance then 			'compare with distance.
		goto drivew1			'if not reached distance then loop.
	end if
	call sleep(1.5)				'pause.
end sub


'------------------------------------------------------------------
'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
'
sub drive()
dim disstr as string * 4
dim trash as string

	speed = speed or asc("0")		'turn into ascii char.
	direction = direction or asc("0")	'turn into ascii char.
	disstr = inttohex(distance)		'convert distance into string.
	call putqueuestr(outbuffer, "!1M1" & chr(direction) & chr(speed) & disstr)
	call getqueue(inbuffer, trash, 10)	'clear out command.
	call getqueue(inbuffer, char, 1)	'get "A" back.
end sub


'------------------------------------------------------------------
'Stop motor
'
sub drives()
dim trash as string

	call putqueuestr(outbuffer,"!1M1100001")	'send out command.
	call getqueue(inbuffer, trash, 10)		'clear out command.
	call getqueue(inbuffer, char, 1)		'Get "A" back.
	call sleep(0.5)					'delay.
end sub


'******************************************************************
'Hex conversion routines
'******************************************************************


'------------------------------------------------------------------
'4 character ascii Hex string to unsigned integer converter.
'
function hextoint(byval hexstr as string) as long
dim lochar as byte				'local cahracter.
dim cnt as integer				'counter.
dim s as string				'local temp string.

	for cnt = 1 to 4			'4 chars to convert.
		s = mid(hexstr,cnt,1)		'get first char.
		lochar = asc(s)			'convert char to byte.
		lochar = lochar - 48		'subtract "0" offset.
		if lochar > 9 then			'check if char was alpha char.
			lochar = lochar - 7		'subtract alpha offset.
		end if
		hextoint = ((hextoint * 16) + clng(lochar))	'add to integer.
	next

end function


'------------------------------------------------------------------
'unsigned integer to 4 character ascii Hex string converter.
'
function inttohex(byval inint as unsignedinteger) as string
dim lochar as byte			'local char.
dim lolong as long			'local long.
dim cnt as integer			'counter.
dim s as string * 4			'local tem string.

	lolong = clng(inint)			'get input integer.
	for cnt = 4 to 1 step -1		'4 chars to convert.
		lochar = cbyte(lolong mod 16)	'get lolong modulus 16 into lochar.
		lolong = lolong \ 16		'divide lolong by 16.
		if lochar > 9 then			'check for alpha offset needed.
			lochar = lochar + 7		'add alpha offset.
		end if
		lochar = lochar + 48		'add "0" offset.
		mid(s, cnt, 1) = chr(lochar)	'put character into string.
	next
	inttohex = s				'put string into output string.
end function


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