'{$STAMP BS2}
'------------------------------------------------------------------
'straight.bs2   3/29/99 Arrick Robotics www.robotics.com
'
'This program helps you adjust ARobot's steering linkage for
'straight operation.
'Download and run this program on your ARobot.  
'Press one of the push buttons (not reset).
'ARobot will move forward about 10 feet then return.
'Adjust the steering arms on ARobot and repeat this
'proceedure until the robot moves straight.
'------------------------------------------------------------------

charn var byte					'Get chars from coprocessor.
net con 8						'Coprocessor network pin.
baud con 396					'Coprocessor network baud rate.
speaker con 9					'Speaker pin.
d1 var word						'Distance.
d2 var word						'Distance.

main:
	gosub beep					'Beep.
	d1=0						'Initialize variables.
	d2=0

main1:
	if in14=0 then main2			'If button then start.
	if in15=0 then main2
	goto main1

main2:
	'Steering RC servo motor straight.
	serout net,baud,["!1R180"]
	serin net,baud,[charn]

	'Move robot forward.
	serout net,baud,["!1M11600F0"]	'Forward,speed=3,distance 10'.
	serin net,baud,[charn]			'Get acknowledge char.
	gosub stopped				'Wait until motor stopped.

	'Wait.
	pause 1000

	'Move robot back.
	serout net,baud,["!1M10600F0"]	'Reverse,speed=3,distance 10'.
	serin net,baud,[charn]			'Get acknowledge char.
	gosub stopped				'Wait until motor stopped.

	'Restart program.
	goto main


'------------------------------------------------------------------
'Wait until motor stops.  Returns when two count readings are the same.
'
stopped:
	pause 500					'Delay between reads.
	serout net,baud,["!1E1"]		'Get encoder count.
	serin net,baud,[hex4 d2]
	if d1 = d2 then stopped1		'If same then stopped.
	d1=d2
	goto stopped				'Loop.
stopped1:
	d1=0
	d2=0
	return


'------------------------------------------------------------------
'Beep.
'
beep:
	freqout speaker,150,2000
	pause 10
	freqout speaker,150,2000
	low speaker
	return


'------------------------------------------------------------------
'End of program
'------------------------------------------------------------------