Page 136 - The Definitive Guide to Building Java Robots

Preston_5564C04.fm Page 117 Wednesday, October 5, 2005 7:22 AM

CHAPTER 4 ■ SENSORS 117

main:
cmd = 0
SERIN 16,16468,main,[WAIT(100), cmd]
IF cmd = 101 THEN get_dinsmore
IF cmd = 102 THEN get_devantech
IF cmd = 103 THEN get_vector
PAUSE 5
GOTO main

get_devantech:
PULSIN cin, 1, headingC ' Get reading
headingC = (headingC-500)/50 ' BS2(e) - Calculate Bearing in degrees
SEROUT 16,N9600,[DEC headingC ' out to PC
GOTO main

get_vector:
LOW sselect 'start the Vector 2x.
PAUSE 200 'wait for heading.
headingV = 0 'clear variable.

'get the data from Vector 2x.
SHIFTIN sdo,sclk,MSBPRE,[headingV\16]
HIGH sclk 'reset pins.
HIGH sselect

' out to PC
SEROUT 16,16468,[DEC headingV]
GOTO main

get_dinsmore:
' north
IF north = 1 AND east = 0 AND west = 0 THEN
headingD = 0
ENDIF

' east
IF east = 1 AND north = 0 AND south = 0 THEN
headingD = 90
ENDIF

' south
IF south = 1 AND east = 0 AND west = 0 THEN
headingD = 180
ENDIF
' west
IF west = 1 AND north = 0 AND south = 0 THEN
headingD = 270
ENDIF