Page 252 - The Definitive Guide to Building Java Robots

Preston_5564C07.fm Page 233 Monday, September 26, 2005 5:38 AM

CHAPTER 7 ■ NAVIGATION 233

Example 7-7. nav1.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}
' {$PORT COM1}
' cmd variable
cmd VAR Byte
N9600 CON 16468
' GPD02 IR
dout1 CON 2 'output to the DIRRS. (green)
din1 CON 3 'input from the DIRRS. (yellow)
dout2 CON 4 'output to the DIRRS. (green)
din2 CON 5 'input from the DIRRS. (yellow)
dout3 CON 6 'output to the DIRRS. (green)
din3 CON 7 'input from the DIRRS. (yellow)
dist1 VAR Byte 'ir dist 1
dist2 VAR Byte 'ir dist 2
dist3 VAR Byte 'ir dist 3
' CMPS03 COMPASS
cin CON 14 'serial data out GREEN (a)
heading VAR Word 'heading

' srf04 sonar
convfac CON 74 '74 inches, 29 cm
ECHO1 CON 9 'input from the SRF04. (red)
INIT1 CON 8 'output to the SRF04. (gry)
ECHO2 CON 11 'input from the SRF04. (yel)
INIT2 CON 10 'output to the SRF04. (grn)
ECHO3 CON 13 'input from the SRF04. (blu)
INIT3 CON 12 'output to the SRF04. (pur)

wDist1 VAR Word 'sonar1
wDist2 VAR Word 'sonar2
wDist3 VAR Word 'sonar3
status VAR Byte
gpsData1 VAR Byte(5)
gpsData2 VAR Byte(4)

N4800 CON 16572 'GPS baudrate (4800)

main:
cmd = 0
SERIN 16,16468,main,[WAIT(100), cmd]
IF cmd = 101 THEN get_compass ' gets compass reading (READ-ms) ➥
- return after a time
IF cmd = 102 THEN ping_sonar ' pings the sonar (READ-ms) ➥
- return after a time