Sonar

Jeśli odwiedzasz ten blog i jesteś posiadaczem Arduino zapewne marzyłeś kiedyś o budowie własnego robota. Te najłatwiejsze wykonują proste rozkazy (obróć się, przesuń o 20 cm). Co innego, gdy chcemy zbudować robota o większej autonomii. Takiego, który samodzielnie szuka drogi do celu, który omija przeszkody. Brzmi fantastycznie? Oczywiście! Ale to nić trudnego wymaga odrobiny praktyki i…
… czujników zmysłów.

Jednym z nich jest oferowany w Nettigo sonar (Maxbotix MB1010). Sensor ten mierzy odległość do przeszkody. Robi to za pomocą ultradźwięków czyli fal o częstotliwości tak wysokiej, że nie reaguje na nie ludzkie ucho (około 42 kHz). Czujnik ten normalnie wykorzystywany jest w systemach alarmowych (w odróżnieniu od czujników na podczerwień pasywną, w tym można ustalić odległość, której przekroczenie powoduje uruchomienie alarmu), oraz w systemach parkowania do samochodów.
Umieszczając ten czujnik w robocie, zyskasz możliwość ustalania jak daleko znajduje się on od przeszkód lub stworzyć mapę otoczenia.
Maksymalna odległość wykrywana przez czujnik to 255 cali czyli ponad 6 metrów z dokładnością do 1 cala [2,54 cm]. Szerokość wiązki dźwięku „gołego” czujnika to około 24 cale [61 cm]. Minimalna odległość jaką wykrywa czujnik to 6 cali [15 cm].

Sonar EZ1 ma sporo wyprowadzeń, ale nie powinieneś się obawiać ich ilości. Większość z nich jest dla Twojej wygody. Pierwsze dwa (GND i +5) służą do zasilania czujnika. Podłącza się je w ich odpowiedniki w Arduino.

Pozostałe wyprowadzenia to różnorodne sygnały przedstawiające wykrytą odległość od przeszkody.

Odczyt analogowy

Analogowy sygnał odległości dostępny jest w czujniku na wyprowadzeniu oznaczonym napisem „AN”. PIN ten możemy podłączyć do jednego z wejść analogowych Arduino i odczytywać za pomocą funkcji „analogRead”.

Instrukcja podaje, że napięcie panujące na „AN” wzrasta o 9,8 mV na każdy cal odległości. Jak wiesz z poprzednich wpisów, zalecam ręczną kompensację, którą opiszę dalej.

Przykładowa funkcja odczytująca odległość z wyjścia „AN” może wyglądać tak:

// Przykład użycia analogowego wyjścia czujnika odległości LV-EZ1

// Analogowy PIN do którego podłączyłeś czujnik
#define SENSOR_PIN 0
// Wartości kalibracji
#define VALUE0 19
#define CM0 29
#define VALUE1 31
#define CM1 45

// Funkcja zwracająca odległość od rzeszkody w cm
unsigned int get_distance()
{
  int data = analogRead(SENSOR_PIN);
  // return data // Usuń znacznik komentarza przy kalibracji
  return map(data, VALUE0, VALUE1, CM0, CM1);
}


void setup()
{
  Serial.begin(9600);
}


void loop()
{
  Serial.print(get_distance(), DEC);
  Serial.println(" cm");
  delay(100);
}

Odczyt impulsowy

Kolejnym wygodnym wyjściem sensora jest PIN oznaczony napisem „PW”. Wyjście to wysyła impulsy nazywane PWM (Pulse Width Modulation). Oznacza to, że wraz z odległością, zwiększa się czas trwania poziomu „HIGH” impulsu. Arduino ma gotową funkcję i na tą okazję. Funkcja nosi nazwę „pulseIn”. Pierwszy jej argument to numer PINu cyfrowego w Arduino ustawionego w tryb „INPUT”, na którym badamy długość impulsu. Kolejny argument to wybór jakiego rodzaju impuls badamy. „HIGH”, jeśli badamy czas trwania sygnału wysokiego, „LOW” jeżeli mierzymy czas sygnału niskiego. Funkcja zwraca czas trwania impulsu w µs.
Funkcja dba też o to, aby program nie zaczął mierzyć impulsu od połowy i czeka na rozpoczęcie kolejnego najbliższego impulsu.

// Przykład użycia impulsowego wyjścia czujnika odległości LV-EZ1

// Cyfrowy PIN do którego podłączyłęś czujnik
#define SENSOR_PIN 2
// Wartości kalibracji
#define VALUE0 1075
#define CM0 19
#define VALUE1 2150
#define CM1 39

// Funkcja zwracająca odległość od przeszkody w cm
unsigned int get_distance()
{
  int data = pulseIn(SENSOR_PIN, HIGH);
  // return data // Usuń znacznik komentarza przy kalibracji
  return map(data, VALUE0, VALUE1, CM0, CM1);
}


void setup()
{
  Serial.begin(9600);
  pinMode(SENSOR_PIN, INPUT);
}


void loop()
{
  Serial.print(get_distance(), DEC);
  Serial.println(" cm");
  delay(100);
}

Instrukcja podaje, że na każdy cal odległości szerokość impulsu zwiększa się o 147 µs.

Odczyt szeregowy

