Reziorek.pl - Wykorzystanie czujnika odległości w robocie

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);
}