Page 151 - The Definitive Guide to Building Java Robots

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

132 CHAPTER 4 ■ SENSORS

Example 4-6. distance.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}
' {$PORT COM1}

' cmd variable
cmd VAR Byte
N9600 CON 16468

' IR
dout1 CON 2 'output to the DIRRS. (green)
din1 CON 3 'input from the DIRRS. (yellow)
dist1 VAR Byte 'ir dist 1

' both sonar
convfac CON 74 '74 inches, 29 cm

' srf04
ECHO1 CON 9 'input from the SRF04.
INIT1 CON 8 'output to the SRF04.
wDist1 VAR Word 'sonar1

' 6500
ECHO2 CON 11 'input from the 6500.
INIT2 CON 10 'output to the 6500.
wDist2 VAR Word 'sonar2

main:
cmd = 0
SERIN 16,16468,main,[WAIT(100), cmd]
IF cmd = 101 THEN ping_ir ' gets compass reading
IF cmd = 102 THEN ping_srf ' pings the SRF04
IF cmd = 103 THEN ping_6500 ' pings the 6500 Sonar Module
PAUSE 5
GOTO main

ping_ir:
LOW dout1
ir1b:
IF IN3=0 THEN ir1b
SHIFTIN din1,dout1,2,[dist1\8]
HIGH dout1
SEROUT 16,N9600,[DEC dist1,CR]
GOTO main