Ostatnim wyjściem danych czujnika jest wyjście szeregowe oznaczone „TX”. Nadaje ono cyfrowe, asynchroniczne dane o odległości. Dane te są w formacie o parametrach:

  • szybkość: 9600 baudów
  • liczba bitów: 8
  • brak bitu parzystości
  • liczba bitów stop: 1

Szeregowa transmisja asynchroniczna

Polega na wysyłaniu informacji (liczb) za pomocą jednego przewodu. Liczby przesyła się za pomocą bajtów tzn. paczek po 8-bitów. Bity wysyła się po kolei od pierwszego do ostatniego.
Do paczki dodane są 2 bity. Na początku – bit startu mający wartość zawsze 0 (LOW). Służy on do rozpoznawania początku transmisji. Na końcu jest bit stopu mający wartość 1 (HIGH). Służy on do rozpoznawania końca transmisji.

Jeśli niczego nie przesyłamy, ustawiana jest wartość 1 (HIGH). Ma to na celu odróżnienie braku transmisji od rozpoczęcia transmisji za pomocą bitu startu.

Transmisja asynchroniczna polega na tym, że urządzenie nadawcze nie informuje kiedy rozpoczyna się nadawanie kolejnych bitów. Wyznacza się to określając prędkość transmisji. Prędkość transmisji oznacza ile bitów będziemy nadawali w ciągu sekundy (wliczając w to dodatkowe bity start i stop). Jednostką prędkości takiej transmisji jest baud.
Przyjmując, że urządzenie nadawcze wysyła dane z prędkością 9600 baudów możemy przyjąć, że każdy kolejny bit od pojawienia się bitu startu będzie pojawiał się co:

t = 1/f = 1/9600 = 0,000104167 s = 104,167 µs

Sygnał szeregowy sonaru idealnie pasowałby do standardu (Pasującego do PINów cyfrowych 0 i 1 (RX i TX)), gdyby nie to jest jest zanegowany. Zanegowany oznacza to, że jego impulsy są odwrócone. Tam gdzie odbiornik spodziewa się 1 logicznej (HIGH), czujnik nadaje 0 (LOW), tak samo w drugą stronę.
Sygnał ten możemy obserwować na komputerze w „Serial Monitor” dołączonym do oprogramowania Arduino IDE.
W tym celu musimy odwrócić sygnał. Można to zrobić za pomocą bramek logicznych typu „NOT” dostępnych w niezastąpionym „UCY 7404”. Bramka NOT, ma jedno wejście i jedno wyjście. Gdy na wejście otrzyma 1 logiczną, na wyjściu pojawi się 0. Gdy na wejściu pojawi się logiczne 0, wyjście ustawi się na 1.
Drugim krokiem jest wyjęcie procesora z Arduino. Brak procesora wykluczy konflikt sygnałów które mogą docierać na raz z niego i z czujnika do komputera, powodując błędy.

Wszystko możesz podłączyć wg tego schematu:

Po podłączeniu do komputera, „Serial Monitor” powinien zwracać nam bardzo szybko napisy:

R123[\r]

To właśnie zwraca czujnik przez port szeregowy.
Pierwszy znak „R” to nagłówek pozwalający rozpoznać początek pakietu danych (Tak, żeby oddzielić i rozpoznać poszczególne odległości). Kolejne 3 znaki to liczba reprezentująca odległość określoną w calach. Maksymalną odległością jest 255 cali.
Ostatnim znakiem jest wartość binarna 13, nazwana w tabeli znaków ASCII „CR” lub oznaczona w języku C specjalnym znakiem „\r”.

Więcej PINów?

Do obsługi portu szeregowego możemy skorzystać z jeszcze dwóch PINów czujnika. Pierwszy to „RX”. Logika podpowiada, że jest to wejście danych szeregowych (odwrotnie do „TX” – wyjście). Jednak w czujniku zastosowanie tego wejścia zostało uproszczone. Służy ono do włączania, bądź wyłączania mierzenia odległości. Sensor mierzy odległość i nadaje ją przez „TX”, gdy sygnał „RX” jest odłączony lub w stanie wysokim („HIGH”). Gdy „RX” jest w stanie niskim „LOW” to czujnik nie mierzy odległości i nie wysyła danych. Aby czujnik prawidłowo zmierzył odległość i wysłał dane, stan wysoki na wejściu „RX” musi trwać minimum 20 µs.
Ostatni PIN to „BW”. Jego ustawienie w stan wysoki powoduje, że PIN „TX” wysyła impulsy odpowiadające danym o wartości binarnej „0”.

Odczyt szeregowy w Arduino

Oczywiście wyjście to służy do tego, aby używać jego danych w Arduino, a nie w komputerze. Mamy do tego dwie możliwości. Jeśli nie będziemy w projekcie używać portu USB, możemy podłączyć wyjście „TX”, przez bramkę „NOT” bezpośrednio do PINu 0 („RX”) w Arduino (W Arduino Mega są jeszcze 2 dodatkowe wejścia szeregowe, których można używać pomimo korzystania z USB) i używać w programie obiektu „Serial”. Zastosowanie dodatkowych elementów jest jednak uciążliwe i skomplikowane.
W oprogramowaniu Arduino jest dostępna też biblioteka „SoftwareSerial”, pozwalająca na obsługę danych szeregowych na dowolnym PINie cyfrowym. Jednak nie obsługuje ona również odwróconego sygnału. Pozwoliłem ją sobie rozszerzyć o dodatkową metodę „nread”, czytająca zanegowany sygnał.

