'------------------------------------------------------------------
'test.bs2		-- Aaron Richardson -- Arrick Robotics -- 12/1/98
'			www.robotics.com
'
'This routine tests all functions on the ARobot.  When it powers up,
'it beeps several times to test the beeper.  It then waits for input
'from the user.
'Input:
'wisker - one wisker makes it beep once and turns on the grn led.
'		the other wisker beeps twice and turns on the red led.
'button - one button toggles the leds on and off and sends data out
'		the serial port(counts from 1 to 25).  The other button
'		moves all servos from left to right and back again.
'jumper - one jumper turns on motor one forward.  The red led will
'		go on and off as the motor turns.  The other jumper turns
'		on motor two forward.  If one of the buttons is pressed,
'		the motor reverses itself.


charn	var	byte(4)						'var for chars from coprocessors.
tmp	var	word							'temp var.

grnled	con	11						'green led.
redled	con	10						'red led.
speaker	con	9						'speaker.
net	con	8							'coprocessor network.
baud	con	396							'network baud rat.

low speaker								'turn off speaker.

gosub honk								'test speaker.
gosub honk								'test speaker.
gosub honk								'test speaker.
gosub honk								'test speaker.
gosub honk								'test speaker.
gosub honk								'test speaker.


'------------------------------------------------------------------
'main loop.
'loop here until something happens.
'
main
	if in0=0 then whisker1					'check whiskers.
	if in1=0 then whisker2

	if in12=0 then switch1					'check switches.
	if in13=0 then switch2

	if in14=0 then button1					'check buttons.
	if in15=0 then button2

	goto main							'loop.


'------------------------------------------------------------------
'honk once and turn on the green led.
'
whisker1
	pause 200							'debounce.
	low	grnled						'turn on led.
	gosub honk							'honk.
whisker1a
	if in0=0 then whisker1a					'wait for whisker to be let go.
	high	grnled						'turn off led.
	pause 200							'debounce.
	goto	main							'go back to loop.


'------------------------------------------------------------------
'honk twice and turn on the red led.
'
whisker2
	pause 200							'debounce.
	low	redled						'turn on led.
	gosub honk							'honk.
	gosub honk							'honk.
whisker2a
	if in1=0 then whisker2a					'wait for whisker to be let go.
	high	redled						'turn off led.
	pause 200							'debounce.
	goto	main							'go back to loop.

	
'------------------------------------------------------------------
'turn on motor 1.
'turn on and off red led every 80H encoder counts.
'
switch1
	low	grnled						'turn on led.
	serout net,baud,["!1M114FFFF"]			'turn on motor 1.
	serin net,baud,[charn]
switch1a
	pause 200
	serout net,baud,["!1E1"]				'get encoder reading.
	serin	net,baud,[str charn\4]
	
	if charn(2)="8" then switch1c				'check for encoder counting.
	if charn(2)="0" then switch1d				'check for encoder counting.
switch1b
	if in12=0 then switch1a					'loop if still switched.
	serout net,baud,["!1M110FFFF"]			'turn on motor 1.
	serin net,baud,[charn]
	high grnled							'turn off leds.
	high redled
	goto main							'loop back.

switch1c
	low	redled						'turn on led.
	goto switch1b
switch1d
	high	redled						'turn on led.
	goto switch1b


'------------------------------------------------------------------
'turn on mototr 2.
'if button 1 is pushed, reverse motor 2.
'
switch2
	low redled							'turn on led.
	serout net,baud,["!1M21"]				'turn on motor 2.
	serin net,baud,[charn]
switch2a
	if in14=0 then switch2c					'check button to reverse motor.
switch2b
	if in13=0 then switch2a
	serout net,baud,["!1M20"]				'turn off motor 2.
	serin net,baud,[charn]
	high redled							'turn off led.
	goto main							'goto main loop.

switch2c
	serout net,baud,["!1M22"]				'reverse motor 2.
	serin net,baud,[charn]
	goto switch2b


'------------------------------------------------------------------
'move serovs from left to right and back again.
'
button1
	low grnled							'turn on led.
	tmp=0								'clear tmp var.
	serout net,baud,["!1R101"]				'move all servos.
	serin	net,baud,[charn]
	serout net,baud,["!1R201"]
	serin	net,baud,[charn]
	serout net,baud,["!1R301"]
	serin	net,baud,[charn]
	serout net,baud,["!1R401"]
	serin	net,baud,[charn]
button1a
	tmp=tmp+1							'increment tmp var.
	if tmp=512 then button1b
	if in14=1 then button1d
	goto button1a

button1b
	low grnled							'turn on led.
	tmp=0								'clear tmp var.
	serout net,baud,["!1R1F1"]				'move all servos.
	serin	net,baud,[charn]
	serout net,baud,["!1R2F1"]
	serin	net,baud,[charn]
	serout net,baud,["!1R3F1"]
	serin	net,baud,[charn]
	serout net,baud,["!1R4F1"]
	serin	net,baud,[charn]
button1c
	tmp=tmp+1							'increment tmp var.
	if tmp=512 then button1
	if in14=1 then button1d
	goto button1c

button1d
	serout net,baud,["!1R100"]				'turn off all servos.
	serin	net,baud,[charn]
	serout net,baud,["!1R200"]
	serin	net,baud,[charn]
	serout net,baud,["!1R300"]
	serin	net,baud,[charn]
	serout net,baud,["!1R400"]
	serin	net,baud,[charn]
	high grnled							'turn off led.
	goto main							'loop again.


'------------------------------------------------------------------
'alternatly flash leds.
'send tmp value out the debug port.
'
button2
	tmp=0								'clear tmp.
	high grnled							'set leds.
	low redled
	gosub honk							'beep.	
button2a
	debug ? tmp							'send out current tmp.
	tmp=tmp+1							'increment tmp.
	if tmp=25 then button2b					'check for 25.
	if in15=1 then button2d					'check for button let go.
	goto button2a

button2b
	tmp=0								'clear tmp.
	low grnled							'set leds.
	high redled
	gosub honk							'honk.
button2c
	debug ? tmp							'print debug.
	tmp=tmp+1
	if tmp=25 then button2					'check for 25.
	if in15=1 then button2d					'check for button let go.
	goto button2c

button2d
	high grnled							'turn off leds.
	high redled
	goto main							'loop again.


'------------------------------------------------------------------
'beep beep honk honk.
'
honk
	freqout speaker,200,1500
	low speaker
return