W artykule zostanie omówione podłączenie czujnika odległości HC-SR04 do robota i będzie służył on do automatycznego hamowania gdy na jego drodze znajdzie się przeszkoda.
Spis treści
- Podłączenie czujników
- Zaprogramowanie robota
- Gotowy kod z wykorzystaniem czujników wraz ze zmianą prędkości jazdy robota
- Pliki do pobrania
Podłączenie czujników
Możemy teraz przejść do wykorzystania czujnika ultradźwiękowego i sprawić, aby robot zatrzymał się kiedy napotka przeszkodę na swojej drodze. Wykorzystamy do tego wcześniej napisany kod, oczywiście zostanie on zmodyfikowany, podpinamy również czujniki, w naszym wypadku wykorzystamy dwa, jeden z przodu, a drugi z tyłu. Aby je podłączyć musimy złączyć piny trig jednego i drugiego czujnika do złączki na kable, następnie wyprowadzamy z niej jeden kabel i wpinamy do pinu drugiego na arduino. Piny echo wpinamy już pojedyńczo, z jednego czujnika echo prowadzimy do pinu 13, a z drugiego do 12. VCC umieszczamy w 5V, a GND w GND. Kiedy mamy już gotowe połączenie możemy przejść do programowania.
Zaprogramowanie robota
Wykorzystamy już wcześniej napisany kod który został omówiony w osobnym artykule, możecie zapoznać się z nim i pobrać potrzebne pliki tutaj.
Zaczynamy od zdefiniowania potrzebnych pinów
#define echo 3 #define trig 13 #define trig1 12
Następnie deklarujemy piny jako wyjścia i wejścia
pinMode(echo, INPUT);//ustawienie pinu echo jako wejście pinMode(echo1, INPUT);//ustawienie pinu echo1 jako wejście pinMode(trig, OUTPUT);//ustawienie pinu trig1 jako wyjście
Teraz skoro mamy dwa czujniki będziemy musieli odczytywać z nich wartości, skorzystamy z biblioteki która umożliwi nam proste odczytywanie wartości, możecie ją pobrać tutaj.
Implementujemy bibliotekę (na samej górze programu), tworzymy obiekt oraz przypisujemy do niego odpowiednie piny
HCSR04 hc(trig, new int[2] {echo, echo1}, 2);
Teraz skorzystamy z funkcji i zapiszemy wyniki odczytów do zadeklarowanych zmiennych, jedna odpowiada za przetrzymywanie danych z tylniego czujnika, a druga z przedniego.
int dys = hc.dist(0);//czujnik tył int dys1 = hc.dist(1);//czujnik przód
Dodatkowo dodajemy instrukcje warunkowe, które zamienią 0 (przypadek gdy obiekt jest ponad 200 cm od czujnika) na większą wartość, aby pojazd mógł jechać.
if(dys1 == 0) { dys1 = 255; } if (dys == 0) { dys = 255; }
Teraz wystarczy, że zmienimy warunki if w taki sposób, gdy odległość będzie mniejsza od 20 cm, to warunek ruszania się nie wykona, a silniki się wyłączą, wygląda to w następujący sposób.
if (galkaY < 108 && dys > 35) { //jeżeli gałka jest przechylona w przód i dystans jest większy od 20, to wykonaj instrukcję poniżej jedzDoPrzodu(); } else if (galkaY > 148 && dys1 > 35) {//w przeciwnym wypadku jeżeli gałka jest przechylona w dół i dystans jest większy od 20, to wykonaj instrukcję poniżej jedzDoTylu(); } else if (galkaY == 128 || dys < 35 || dys1 < 35) {//w przeciwnym wypadku jeżeli gałka jest na swojej pozycji albo dystans jest mniejszy od 20, to wykonaj instrukcję poniżej stoj(); }
To by było na tyle, teraz nasz robot napotykając przeszkodę na swojej drodze zatrzyma się, co uchroni go przed możliwymi uszkodzeniami. Cały gotowy kod znajduje się do pobrania poniżej.
Gotowy kod z wykorzystaniem czujników wraz ze zmianą prędkości jazdy robota
Połączę tutaj użycie czujników odległości i zmiany prędkości, gotowy kod wygląda następująco(cały kod można pobrać na końcu artykułu).
#include <PS2X_lib.h> #include <HCSR04.h> #define CLK A3 #define DAT A0 #define CMD A1 #define SEL A2 //silnik 1 #define out1 11 #define out2 10 //silnik 2 #define out3 9 #define out4 8 //silnik 3 #define out5 7 #define out6 6 //silnik 4 #define out7 5 #define out8 4 //czujniki #define trig 2 #define echo 13 #define echo1 12 #define PWM 3 PS2X pad; HCSR04 hc(trig, new int[2] {echo, echo1}, 2); int error = 0; int V = 85; void setup() { Serial.begin(9600); delay(300); pinMode(out1, OUTPUT); pinMode(out2, OUTPUT); pinMode(out3, OUTPUT); pinMode(out4, OUTPUT); pinMode(out5, OUTPUT); pinMode(out6, OUTPUT); pinMode(out7, OUTPUT); pinMode(out8, OUTPUT); pinMode(PWM, OUTPUT); pinMode(echo, INPUT); pinMode(echo1, INPUT); pinMode(trig, OUTPUT); error = pad.config_gamepad(CLK, CMD, SEL, DAT); } void loop() { if (error == 1) { error = pad.config_gamepad(CLK, CMD, SEL, DAT); return; } pad.read_gamepad(); int galkaY = pad.Analog(PSS_LY); int galkaX = pad.Analog(PSS_LX); int dys = hc.dist(0); int dys1 = hc.dist(1); if(dys1 == 0) { dys1 = 255; } if (dys == 0) { dys = 255; } if (V > 245) { V = 245; } else if (V < 55) { V = 55; } if (pad.Button(PSB_L1)) { V = V + 10; } else if (pad.Button(PSB_R1)) { V = V - 10; } if (pad.Button(PSB_L2)) { analogWrite(PWM, 255); } else { analogWrite(PWM, V); } if (galkaY < 108 && dys > 35) { jedzDoPrzodu(); } else if (galkaY > 148 && dys1 > 35) { jedzDoTylu(); } else if (galkaY == 128 || dys < 35 || dys1 < 35) { stoj(); } if (galkaX < 108) { skrecLewo(); } else if (galkaX > 148) { skrecPrawo(); } if (pad.Button(PSB_CROSS)) { obrotLewo(); } else if (pad.Button(PSB_CIRCLE)) { obrotPrawo(); } } void jedzDoPrzodu() { digitalWrite(out1, LOW); digitalWrite(out2, HIGH); digitalWrite(out3, LOW); digitalWrite(out4, HIGH); digitalWrite(out5, LOW); digitalWrite(out6, HIGH); digitalWrite(out7, LOW); digitalWrite(out8, HIGH); } void jedzDoTylu() { digitalWrite(out1, HIGH); digitalWrite(out2, LOW); digitalWrite(out3, HIGH); digitalWrite(out4, LOW); digitalWrite(out5, HIGH); digitalWrite(out6, LOW); digitalWrite(out7, HIGH); digitalWrite(out8, LOW); } void skrecLewo() { digitalWrite(out1, LOW); digitalWrite(out2, HIGH); digitalWrite(out3, LOW); digitalWrite(out4, HIGH); digitalWrite(out5, LOW); digitalWrite(out6, LOW); digitalWrite(out7, LOW); digitalWrite(out8, LOW); } void skrecPrawo() { digitalWrite(out1, LOW); digitalWrite(out2, LOW); digitalWrite(out3, LOW); digitalWrite(out4, LOW); digitalWrite(out5, LOW); digitalWrite(out6, HIGH); digitalWrite(out7, LOW); digitalWrite(out8, HIGH); } void obrotLewo() { digitalWrite(out1, LOW); digitalWrite(out2, HIGH); digitalWrite(out3, LOW); digitalWrite(out4, HIGH); digitalWrite(out5, HIGH); digitalWrite(out6, LOW); digitalWrite(out7, HIGH); digitalWrite(out8, LOW); } void obrotPrawo() { digitalWrite(out1, HIGH); digitalWrite(out2, LOW); digitalWrite(out3, HIGH); digitalWrite(out4, LOW); digitalWrite(out5, LOW); digitalWrite(out6, HIGH); digitalWrite(out7, LOW); digitalWrite(out8, HIGH); } void stoj() { digitalWrite(out1, LOW); digitalWrite(out2, LOW); digitalWrite(out3, LOW); digitalWrite(out4, LOW); digitalWrite(out5, LOW); digitalWrite(out6, LOW); digitalWrite(out7, LOW); digitalWrite(out8, LOW); }