int SoftwareSerial::nread()
{
  int val = 0;
  int bitDelay = _bitPeriod - clockCyclesToMicroseconds(50);

  // one byte of serial data (LSB first)
  // ...--\    /--\/--\/--\/--\/--\/--\/--\/--\/--...
  //     \--/\--/\--/\--/\--/\--/\--/\--/\--/
  //    start  0   1   2   3   4   5   6   7 stop

  while (!digitalRead(_receivePin));

  // confirm that this is a real start bit, not line noise
  if (digitalRead(_receivePin) != LOW) {
    // frame start indicated by a falling edge and low start bit
    // jump to the middle of the low start bit
    delayMicroseconds(bitDelay / 2 - clockCyclesToMicroseconds(50));

    // offset of the bit in the byte: from 0 (LSB) to 7 (MSB)
    for (int offset = 0; offset < 8; offset++) {
    // jump to middle of next bit
    delayMicroseconds(bitDelay);

    // read bit
    val |= (~digitalRead(_receivePin) & 1) << offset;
    }

    delayMicroseconds(_bitPeriod);

    return val;
  }

  return -1;
}

Pozwoli ona bez problemów czytać dane z sensora z pominięciem jakichkolwiek dodatkowych elementów.

// Przykład użycia szeregowego wyjścia czujnika odległości LV-EZ1

#include <SoftwareSerial.h>

// Cyfrowe piny do których podłączyłeś czujnik
#define SENSOR_TX_PIN 2
#define SENSOR_RX_PIN 3

// Deklaracja obiektu programowej komunikacji szeregowej
SoftwareSerial sensor_serial = SoftwareSerial(SENSOR_TX_PIN, SENSOR_RX_PIN);

// Funkcja zwracająca odległość w calach
int get_distance()
{
  char data[4] = "000";
  char header = ' ';
  // Oczekiwanie na pojawienie się nagłówka danych (litery "R")
  while (header != 'R')
    header = sensor_serial.nread();
  // Odczyt poszczególnych cyfr odległości
  for (byte i = 0; i < 3; i++)
    data[i] = sensor_serial.nread();
  // Sprawdzenie poprawności zakończenia pakietu danych (znak '\r')
  header = sensor_serial.nread();
  if (header != 13)
    return -1; // Wartość zwracana jeśli pakiet danych nie jest poprawny
  return atoi(data); // Konwersja ciągu tekstowego na liczbe typu int
}


void setup()
{
  Serial.begin(9600);
  pinMode(SENSOR_PIN, INPUT);
  pinMode(3, OUTPUT);
  digitalWrite(3, LOW);
  sensor_serial.begin(9600);
}


void loop()
{
  Serial.println(get_distance());
  delay(100);
}

Niestety „SoftwareSerial” ma też pewne wady. Najważniejszą jest to, że nie jest asynchroniczna. Oznacza to, że jeśli chcemy odebrać dane, to musimy czekać aż czujnik nam je wyśle (metoda „read” i „nread” blokuje działanie programu aż do odebrania danych). Nie da się ustalić (jak w przypadku sprzętowego portu za pomocą metody „Serial.available”) czy już jakieś dane czekają na odbiór.

Na szczęście jest jeszcze jedna biblioteka „NewSoftSerial”, która jest pozbawiona tych wad i na dodatek obsługuje zanegowany sygnał szeregowy.

// Przykład użycia szeregowego wyjścia czujnika odległości LV-EZ1

#include <NewSoftSerial.h>

// Cyfrowe piny do których podłączyłeś czujnik
#define SENSOR_PIN_TX 2
#define SENSOR_PIN_RX 3

// Deklaracja obiektu programowej komunikacji szeregowej
NewSoftSerial sensor_serial(SENSOR_PIN, SENSOR_PIN_RX, true);

// Funkcja zwracająca odległość w calach
int get_distance()
{
  char data[4] = "000";
  char header = ' ';
  // Oczekiwanie na pojawienie się nagłówka danych (litery "R")
  while (header != 'R')
    header = sensor_serial.read();
  // Oczekiwanie na pojawienie sie w buforze 3 bajtów danych
  while (sensor_serial.available() < 4);
  // Odczyt cyfr odległości
  for (byte i = 0; i < 3; i++)
    data[i] = sensor_serial.read();
  // Sprawdzenie poprawności zakończenia pakietu danych (znak '\r')
  header = sensor_serial.read();
  if (header != 13)
    return -1; // Wartość zwracana jeśli pakiet danych nie jest poprawny
  return atoi(data);
}


void setup()
{
  Serial.begin(9600);
  pinMode(SENSOR_PIN, INPUT);
  pinMode(3, OUTPUT);
  digitalWrite(3, LOW);
  sensor_serial.begin(9600);
}


void loop()
{
  int distance = 0;
  // Sprawdzenie czy w buforze są jakieś dane
  while (sensor_serial.available())
  {
    distance = get_distance();
    if (distance > 0) // Sprawdzenie poprawności pakietu
      Serial.println(distance, DEC);
  }
}

