- Κώδικας:
' {$STAMP BS2}
' {$PBASIC 2.5}
' -----[ I/O Definitions ]-------------------------------------------------
Piezo PIN 4
RightServo PIN 12
LeftServo PIN 13
PingServo PIN 14
Ping PIN 15
' -----[ Variables ]-------------------------------------------------------
irDetectLeft VAR Bit
irDetectRight VAR Bit
pulseCount VAR Byte
distance VAR Word
oldDistance VAR Word
counter VAR Word
task VAR Nib
' -----[ Initialization ]--------------------------------------------------
FREQOUT Piezo, 2000, 3000
' -----[ Program Code ]----------------------------------------------------
Main:
DO
FREQOUT 8, 1, 38500
irDetectLeft = IN9
FREQOUT 2, 1, 38500
irDetectRight = IN0
IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
GOSUB Ping_Around
ELSEIF (irDetectLeft = 0) THEN
GOSUB Ping_Around
ELSEIF (irDetectRight = 0) THEN
GOSUB Ping_Around
ENDIF
counter = counter + 1
IF counter > 10 THEN
GOSUB Ping_Out
ENDIF
IF (distance > 30) THEN
GOSUB Forward_Pulse
ELSE
GOSUB Ping_Around
ENDIF
LOOP
' -----[ Subroutines ]-----------------------------------------------------
Forward_Pulse:
PULSOUT PingServo, 750
PULSOUT LeftServo, 850
PULSOUT RightServo, 650
PAUSE 20
RETURN
Turn_Left:
FOR pulseCount = 0 TO 7
PULSOUT PingServo, 750
PULSOUT LeftServo, 650
PULSOUT RightServo, 650
PAUSE 20
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 7
PULSOUT PingServo, 750
PULSOUT LeftServo, 850
PULSOUT RightServo, 850
PAUSE 20
NEXT
RETURN
Back_Up:
FOR pulseCount = 0 TO 40
PULSOUT PingServo, 750
PULSOUT LeftServo, 650
PULSOUT RightServo, 850
PAUSE 20
NEXT
RETURN
Ping_Out:
counter = 0
LOW Ping
PULSOUT Ping, 5
PULSIN Ping, 1, distance
distance = distance ** 2257
RETURN
Ping_Around:
counter = 0
oldDistance = 30
task = 0
FOR pulseCount = 0 TO 20
LOW Ping
PULSOUT PingServo, 1085
PULSOUT Ping, 5
PULSIN Ping, 1, distance
PAUSE 20
NEXT
distance = distance ** 2257
IF distance > oldDistance THEN
oldDistance = distance
task = 1
ENDIF
FOR pulseCount = 0 TO 20
LOW Ping
PULSOUT PingServo, 850
PULSOUT Ping, 5
PULSIN Ping, 1, distance
PAUSE 20
NEXT
distance = distance ** 2257
IF distance > oldDistance THEN
oldDistance = distance
task = 2
ENDIF
FOR pulseCount = 0 TO 20
LOW Ping
PULSOUT PingServo, 400
PULSOUT Ping, 5
PULSIN Ping, 1, distance
PAUSE 20
NEXT
distance = distance ** 2257
IF distance > oldDistance THEN
oldDistance = distance
task = 3
ENDIF
FOR pulseCount = 0 TO 20
LOW Ping
PULSOUT PingServo, 225
PULSOUT Ping, 5
PULSIN Ping, 1, distance
PAUSE 20
NEXT
distance = distance ** 2257
IF distance > oldDistance THEN
oldDistance = distance
task = 4
ENDIF
ON task GOSUB Task0, Task1, Task2, Task3, Task4
distance = 50
RETURN
Task0:
GOSUB Turn_Right
GOSUB Turn_Right
GOSUB Turn_Right
GOSUB Turn_Right
RETURN
Task1:
GOSUB Turn_Left
GOSUB Turn_Left
RETURN
Task2:
GOSUB Turn_Left
RETURN
Task3:
GOSUB Turn_Right
RETURN
Task4:
GOSUB Turn_Right
GOSUB Turn_Right
RETURN
2 απαντήσεις
ΠΡΟΓΡΑΜΜΑ ΠΛΟΗΓΗΣΗΣ ΡΟΜΠΟΤ BS2 ΜΕ PING))) ULTRASONIC SENSOR
dimbot- Master
- Αριθμός μηνυμάτων : 344
Ημερομηνία εγγραφής : 22/12/2009
- Δημοσίευση Ν°1
ΠΡΟΓΡΑΜΜΑ ΠΛΟΗΓΗΣΗΣ ΡΟΜΠΟΤ BS2 ΜΕ PING))) ULTRASONIC SENSOR
spilios- Αριθμός μηνυμάτων : 20
Ημερομηνία εγγραφής : 24/09/2011
- Δημοσίευση Ν°2
Βοηθεια
Καλησπέρα φίλε, ειμαι σχετικά νέος στο σπορ, έχω το BS2p, και θα ήθελα αν δεν σου είναι κόπος, να μου πεις πως θα συνδέσω τον sensor με τον BS2p, και πως θα φτιαξω ενα πρόγραμμα, το οποίο θα μου επιστρέφει στην οθονη (πχ με debug) την απόσταση που έχει καθε φορά. Ευχαριστώ
spilios- Αριθμός μηνυμάτων : 20
Ημερομηνία εγγραφής : 24/09/2011
Συνγώμη, ο sensor που έχω έχει 5 επαφές.
dimbot- Master
- Αριθμός μηνυμάτων : 344
Ημερομηνία εγγραφής : 22/12/2009
Καλώς ήρθες στο robotichellas και καλή αρχή στο άθλημα!spilios έγραψε:Καλησπέρα φίλε, ειμαι σχετικά νέος στο σπορ, έχω το BS2p, και θα ήθελα αν δεν σου είναι κόπος, να μου πεις πως θα συνδέσω τον sensor με τον BS2p, και πως θα φτιαξω ενα πρόγραμμα, το οποίο θα μου επιστρέφει στην οθονη (πχ με debug) την απόσταση που έχει καθε φορά. Ευχαριστώ
Θα ήθελα να μου πεις, ο αισθητήρας PING που έχεις είναι της parallax? Αν όχι, ποιάς εταιρείας είναι?
dimbot- Master
- Αριθμός μηνυμάτων : 344
Ημερομηνία εγγραφής : 22/12/2009
Πάντως γενικά να ξέρεις για τη σύνδεση του συγκεκριμένου αισθητήρα στον Basic Stamp θα πρέπει να συνδέσεις τα 2 καλώδια της τροφοσοσίας του στο Vss (το μαύρο) και στο Vdd (το κόκκινο) και το καλώδιο της επικοινωνίας με το μικροελεγκτή στο πιν I/O (P1, P2, P3......P14, P15). Προσωπικά το βάζω στο P15 αλλά δεν έχει καμία σημασία (εκτός από την αντιστοιχία με το πρόγραμμα).
Επίσης να μην ξεχάσεις να γεφυρώσεις το Vdd-Vin που βρίσκεται ανάμεσα στις 2 υποδοχές των σερβοκινητήρων Χ4 και Χ5 ώστε να μην πάθει ζημιά ο αισθητήρας.
Επίσης να μην ξεχάσεις να γεφυρώσεις το Vdd-Vin που βρίσκεται ανάμεσα στις 2 υποδοχές των σερβοκινητήρων Χ4 και Χ5 ώστε να μην πάθει ζημιά ο αισθητήρας.
spilios- Αριθμός μηνυμάτων : 20
Ημερομηνία εγγραφής : 24/09/2011
Ευχαριστώ πολύ φίλε μου, ο σενσορας είναι ο SRF04 Ultrasonic Range Finder.
spilios- Αριθμός μηνυμάτων : 20
Ημερομηνία εγγραφής : 24/09/2011
Φίλε εβγαλα άκρη από την διεύθυνση http://www.acroname.com/examples/10019/10019.html.
Αυτό δούλεψε κανονικα. Ενα ευχαριστώ και σε σένα και στον φίλο μου τον Μανωλη Φραγκ.
Αυτό δούλεψε κανονικα. Ενα ευχαριστώ και σε σένα και στον φίλο μου τον Μανωλη Φραγκ.
dimbot- Master
- Αριθμός μηνυμάτων : 344
Ημερομηνία εγγραφής : 22/12/2009
Πολύ χαίρομαι φίλε μου που βρήκες άκρη τελικά! Δώσε χαιρετίσματα στον Μάνο από όλη την κοινότητα!
|
|