Page 253 - The Definitive Guide to Building Java Robots

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

234 CHAPTER 7 ■ NAVIGATION

IF cmd = 103 THEN ping_ir ' pings the sonar (READ-ms) ➥
- return after a time
IF cmd = 104 THEN ping_all ' pings the sonar (READ-ms) ➥
- return after a time
IF cmd = 105 THEN get_lat ' gets gps latitude
IF cmd = 106 THEN get_lon ' gets gps longitude
IF cmd = 107 THEN get_diag ' gets diagnostic
PAUSE 5
GOTO main

get_compass:
PULSIN cin, 1, heading ' Get reading
heading = (heading-500)/50 ' BS2(e) ➥
- Calculate Bearing in degrees
SEROUT 16,N9600,[DEC heading] ' out to PC
GOTO main
ping_sonar:
GOSUB sonar1
GOSUB sonar2
GOSUB sonar3
' output is s1~s2~s3
SEROUT 16,N9600,[wDist1,wDist2,wDist3]
GOTO main
ping_ir:
GOSUB ir1
GOSUB ir2
' output is ir1~ir2
SEROUT 16,N9600,[dist1,dist2]
GOTO main
ping_all:
GOSUB ir1
GOSUB ir2
GOSUB sonar1
GOSUB sonar2
GOSUB sonar3
' output is ir1~ir2~s1~s2~s3
SEROUT 16,N9600,[dist1,dist2,wDist1,wDist2,wDist3]
GOTO main
ir1:
LOW dout1
ir1b:
IF IN3=0 THEN ir1b
SHIFTIN din1,dout1,2,[dist1\8]
HIGH dout1
RETURN
ir2:
LOW dout2

97022d2480fe4a63cfdfa123a6e70098