Kalibracja

Pierwsze dwie metody pomiaru (analogowa i impulsowa) wymagają kalibracji (przez opory elektryczne i opóźnienia w programie). Należy sprawdzić przy jakich odległościach, jakie wartości zwracają funkcje odczytujące odległość. Potem za pomocą wcześniej opisywanej funkcji „map” ustalić przełożenie wartości na nasze jednostki.
Do kalibracji potrzebny jest centymetr krawiecki lub długa linijka. Czujnik kierujemy w bok, a na jego drodze kładziemy linijkę z zerem w miejscu gdzie kończy się obudowa czujnika. Potem w polu „widzenia” czujnika kładziemy na linijce dłoń (w miejscu powyżej 15 cm), zapisujemy odległość i wartość zwróconą przez funkcję. Potem umieszczamy dłoń nieco dalej i zapisujemy kolejne wartości.
Ponieważ czujnik zmienia wartość co 2,5 cm, dla zwiększenia dokładności najlepiej jest ustawiać przeszkodę w punkcie granicy zmiany wartości.

Zapisane wartości umieszczamy w definicjach programu. „CM0” i „VALUE0” to pierwsza odległość od czujnika i wartość zwracana z funkcji, „CM1” i „VALUE1” to kolejne.
Moje przykładowe wartości dla wejścia analogowego:

// Wartości kalibracji
#define VALUE0 19
#define CM0 29
#define VALUE1 31
#define CM1 45

oraz impulsowego:

// Wartości kalibracji
#define VALUE0 1075
#define CM0 19
#define VALUE1 2150
#define CM1 39

Radar

Dobrym przykładem wykorzystania sonaru będzie „radar”. Rozumiem przez to urządzenie, które obracając sensorem, zrobi mapę przeszkód w jego okolicy. W Twoim projekcie może się to przydać do planowania trasy przez robota (wyszukiwania przejść, orientacji w przestrzeni na podstawie charakterystycznych punktów).

Do obracania sensora wykorzystałem modelarski serwomechanizm.

Serwo to moduł zbudowany z silnika, kilku przekładni i odrobiny elektroniki. Tworzy to dość silny napęd (o sile w zależności od typu około 1 kg na 1 cm ramienia). To co odróżnia serwo od normalnego silnika, to możliwość ustawiania dowolnego kąta jego osi (z zakresu 0 – 180°). Tą właściwość wykorzystuje się umieszczając serwomechanizmy w modelach samolotów do napędzania sterów. Mamy pewność, że wysyłając odpowiedni sygnał, ster będzie pod takim kątem jaki ustalimy.

Z serwomechanizmu wychodzą zwykle trzy przewody.

  • Brązowy – GND
  • Czerwony – Zasilanie (maksymalnie 6V)
  • Żółty – sygnał sterujący

Sterowanie serwomechanizmem jest proste. Polega na dostarczaniu sygnału sterującego podpiętego pod dowolny PIN cyfrowy. Sygnał sterujący to impulsy PWM, których długość odpowiada kątowi o jaki jest obrócona oś serwomechanizmu. Przyjęta norma mówi, że impuls o długości 544 µs to kąt 0°, zaś 2400 µs to kąt 180°.
Dodatkowym elementem właściwie sztuczką jest odstęp między impulsami. Można przyjąć, że im większa przerwa między impulsami, tym serwo wolniej obraca się do ustalonego kąta. Ta sama zależność dotyczy siły z jaką działa i utrzymuje pozycje.

W ramach projektu Arduino dostępnych jest kilka bibliotek obsługujących serwomechanizmy. Wbudowana o nazwie Servo jest bardzo dobra do podstawowych operacji jednak ma pewną wadę. Wykorzystuje sprzętowy Timer procesora Arduino, z którego korzysta też programowy licznik czasu. Stosując tą bibliotekę, jesteś zmuszony do nieużywania funkcji takich jak „delay” i „millis”.

W przykładowym programie pozwoliłem sobie użyć innej biblioteki – SoftwareServo
Ma ona tę przewagę, że nie zakłóca działania innych funkcji. Dodatkową wadą, która może być jednocześnie zaletą jest konieczność wywoływania co jakiś czas metody „SoftwareServo::refresh()”, która odpowiada za wysłanie impulsu. Zaletą jest oczywiście oddanie „w nasze ręce” regulacji siły i szybkości serwomechanizmu (zależnej od częstotliwości wywoływania tej metody).

Budowa elektryczna radaru opiera się na połączeniu wyjścia szeregowego czujnika odległości do cyfrowych PINów (PIN2 – Czujnik TX, PIN3 – Czujnik RX). Sygnał sterujący serwomechanizmu został podłączony do cyfrowego PIN4. Zasilanie serwomechanizmu powinno wynosić maksymalnie 6V. Dlatego jeśli zasilamy Arduino takim napięciem lub z portu USB dobrze jest zasilić serwomechanizm z z PINu „Vin”.

Komunikacja z PC

