'{$STAMP BS2p}
'{$PBASIC 2.5}
' Robot Pemadam Api
'
ping PIN 7
#SELECT $stamp
#CASE BS2,BS2E
trigger CON 5
<span lang="IT">scale CON $200</span>
<span lang="IT">#CASE BS2SX, BS2P, BS2PX</span>
<span lang="IT">trigger CON 13</span>
<span lang="IT">scale CON $0cd</span>
#ENDSELECT
rawtoin CON 889
rawtocm CON 2257
isHigh CON 1
isLow CON 0
rawDist VAR Word
inches VAR Word
cm VAR Word
<span lang="SV">val01 VAR Byte ' sensor sebelah kanan</span>
<span lang="SV">val02 VAR Byte ' sensor sebelah kiri</span>
i VAR Byte ' count variable for loop
'--------------------------------------------------------
' deklarasi konstanta
cl CON 14 ' pin 14 is output (clock) pada sensor Sharp GP2D02 sebelah kanan
dt CON 15 ' pin 15 is the input (data)
c2 CON 12 ' pin 12 is output (clock) pada sensor Sharp GP2D02 sebelah kiri
dt2 CON 13 'pin 13 is the input
'--------------------------------------------------------
' I/O pin setup for detector
INPUT dt ' make pin 15 the input
INPUT dt2
HIGH cl ' make pin 14 output and high
HIGH c2
wDist VAR Word
wDistkanan VAR Word
wDistkiri VAR Word
<span lang="ES">'sensor srf04 sebelah kanan</span>
<span lang="ES">INITSRFKANAN CON 10</span>
<span lang="ES">ECHOSRFKANAN CON 11</span>
<span lang="IT">'sensor srf04 sebelah kiri</span>
<span lang="IT">INITSRFKIRI CON 6</span>
<span lang="ES">ECHOSRFKIRI CON 8</span>
<span lang="ES"> </span>
<span lang="ES">'sensor srf04 belakang robot</span>
<span lang="ES">INIT CON 4</span>
<span lang="ES">ECHO CON 5</span>
<span lang="ES"> </span>
<span lang="SV">' berjalan 1 arah 1 uS</span>
<span lang="SV">'jarak=(echo time)/ faktor konversi</span>
<span lang="IT">'gunakan 74 inchi (73.74 uS per inchi)</span>
<span lang="SV">'gunakan 29 untuk sentimeter ( 29.033 per 1 cm)</span>
convfac CON 74 ' use inches
counter VAR Byte
main:
GOSUB PingDepan
inches=rawDist ** RawToIn
cm=rawDist ** RawToCm
DEBUG CRSRXY, 15, 3,
DEC rawDist, CLREOL,
CRSRXY, 15,4,
DEC inches, CLREOL,
CRSRXY, 15,5,
DEC cm, CLREOL
PAUSE 100
<span lang="SV"> GOSUB sr_sonar ' baca jarak depan</span>
<span lang="SV"> GOSUB sharp ' baca jarak sisi samping</span>
DEBUG "Sensor SRF04 belakang:"
DEBUG DEC wDist, CR
DEBUG "Sensor SRF04 kanan:"
DEBUG DEC wDistKanan, CR
DEBUG "Sensor SRF04 kiri:"
DEBUG DEC wDistKiri, CR
jika di depan gak ada halangan dan cukup jauh >30cm
IF cm >30 AND val01 <145 AND val02 <145 THEN
'maju terus
FOR counter = 1 TO 30
PULSOUT 0,1000 arah putaran servo continuous Parallax
PULSOUT 1,3500
PAUSE 2
NEXT
ENDIF
'jika ada halangan dan sebelah kiri sempit
IF cm<=30 AND val01<140 AND wdistKiri < 8 AND wdistKanan>8 THEN
'mundur dulu
FOR counter = 1 TO 50
PULSOUT 1,1000
PULSOUT 0,3500
PAUSE 2
NEXT
''belok kanan
FOR counter=1 TO 120
PULSOUT 1,3500
PAUSE 2
NEXT
ELSE
'selain dari itu
'maju dikit
FOR counter = 1 TO 20
PULSOUT 0,1000
PULSOUT 1,3500
PAUSE 2
NEXT
IF cm<=30 AND (wdistKiri < wdistKanan) THEN
'mundur dulu
FOR counter = 1 TO 50
PULSOUT 1,1000
PULSOUT 0,3500
PAUSE 2
NEXT
''belok kanan
FOR counter=1 TO 120
PULSOUT 1,3500
PAUSE 2
NEXT
ELSE
'selain dari itu
'maju dikit
FOR counter = 1 TO 20
PULSOUT 0,1000
PULSOUT 1,3500
PAUSE 2
NEXT
ENDIF
ENDIF
'jika ada halangan dan sebelah kanan sempit
IF cm<=30 AND wDistKiri >8 AND wDistKanan<8 THEN
'mundur dulu
FOR counter = 1 TO 50
PULSOUT 1,1000
PULSOUT 0,3500
PAUSE 10
NEXT
'belok kiri
FOR counter=1 TO 120
PULSOUT 0,1000
PAUSE 10
NEXT
IF cm<=30 AND val02 <140 AND (wdistKiri > wdistKanan) THEN
'mundur dulu
FOR counter = 1 TO 50
PULSOUT 1,1000
PULSOUT 0,3500
PAUSE 2
NEXT
'belok kiri
FOR counter=1 TO 120
PULSOUT 0,1000
PAUSE 10
NEXT
ELSE
'selain dari itu
'maju dikit
FOR counter = 1 TO 20
PULSOUT 0,1000
PULSOUT 1,3500
PAUSE 2
NEXT
ENDIF
ENDIF
<span lang="IT">GOTO main</span>
<span lang="IT">sensor ping di pasang di depan robot</span>
PingDepan:
ping =IsLow
PULSOUT ping, trigger
PULSIN ping, isHigh, rawDist
rawDist=rawDist */Scale
rawDist =rawDist/2
RETURN
inches=rawDist ** RawToIn
cm=rawDist ** RawToCm
DEBUG CRSRXY, 15, 3,
DEC rawDist, CLREOL,
CRSRXY, 15,4,
DEC inches, CLREOL,
CRSRXY, 15,5,
DEC cm, CLREOL
PAUSE 100
sensor SRF04
sr_sonar:
PULSOUT INIT,5 ' 10us init pulse
PULSIN ECHO,1,wDist ' measure echo time
wDist=wDist/convfac ' convert to inches
PULSOUT INITSRFKANAN,5
PULSIN ECHOSRFKANAN, 1, WDistkanan
WdistKanan=wDistKanan/convfac
PULSOUT initSRFKIRI,5
PULSIN ECHOSRFKIRI,1 ,wDistKiri
WDistKiri=wDistKiri/convfac
RETURN
sensor SHARP
sharp:
GOSUB read01 ' call measurement routine
GOSUB read02
DEBUG "Sensor 1:"
DEBUG DEC val01, CR ' display the value
DEBUG "Sensor 2:"
DEBUG DEC val02, CR
PAUSE 10
RETURN
read01:
LOW cl ' turn on detector for reading
rl:
IF IN15 = 0 THEN rl ' wait for input high
<span lang="SV">SHIFTIN dt, cl, MSBPOST, [val01]</span>
<span lang="SV"> HIGH cl ' turn detector off</span>
PAUSE 1 ' let detector reset
RETURN
read02:
LOW c2 ' turn on detector for reading
r2:
IF IN13 = 0 THEN r2 ' wait for input high
<span lang="SV">SHIFTIN dt2, c2, MSBPOST, [val02]</span>
<span lang="SV"> HIGH c2 ' turn detector off</span>
PAUSE 1 ' let detector reset
RETURN
Tidak ada komentar:
Posting Komentar