Do komunikacji radaru z komputerem wykorzystałem port USB, służący normalnie do programowania Arduino. Port ten jest podłączony do wejścia/wyjścia szeregowego (PIN 0 [RX] – odbieranie danych z USB, PIN 1 [TX] – wysyłanie danych). Za komunikacje tym kanałem w programie Arduino służy wymieniony wyżej obiekt „Serial”.
Postanowiłem w oparciu o ten obiekt napisać funkcję podobną do tej, która odbiera dane szeregowe z sensora. Będzie można wysłać dwa rozkazy:

  • Rnnn\n – Wybiera kąt obrotu czujnika. W miejsce nnn należy wstawić kąt w stopniach w formacie 3 cyfrowym np. 175, 032 lub 005.
  • D – W odpowiedzi na to polecenie zostanie zwrócona odległość od przeszkody w aktualnym położeniu czujnika. Format odpowiedzi to „Dnnn\r\n”, gdzie „nnn” to odległość w calach.

Wyjaśnienia wymaga kilka uwag co do działania programu. Biblioteka „NewSoftSerial” podobnie jak obiekt „Serial” stosuje asynchroniczne pobieranie danych z udziałem bufora. Oznacza to, że pobiera dane szeregowe bez udziału głównego programu do specjalnego obszaru pamięci, w którym czekają one na odbiór. Ilość danych w buforze można sprawdzić za pomocą metody „available”. Program został skonstruowany tak, aby nie zaśmiecać bufora nieaktualnymi danymi o odległości. Dlatego jeśli nie pobierasz danych poleceniem „D”, program automatycznie blokuje wysyłanie danych za pomocą PIN3 (Sensor RX) i oczyszcza bufor funkcją „clear_buffer”.

// Program Radar dla Arduino

#include 
#include <NewSoftSerial.h>

// Cyfrowe PINy do których podłączyłeś czujnik
#define SENSOR_PIN_TX 2
#define SENSOR_PIN_RX 3

// Cyfrowy PIN do którego podłączyłęś sygnał sterujący PWM serwomechanizmu
#define SERVO_PIN 4

// Zmienna przechowująca aktualny kierune w jakim ma być skierowany czujnik
int direction = 90;
// Zmienna przechowująca informacje o tym czy zarządano podania odległości
byte read_distance = 0;

// Deklaracja obiektu komunikacji szeregowej z czujnikiem
NewSoftSerial sensor_serial(SENSOR_PIN_TX, SENSOR_PIN_RX, true);
// Deklaracja obiektu sterowania serwomechanizmem
SoftwareServo sensor_servo;

// Funkcja zwracająca odległość w calach
int get_distance()
{
  char data[4] = "000";
  char header = ' ';
  while (header != 'R')
    header = sensor_serial.read();
  while (sensor_serial.available() < 4);
  for (byte i = 0; i < 3; i++)
    data[i] = sensor_serial.read();
  header = sensor_serial.read();
  if (header != 13)
    return -1;
  return atoi(data);
}

// Funkcja wysyłająca liczbę w formacie "nnn" (3-cyfrowym)
void send_number(int number)
{
  if (number < 100)
    Serial.print("0");
  if (number < 10)
    Serial.print("0");
  Serial.print(number, DEC);
}

// Funkcja wysyłająca do komputera dane o odległości
void send_data(int distance)
{
  Serial.print("D");
  send_number(distance);
  Serial.println("");
}

// Funkcja odbierająca rozkazy z komputera
// Argument: timeout - czas w [ms] po którym przerywane są próby odczytu
int get_direction(unsigned long timeout=1000)
{
  char data[4] = "000";
  char header = ' ';
  // Zapamiętanie aktualnego czasu
  unsigned long time = millis();
  // Pobranie nagłówka
  header = Serial.read();
  // Sprawdzenie czy jest to żadanie odległości
  if (header == 'D')
  {
    read_distance = 1;
    return -4;
  }
  // Sprawdzenie czy jest to nagłówek rozkazu kierunku
  if (header != 'R')
    return -1; // Jeśli inny to zwrócenie błędu o kodzie -1
  // Oczekiwanie na pojawienie się 3 bajtów danych
  while (Serial.available() < 4)
  {
    // Sprawdzenie czy upłynął dopuszczalny czas oczekiwania
    if ((millis() - time) > timeout)
      return -2; // Jeśli tak to zwrócenie kodu błędu -2
  }
  // Odczyt cyfr odległości
  for (byte i = 0; i < 3; i++)
    data[i] = Serial.read();
  // Sprawdzenie poprawności zakończenia nadawania danych
  header = Serial.read();
  if (header != '\n')
    return -3; // Zwrócenie -3 oznacza niepoprawne zakończenie
  return atoi(data);
}

// Funkcja sprawdza czy wysłano rozkaz o obrocie i wykonanie go
void rotate()
{
  // Sprawdzenie czy w buforze czekają jakieś dane
  while (Serial.available())
  {
    direction = get_direction();
    if (direction >= 0)
      // Jeśli dane były poprawne to ustawienie kąta serwomechanizmu
      sensor_servo.write(direction);
  }
}

// Czyszczenie bufora komunikacji programowej z czujnikiem
void clear_buffer()
{
  while (sensor_serial.available())
    sensor_serial.flush();
}

// Funkcja zwraca odległość jeśli tego zażądano
void distance()
{
  // Sprawdzenie czy żądano zwrócenia odległości
  if (read_distance == 1)
  {
    // Aktywacja czujnika
    digitalWrite(SENSOR_PIN_RX, HIGH);
    // 20 us dla czujnika na określenie odległości
    delayMicroseconds(20);
    // Oczekiwanie na dane z czujnika
    while (sensor_serial.available())
    {
      int distance = get_distance();
      if (distance > 0)
      {
        // Jeśli czujnik zwrócił poprawne dane to wysłanie ich do komputera
        send_data(distance);
        // Skasowanie żądania o podanie odległości
        read_distance = 0;
        // Zatrzymanie pracy czujnika
        digitalWrite(SENSOR_PIN_RX, LOW);
        // Wyczyszczenie bufora
        clear_buffer();
      }
    }
  }
}


void setup()
{
  Serial.begin(9600);
  pinMode(SENSOR_PIN_TX, INPUT);
  pinMode(SENSOR_PIN_RX, OUTPUT);
  digitalWrite(SENSOR_PIN_RX, LOW);
  sensor_serial.begin(9600);
  // Kalibracja serwomechanizmu - minimalny i maksymalny czas impulsu w us
  sensor_servo.setMinimumPulse(560);
  sensor_servo.setMaximumPulse(2550);
  // Ustalenie pinu sterującego serwomechanizm
  sensor_servo.attach(SERVO_PIN);
  // Ustawienie serwomechanizmu na środek
  sensor_servo.write(90);
  // Odczekanie na ustawienie serwomechanizmu i uruchomienie czujnika
  delay(1000);
  // Czyszczenie bufora
  clear_buffer();
}


void loop()
{
  rotate();
  distance();
  
  // Wysłanie impulsu do serwomechanizmu
  SoftwareServo::refresh();
  delay(10);
}

Program na PC

Do przetwarzania danych w komputerze użyłem języka skryptowego Python. Komunikację z Arduino zapewnia moduł „PySerial”.

Aby przygotować komunikacje z Arduino należy zaimportować moduł „serial” i przygotować obiekt klasy „Serial”.

Pod Linuksem u mnie wygląda to tak:

import serial
radar = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)

Pierwszy argument to plik urządzenia reprezentującego port szeregowy. Jego nazwa odpowiada temu co wybraliśmy w Arduino IDE w menu „Tools > Serial Port”. Pod Windows będzie to „COMn”, gdzie w miejsce „n” należy wstawić prawidłowy numer portu.
Kolejnym argumentem jest prędkość transmisji szeregowej. Ostatnim odpowiada za czas w sekundach po jakim metoda „read” ma przestać czekać na dane.
Wynikiem działania tego fragmentu jest obiekt „radar” na którym dokonujemy operacji zapisu lub odczytu z przypisanego portu.

Aby wysłać polecenie ustawienia kąta obrotu czujnika na 180°:

radar.write('R180\n')

Aby odczytać odległość postępujemy tak:

radar.write('D')
distance = radar.read(6)

W zmiennej „distance” powinien pojawić się ciąg tekstowy „Dnnn\r\n” z odległością w miejscu „nnn”.

Dużo wygodniejszym jest odebranie danych za pomocą metody „readline”. Odczytuje ona serie danych aż do pojawienia się znaków rozpoczęcia nowej linii („\n” lub „\r\n”).

radar.write('D')
distance = radar.readline()

Żeby zamienić te dane w wartość liczbową w centymetrach można napisać:

if distance[0] == 'D': # sprawdzenie poprawności nagłówka danych
  distance = int(distance[1:4]) * 2.54 # konwersja fragmentu ciągu na liczbę i zmiana jednostki

W wyniku czego w zmiennej „distance” pojawi się odległość w centymetrach.

Praktyczny program

Do atrakcyjnego zobrazowania idei radaru postanowiłem napisać program, który będzie symulował jego działanie. Do tego użyłem wyżej przedstawionego języka Python oraz modułów PyGTK, Cairo i Serial. Do działania w Ubuntu wymaga zainstalowania jedynie pakietu „python-serial”. Pod Windows również można zgromadzić niezbędne pakiety w formie pakietów instalacyjnych exe/msi python, pygtk, pyserial.

#!/usr/bin/env python
#-*- coding:utf-8 -*-

# program do obsługi Radaru
import math
import gtk
import cairo
import pangocairo
import gobject
import serial

# Klasa ładowania obrazków PNG
class Image(object):

    def __init__(self, filename):
        self.set_file(filename)
        
    def set_file(self, filename):
        self.surface = cairo.ImageSurface.create_from_png(filename)
        
    def draw(self, cr):
        cr.save()
        cr.set_source_surface(self.surface)
        cr.paint()
        cr.restore()

# Funkcja obracająca względem współrzędnych
def rotatexy(x, y, angle, cr):
    rad = math.radians(angle)
    s = math.sin(rad)
    c = math.cos(rad)
    matrix = cairo.Matrix(c, s, -s, c, x-c*x+s*y, y-s*x-c*y)
    cr.transform(matrix)

# Klasa komunikacji z Arduino
class Hardware(object):

    def __init__(self, port):
        self.serial = serial.Serial(port, 9600)
        
    def set_direction(self, direction):
        self.serial.write('R%03i\n' % direction)
        
    def get_distance(self):
        # Podwójny odczyt odległości z winy błędów w czujniku
        self.serial.write('D')
        data = self.serial.readline()
        self.serial.write('D')
        data = self.serial.readline()
        return int(data[1:4])

# Widget GTK przedostawiający radar
class Radar(gtk.DrawingArea):
    
    def __init__(self):
        gtk.DrawingArea.__init__(self)
        self.distances = [0] * 180
        self.angle = 0
        self.direction = True #True == Right
        
        self.background = Image('background.png')
        self.left_hand = Image('left_hand.png')
        self.right_hand = Image('right_hand.png')
        self.glass = Image('glass.png')
        
        self.connect("expose-event", self.expose)
        self.set_size_request(512, 256)
        
    def draw(self, cr):
        cr.set_source_rgb(0, 0, 0)
        cr.paint()
        
        self.background.draw(cr)
        
        cr.save()
        rotatexy(255, 255, self.angle, cr)
        if self.direction:
            self.right_hand.draw(cr)
        else:
            self.left_hand.draw(cr)
        cr.restore()
        
        cr.save()
        cr.set_source_rgba(0, 1, 0, 0.2)
        cr.set_line_width(2)
        cr.move_to(0, 0)
        cr.line_to(0, 255)
        for angle in xrange(180):
            if self.distances[angle] > 0:
                rad = math.radians(angle + 180)
                x = math.cos(rad) * self.distances[angle] + 255
                y = math.sin(rad) * self.distances[angle] + 255
                cr.line_to(x, y)
        cr.line_to(511, 255)
        cr.line_to(511, 0)
        cr.close_path()
        cr.fill_preserve()
        cr.set_source_rgb(0, 1, 0)
        cr.stroke()
        cr.restore()
        
        cr.save()
        cr.translate(0, 175)
        self.glass.draw(cr)
        cr.restore()
        
    def expose(self, widget, event, data=None):
        cr = self.window.cairo_create()
        cr = pangocairo.CairoContext(cr)
        self.draw(cr)
        return False
        
    def refresh(self):
        alloc = self.get_allocation()
        rect = gtk.gdk.Rectangle(0, 0, alloc.width, alloc.height)
        self.window.invalidate_rect(rect, True)
        self.window.process_updates(True)
        
    def set_distance(self, angle, distance):
        self.angle = angle
        self.distances[self.angle] = distance
        self.refresh()

# Kontroler - klasa aplikacji
class App(gtk.Window):

    def __init__(self):
        gtk.Window.__init__(self)
        
        self.angle = 90
        self.direction = 10
        
        self.hardware = Hardware('/dev/ttyUSB0') # Tu ustaw odpowiedni port
        
        self.radar = Radar()
        self.add(self.radar)
        
        self.connect('destroy', self.close)
        
        self.show_all()
        
        self.set_resizable(False)
        
        gobject.timeout_add(500, self.rotate)
        
        gtk.main()
        
    def distance(self):
        if self.direction > 0:
            self.radar.direction = True
        else:
            self.radar.direction = False
        distance = self.hardware.get_distance()
        self.radar.set_distance(self.angle, distance)
        return False
        
    def rotate(self):
        self.angle += self.direction
        if self.angle > 179:
            self.angle = 179
            self.direction = -10
        elif self.angle < 0:
            self.angle = 0
            self.direction = 10
        
        self.hardware.set_direction(self.angle)
        gobject.timeout_add(50, self.distance)
        return True
        
    def close(self, widget, data=None):
        gtk.main_quit()


if __name__ == '__main__':
    App()


Uwaga!
Dla łatwego zrozumienia działania programy nie posiadają rozbudowanej obsługi błędów. Obsługa ta jednak może być w prosty sposób dopisana.

Źródła

19 myśli nt. „Sonar

  1. RaV

    Fajne!
    Teraz czekamy na serwa, silniczki itp. w ofercie, i będzie można w jednym sklepie kupić wszystko do zabawy w robotykę :-)

    Odpowiedz
  2. Pingback: Arduino « Piotr Pietruszka

  3. Hubert

    Super sprawa, trzymam kciuki i wspieram rozwój tego serwisu, bo sporo można się nauczyć, nie powiem bo dość skomplikowane to jest, ale można już zacząć się bawić!

    Odpowiedz
    1. netmaniac

      Tak naprawdę to nie jest aż tak skomplikowane. Trzeba samemu spróbować.

      I pamiętać, że rzadko co działa ‚od pierwszego kopa’. Trzeba kawałek po kawałku budować klocki i testować :)

      Odpowiedz
  4. Torvus

    Jakoś na razie nic z tego języka nie rozumiem. Może ktoś by miał ochotę rozpisać prosty przykład jaki sobie ułożyłem na płytce a nie potrafię go zaprogramować.

    Mam siedem diodek podłączonych z opornikami do portów cyfrowych 2-8 i sonar podpięty analogowo przez AN do A1
    Chciałbym aby zależnie od odległości gasły i zapalały się diodki :)

    Odpowiedz
  5. sprae Autor wpisu

    Torvus: Do czytania czujnika jest funkcja analogRead(nr_pinu). Potem trzeba skonwertować to co zwraca ta funkcja na poszczególne piny ledów np. za pomocą instrukcji „if”.
    w loop może to wyglądać tak:
    int value = analogRead(1);
    if (value > 10) digitalWrite(2, HIGH);

    Ten fragment czyta wartość z pinu A1 i jeśli przekroczy ona 10 to zapala LED podłączoną do pinu D2.

    Odpowiedz
  6. Torvus

    Dzięki! To mi rozjaśnia kolejną świadomość.
    To jeszcze pytanie drugie :)
    Mam zaprogramowany sonar, który jest przetwarzany na dźwięk.
    Wszystko działa tak jakbym chciał ale nie wiem jakie mu dać polecenie aby dźwięk się urywał przy maksymalnej odległości, jaką czyta sonar…

    Odpowiedz
  7. MArek

    Czy można by poprosić o uaktualnienie kodów dla ArduinoUno oraz programu w Pythonie.
    Próbowałem złożyć to w całość i musiałem dokonać następujących zmian w obu programach:

    Zmiany w stosunku do SONAR z nettigo.pl

    W kodzie programu (wgrywanego do ArduinoUno) radar.ino zmieniono nazwe biblioteki NewSoftSerial na SoftwareSerial:
    #include
    na #include
    oraz zamieniono wszystkie odwolania do NewSoftSerial na SoftwareSerial
    W pliku naglowkowym SoftwareSerwo.h bedacym czescia biblioteki SoftwareServo zamieniono
    to -> #include
    na to -> #include „Arduino.h”
    W skrypcie python sonar.py skorygowano port:
    self.hardware = Hardware(‚/dev/ttyACM0’) # Tu ustaw odpowiedni port

    ale niestety nie działa tj:
    Program w Pythonie na PC jest interpretowany (uruchomiony Linux UBUNTU) ale nie „omiata skali” i nie wskazuje odległości. Serwo nie kręci się. Jest jakiś błąd. Czy istnieje możliwość odświeżenia tematu tj. Wystawienie „nowych” kodów (programów). Może również schemat w Fritzing PLEASE!

    Odpowiedz
  8. MArek

    w poprzednim wpisie nie wyświetliło
    pierwszy #include NewSoftSerial – w ostrych nawiasach
    drugi #include SoftwareSerial – w ostrych nawiasach
    trzeci #include WProgram.h – w ostrych nawiasach

    Odpowiedz
  9. Marek

    Bardzo dziękuje za natychmiastową odpowiedź i proszę wybaczyć moje spóźninenie.
    Uruchomiłem nowe IDE1.0.2 (pracowałem na 1.0.1)
    Skompilowałem – (z małą poprawką w kodzie includowana jest SoftSerial.h ale kompiluje się SoftwareSerial.h ;) – więc poprawiłem)
    Podłączenie czujnika i serwa wg. rys sonar-rada.png (nad Komunikacja z PC)
    Jeśli chodzi o sonar.py to tylko nadmieniona wyżej kwestia podania odpowiedniego portu i … i … iii
    CHODZI CHODZI ! HURRRRA a właściwie DZIĘĘĘĘĘKKKKKIIIIIII !
    Po zmontowaniu modelu podłączę filmik z youtuba.
    Poleceam się pamięci :)

    Odpowiedz
  10. sprae

    O świetnie, że się udało :) Akurat nie miałem dostępu do Arduino IDE i poprawiałem tylko na podstawie dokumentacji :)
    Ale poradziłeś sobie znakomicie. Dołączę do wpisu poprawiony plik.

    Ponieważ GTK podupadło ostatnio w rozwoju, postanowiłem że w następnych wpisach z przykładami będę się opierał na programach na przeglądarkę Chrome, bo dostała obsługę portów szeregowych i jest chyba wystarczająco międzyplatformowa. Czy to dobry pomysł?

    Odpowiedz
  11. Marek

    Myślę, że pomysł ma przyszłość. Oczywiście my „robaczki” prosimy o umieszczenie przykładowych projektów, tak byśmy się mogli uczyć :)

    Odpowiedz
  12. Marek

    Jeszcze taki trick.
    Z nieznanych, jeszcze :) powodów aby program obrazujący odczyty czujnika odległości pracował poprawnie należy uruchomić IDE1.0.2 (Arduino) a następnie Serial Monitor. Dopiero wtedy i tak się dzieje w moim przypadku program w Pythonie zaczyna działać. Do momentu uruchomienia Serial Monitora servo burczy ale się nie obraca a okno wizualizujące nie jest „omiatane wiązką promieniowania”

    Reasumując:

    Arduino ma wgrany program a platforma jest podłączona via USB do PC i zasilana. Czujnik i servo poprawnie podłączone wg schematu.
    Program w Pythonie uruchomiony (tylko nie działa – nie omiata promieniem swego „ekranu”).
    Teraz uruchamiamy IDE ArduinoUno i włączamy SerialMonitor i zaczyna działać.
    Wołamy rodzinę aby mogła być dumna, pierwszy element TARCZY POLSKI gotowy!

    Odpowiedz
  13. Pingback: Taśma LED sterowana czujnikiem odległości » Majsterkowo.pl

Dodaj komentarz

Twój adres email nie zostanie opublikowany. Pola, których wypełnienie jest wymagane, są oznaczone symbolem *