Laboratorium Robotów i Manipulatorów

Robot mobilny kołowy Boe-Bot

Gliwice, 2008

Opracował: D. Krawczyk

1. Wprowadzenie

Robot mobilny kołowy Boe-Bot jest robotem edukacyjnym produkowanym przez firmę Parallax. Robot ten jest napędzany za pośrednictwem dwóch serwonapędów modelarskich, z których każdy napędza jedno koło robota. Sterowanie robotem odbywa się z wykorzystaniem mikrokontrolera z serii Basic Stamp. Programowanie robota możliwe jest za pośrednictwem języka wysokiego poziomu PBASIC. Możliwa jest współpraca oraz nawigacja robota z użyciem wielu różnego typu czujników (stykowe, podczerwieni, ultradźwiękowe, optyczne i wiele innych). Niniejsza instrukcja stanowi wprowadzenie do zagadnień programowania i nawigacji robotem Boe-Bot. Dodatkowe informacje o robocie jak również o dodatkowych urządzeniach, które mogą współpracować z robotem można uzyskać na stronie internetowej producenta, firmy Parallax (www.parallax.com).

2. Budowa robota

Głównymi elementami składowymi robota Boe-Bot są: metalowa konstrukcja nośna, dwa serwonapędy napędzające koła lewe i prawe (koło tylne nie jest napędzane) oraz układ sterowania robota (mikrokontroler, płytka montażowa, złącze AppMod, złącza serwonapędów, złącze zasilania). Widok robota przedstawiony jest na rys. 1.

Rys. 1. Ogólny widok robota Boe-Bot.

1. Napęd robota

Do napędu robota służą dwa serwonapędy o ruchu ciągłym, napędzające koło lewe i prawe. Widok takiego serwonapędu jest przedstawiony na rys. 2.

Rys. 2. Widok pojedynczego serwonapędu o ruchu ciągłym.

Serwonapędy te wykorzystują silniki prądu stałego z magnesem trwałym w stojanie. Oprócz silnika w skład serwonapędu wchodzi jeszcze przekładnia mechaniczna (zębata) zwielokrotniająca moment obrotowy, układ zasilający silnik w postaci mostka typu H oraz procesor sterujący. Serwonapęd posiada trzy wyprowadzenia: masa (przewód czarny), zasilanie +5V (przewód czerwony) oraz sterowanie (przewód biały). Sygnał sterujący serwonapędem ma postać impulsu prostokątnego, którego czas trwania zawiera informację zarówno o kierunku jak i prędkości wirowania serwonapędu. Sygnał sterujący powinien być wysyłany do serwonapędu cyklicznie co 20ms tak, jak przykładowo przedstawia to rys. 3.

Rys. 3. Zasada sterowania pracą serwonapędu poprzez impuls prostokątny.

Na rys. 3. impuls sterujący ma długość 1.5ms, co dla rozpatrywanych tu serwonapędów oznacza zatrzymanie silników. Aby wprawić dany serwonapęd w ruch należy wysłać do niego impuls dłuższy lub krótszy od impulsu 1.5ms. To czy impuls sterujący jest dłuższy czy krótszy od impulsu 1.5ms decyduje o kierunku jego wirowania, natomiast to, o ile dany impuls jest dłuższy bądź krótszy od 1.5ms decyduje o prędkości wirowania serwonapędu. Obrazowo przedstawiają to rys. 4 i 5.

Rys. 4. Wysterowanie serwonapędu do wirowania w prawo (czas trwania impulsu sterującego krótszy od 1.5ms).

Rys. 5. Wysterowanie serwonapędu do wirowania w lewo (czas trwania impulsu sterującego dłuższy od 1.5ms).

Należy zauważyć, że w obu przypadkach przedstawionych na rys. 4 i 5 serwonapędy będą wirowały w kierunkach przeciwnych, lecz z takimi samymi prędkościami. Wynika to z tego, że czasy trwania impulsów sterujących różnią się w obu przypadkach o 0.2ms względem 1.5ms. Sterownie ruchem robota polega zatem na odpowiednim, jednoczesnym wysterowaniu obu serwonapędów. Przykładowo chcąc wprawić robota w ruch do przodu należy serwonapęd prawy wysterować do wirowania w prawo z określoną prędkością, natomiast serwonapęd lewy wysterować do wirowania w lewo z dokładnie taką samą prędkością.

2. Sterowanie robotem

Do sterowania robotem wykorzystuje się mikrokontrolery z rodziny Basic Stamp (BS2 lub BS2e), przedstawione na rys. 6.

Rys. 6. Mikrokontrolery BASIC Stamp 2 i 2e.

W tabeli 1 przedstawiono zestawienie podstawowych cech obu typów mikrokontrolerów.

Tabela 1. Podstawowe parametry mikrokontrolerów BS2 i BS2e

	BS2	BS2e
Szybkość procesora	20 MHz	20 MHz
Szybkość wykonywania programu	~4000 instrukcji na sekundę	~4000 instrukcji na sekundę
Rozmiar pamięci RAM	32 B (6 I/O i 26 na zmienne)	32 B (6 I/O i 26 na zmienne)
Rozmiar pamięci RAM notatnikowej	brak	64 Bajty
Rozmiar pamięci programu (EEPROM)	2 KB	8x2 KB
Liczba pinów wejść/wyjść	16 + 2 dedykowane szeregowe	16 + 2 dedykowane szeregowe
Liczba komend języka PBASIC	42	45

W niniejszej instrukcji zaprezentowano wykorzystanie wybranych komend języka PBASIC do sterowania robotem. Wykaz wszystkich komend wraz z ich szczegółowym opisem znajduje się w elektronicznej dokumentacji mikrokontrolerów BASIC Stamp.

3. Programowanie robota

Do programowania robota wykorzystuje się język PBASIC, który jest językiem programowania mikrokontrolerów typu Basic Stamp. Język PBASIC jest językiem wysokiego poziomu w pełni funkcjonalnym i uniwersalnym. Dokładna specyfikacja języka wraz z opisem wszystkich dostępnych instrukcji znajduje się w dokumentacji anglojęzycznej dołączonej do środowiska Basic Stamp Editor. Programowanie robota odbywa się z poziomu środowiska Basic Stamp Editor. Widok głównego okna tego środowiska jest przedstawiony na rys. 7.

Rys. 7. Widok głównego okna środowiska programistycznego mikrokontrolerów typu Basic Stamp - Basic Stamp Editor.

Poniżej przedstawiono przykładowy, bardzo prosty program napisany w języku PBASIC:

' Laboratorium Robotów I Manipulatorów - robot mobilny kołowy Boe-Bot ' Wysłanie komunikatu tekstowego z mikrokontrolera BS do komputera. ' {$STAMP BS2} ' {$PBASIC 2.5} DEBUG "Hello, this is a message from your Boe-Bot." END

Linie programu poprzedzone znakiem apostrofu stanowią komentarz. Wyjątkiem są tutaj linie trzecia i czwarta, które stanowią tak zwane dyrektywy. Pierwsza z nich ({$STAMP BS2}) określa rodzaj procesora, do którego będzie ładowany program ze środowiska BASIC Stamp Editor. Druga dyrektywa ({$PBASIC 2.5}) określa z kolei, z której wersji języka PBASIC korzystamy (w tym przypadku wersja 2.5). Na początku każdego programu pisanego w języku PBASIC powinny znajdować się dyrektywy.

Dalszą część programu stanowią dwie linie zawierające instrukcje DEBUG i END języka PBASIC. Instrukcja DEBUG powoduje wysłanie komunikatu z mikrokontrolera BS do komputera PC. Komunikat ten jest wyświetlany w specjalnie w tym celu otwieranym okienku, jak przedstawia to rys. 8.

Rys. 8. Okno komunikatu wysyłanego z robota za pomocą komendy DEBUG.

Ostatnia instrukcja, czyli END oznacza koniec programu i jej wykonanie powoduje przejście mikrokontrolera do stanu niskiego poboru energii i oczekiwanie mikrokontrolera bądź na zresetowanie bądź na załadowanie nowego programu.

Jeśli chodzi o sterownie ruchem serwonapędów robota, to można do tego celu wykorzystać instrukcję PULSOUT, która powoduje wysłanie przez port mikrokontrolera o numerze Pin impulsu prostokątnego o czasie trwania Duration. Składnia instrukcji jest następująca:

PULSOUT Pin,Duration

Wartość parametru Duration jest wielokrotnością 2s. Zatem chcąc wysłać do określonego serwonapędu impuls sterujący o długości np. 1.5ms w polu Duration komendy PULSOUT należy wpisać wartość 750 (750*2s=1500s=1.5ms). Wprawienie robota w ruch do przodu wymaga jednoczesnego wysterowania obu serwonapędów, z tym że do jednego serwonapędu wysyłany jest impuls dłuższy (o wartość zależną od wymaganej prędkości ruchu) od impulsu bazowego (1.5ms), a do drugiego krótszy (o wartość taką samą jak w przypadku impulsu dłuższego). Zakładając, że serwonapędy podłączone są do portów 14 i 15 odpowiedni fragment programu mógłby wyglądać następująco:

PULSOUT 14,800

PULSOUT 15,700

Zamiana miejscami wartości parametru Duration spowodowałaby zmianę kierunku ruchu robota (jazda w tył). Każdorazowe wywołanie obu powyższych linii spowoduje wykonanie przez robota niewielkiego ruchu w przód. Chcąc uzyskać ruch ciągły należy instrukcje te wywoływać wielokrotnie (z odstępem czasu równym 20ms). Poniżej przedstawiono program realizujący ciągłą jazdę robota w przód.

' Laboratorium Robotów I Manipulatorów - robot mobilny kołowy Boe-Bot ' Jazda robota w przod. ' {$STAMP BS2} ' {$PBASIC 2.5} DO PULSOUT 14,750 PULSOUT 15,750 PAUSE 20 LOOP

Instrukcja DO-LOOP tworzy nieskończoną pętlę, czyli wszystkie instrukcje zawarte pomiędzy DO a LOOP są wykonywane nieskończoną ilość razy, czyli uzyskujemy efekt ciągłej jazdy w przód. Instrukcja PAUSE wprowadza opóźnienie 20ms (jednostką parametru tej instrukcji jest 1ms) po każdorazowym wysłaniu impulsów sterujących do serwonapędów. W podobny sposób, poprzez odpowiedni dobór parametru Duration instrukcji PULSOUT, można uzyskiwać skręty robota.

4. Czujniki

Ogromną zaletą robota Boe-Bot jest jego uniwersalność jeśli chodzi o możliwości stosowania różnorakich czujników. Najprostsze czujniki można budować z podstawowych elementów elektronicznych (rezystory, diody, fotorezystory, fotodiody, itp.) łączonych z wykorzystaniem uniwersalnej płytki montażowej, w którą jest wyposażony robot. Ponadto dostępnych jest szereg gotowych, bardziej zaawansowanych czujników, które można łączyć wprost z mikrokontrolerem BASIC Stamp i następnie należy je oprogramować z poziomu języka PBASIC. W dalszej części niniejszego rozdziału przedstawiono podstawowe informacje o wybranych czujnikach i zasadach ich wykorzystania w robocie Boe-Bot.

1. Czujnik elektromechaniczny

Jest to najprostszy rodzaj czujnika przeszkód jaki można wykonać z użyciem metalowych wąsów oraz czterech rezystorów. Widok czujnika oraz jego schemat elektryczny przedstawia rys. 9.

Rys. 9. Układ połączeń czujnika elektromechanicznego.

Czujnik taki umożliwia wykrywanie przeszkód w bezpośrednim sąsiedztwie robota, poprzez programową kontrolę stanów logicznych odpowiednich portów mikrokontrolera (np. porty nr 7 i 5 jak na rys. 8). Możliwe do rozróżnienia są cztery zdarzenia: brak przeszkody (oba łączniki otwarte), przeszkoda na wprost robota (oba łączniki zamknięte), przeszkoda z lewej strony robota (lewy łącznik zamknięty, prawy otwarty) bądź przeszkoda z prawej strony (lewy łącznik otwarty, prawy zamknięty). Załóżmy, że pożądany algorytm nawigacji polega na jeździe w przód, gdy nie ma żadnych przeszkód. Natomiast w przypadku wykrycia przeszkody robot w pierwszej kolejności ma wycofać się o pewną odległość i następnie odwrócić się o 180^o (przeszkoda na wprost), wycofać się o pewną odległość i skręcić w prawo (przeszkoda z lewej strony) lub też wycofać się o pewną odległość i skręcić w lewo (przeszkoda z prawej strony). Realizacja programowa takiej nawigacji robotem może wyglądać następująco:

' Laboratorium Robotów I Manipulatorów - robot mobilny kołowy Boe-Bot ' Nawigacja robota z omijaniem przeszkód z wykorzystaniem wąsów. ' {$STAMP BS2} ' {$PBASIC 2.5} ' Deklaracja zmiennych pulseCount VAR Byte 'zmienna wykorzystywana w pętli FOR...NEXT. ' Główna pętla DO IF (IN5 = 0) AND (IN7 = 0) THEN ' Oba łączniki zamknięte (przeszkoda na ' wprost) GOSUB Back_Up ' Cofnięcie do tyłu i dwukrotny skręt w lewo GOSUB Turn_Left GOSUB Turn_Left ELSEIF (IN5 = 0) THEN ' Lewy łącznik zamknięty (przeszkoda z lewej) GOSUB Back_Up ' Cofnięcie do tyłu GOSUB Turn_Right ' Skręt w prawo ELSEIF (IN7 = 0) THEN ' Prawy łącznik zamknięty (przeszkoda z prawej) GOSUB Back_Up ' Cofnięcie do tyłu i skręt w lewo GOSUB Turn_Left ELSE ' Oba łączniki otwarte (brak przeszkód) GOSUB Forward_Pulse ' Jazda w przód ENDIF ' ponowne sprawdzenie warunków LOOP ' Podprogramy realizujące manewry robota Forward_Pulse: ' Jazda w przód (pojedynczy impuls) PULSOUT 13,850 PULSOUT 12,650 PAUSE 20 RETURN Turn_Left: ' Skręt w lewo (o około 90 stopni) FOR pulseCount = 0 TO 20 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT RETURN Turn_Right: FOR pulseCount = 0 TO 20 ' Skręt w prawo (o około 90 stopni) PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 20 NEXT RETURN Back_Up: ' Jazda w tył (o określony odcinek) FOR pulseCount = 0 TO 40 PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 20 NEXT RETURN

W powyższym programie instrukcja IF…ELSEIF…ELSE…ENDIF służy do sprawdzania, które z czterech możliwych zdarzeń miało miejsce. Odczytywanie stanu portów (P5 i P7) zostało wykonane za pomocą instrukcji IN5 i IN7. Odpowiednie ruchy robota (jazda w przód, jazda w tył, skręty w lewo i prawo) zostały zaimplementowane w postaci podprogramów, które wywoływane są (z poziomu instrukcji IF…ELSEIF…ELSE…ENDIF) za pomocą instrukcji GOSUB…RETURN. Wykonanie instrukcji GOSUB etykieta powoduje przejście wykonywania programu do miejsca programu oznaczonego etykieta:. Powrót do instrukcji następnej po GOSUB etykieta następuje po wykonaniu instrukcji RETURN. Pętla FOR…TO…NEXT powoduje powtórzenie sekwencji instrukcji znajdujących się pomiędzy FOR a NEXT z góry zadaną ilość razy (zmienna pulseCount). W przedstawionym powyżej programie zmienna pulseCount decyduje o czasie (odległości) jazdy w tył (podprogram jazdy w tył), czy też o kącie skrętu w lewo bądź prawo (podprogramy skrętu w lewo bądź prawo).

2. Czujnik optoelektroniczny

Innym prostym czujnikiem, który można wykorzystać do nawigacji robota jest układ optoelektroniczny z wykorzystaniem dwóch fotorezystorów (rezystorów o rezystancji zależnej od natężenia światła). Układ taki (widok i schemat elektryczny) przedstawiono na rys. 10.

Rys. 10. Układ połączeń czujnika elektromechanicznego

Fotorezystory wraz z rezystorami o wartościach 2k stanowią dzielniki napięcia, o zmiennej (zależnej od zmiennej rezystancji fotorezystorów) przekładni napięciowej. Napięcia wyjściowe dzielników podawane są na porty cyfrowe mikrokontrolera (na rys. 10 porty P6 i P3). W przypadku gdy natężenie światła padającego na fotorezystor jest małe, jego rezystancja jest duża, co w dalszej kolejności powoduje pojawienie się na odpowiednim porcie mikrokontrolera stanu niskiego. W przypadku kiedy natężenie promieniowania jest duże, rezystancja fotorezystora jest mała, stan logiczny na porcie jest wysoki.

Czujnik taki umożliwia realizację algorytmu nawigacji polegającego na podążaniu robota w kierunku źródła światła. Odpowiedni program do takiej nawigacji robota będzie podobny do programu w punkcie 4.1.

3. Czujnik podczerwieni

Kolejnym prostym układem czujnika, który można zbudować z pojedynczych elementów dyskretnych wprost na płytce uniwersalnej robota jest układ wykorzystujący podczerwień. Na układ taki składają się dwie pary dioda podczerwieni-detektor podczerwieni. Schemat takiego układu przedstawiono na rys. x.

Rys. 11. Schemat czujnik podczerwieni.

Układ taki można wykorzystać do detekcji przeszkód. Zasada działania takiego czujnika polega na wysterowaniu diody emitującej podczerwień (poprzez wystawienie na odpowiedni port mikrokontrolera stanu wysokiego - na rys. 11 są to porty P8 i P2) i sprawdzaniu stanów logicznych na wejściach mikrokontrolera, do których podłączone są detektory podczerwieni (na rys. 11 są to porty P0 i P9). W przypadku obecności przeszkody wyemitowane promieniowanie podczerwone odbija się od niej i dociera do detektora podczerwieni, powodując wystąpienie na odpowiednim porcie mikrokontrolera stanu niskiego. Odpowiedni program do nawigacji robota z wykorzystaniem czujnika podczerwieni można stworzyć w oparciu o program z punktu 4.1. Czujnik z rys. 11 można również wykorzystać do określania przybliżonej odległości przeszkody od robota.

4. Czujnik śledzenia linii

Czujnik śledzenia linii jest czujnikiem optoelektronicznym, który umożliwia śledzenie ciemnej (bądź jasnej) linii narysowanej na jasnym (bądź ciemnym) tle. Czujnik ten jest zainstalowany w dolnej części robota, jak najbliżej podłoża, po którym porusza się robot. Czujnik składa się z 3 detektorów złożonych z diody emitującej podczerwień oraz fototranzystora. Czujnik ten dodatkowo jest wyposażony w trzy czerwone diody LED, które sygnalizują aktualny stan poszczególnych detektorów. Schemat czujnika jest przedstawiony na rys. 12.

Rys. 12. Schemat optoelektronicznego detektora linii.

Jeśli dany detektor znajduje się nad obszarem ciemnym (czarnym) daje na wyjściu sygnał niski, jeśli nad obszarem jasnym (białym) daje na wyjściu sygnał wysoki. W sytuacji kiedy skrajne detektory (lewy i prawy) dają jednakowy sygnał (oba stan wysoki lub niski), a środkowy detektor sygnał inny (niski lub wysoki) to robot znajduje się dokładnie nad linią prowadzącą. Wówczas należy robotem sterować w taki sposób, aby utrzymywać taki stan detektorów. W przypadku każdego innego stanu detektorów należy podjąć odpowiednie działania korygujące tor jazdy robota. Jako, że na czujnik składają się trzy detektory, w sumie jest osiem możliwych przypadków stanów detektorów i dla każdego z nich należy przewidzieć odpowiednie ruchy korygujące tor jazdy robota. Odpowiedni program nawigacji robota można stworzyć w oparciu o program przedstawiony w punkcie 4.1 (w instrukcji warunkowej IF…ELSEIF…ELSE…ENDIF należy jednocześnie sprawdzać stany trzech wejść cyfrowych mikrokontrolera).

5. Czujnik ultradźwiękowy

Czujnik ultradźwiękowy jest czujnikiem wykorzystującym ultradźwięki do określania odległości od przeszkody. W robocie Boe-Bot wykorzystany jest czujnik PING))). Zakres pomiarowy tego czujnika wynosi od około 2cm do 3 m. Zasada działania takiego czujnika opiera się na pomiarze czasu pomiędzy wysłaniem wiązki ultradźwięków, a odbiorem ultradźwięków odbitych od przeszkody. Zasada działania takiego czujnika jest przedstawiona na rys. 12.

Rys. 12. Zasada działania czujnika ultradźwiękowego typu PING))).

Aktywacja czujnika następuje w momencie wysłania przez mikrokontroler BS2 sygnału aktywującego w postaci impulsu prostokątnego o czasie trwania co najmniej 2s (typowo 5s). Wówczas czujnik generuje wiązkę ultradźwięków (40kHz), które poruszają się w przestrzeni z prędkością około 340m/s, odbijają się od przeszkód i wracają do czujnika. Następnie na wyjściu czujnika pojawia się impuls, który trwa tak długo, aż czujnik zarejestruje odbitą wiązkę ultradźwięków. Tak więc długość tego impulsu jest proporcjonalna do odległości przeszkody od czujnika. Poniżej przedstawiono testowy program wykorzystujący czujnik PING))) do określania odległości przeszkody od czujnika.

' Laboratorium Robotów I Manipulatorów - robot mobilny kołowy Boe-Bot ' Pomiar odległości za pomocą czujnika ultradźwiękowego PING))). ' {$STAMP BS2} ' {$PBASIC 2.5} ' Deklaracja stałych. CmConstant CON 2260 ' Deklaracja zmiennych. cmDistance VAR Word time VAR Word DO PULSOUT 15, 5 PULSIN 15, 1, time cmDistance = cmConstant ** time DEBUG HOME, DEC3 cmDistance, " cm" PAUSE 100 LOOP

Do inicjacji czujnika wykorzystano komendę PULSOUT (czujnik połączony do portu 15 mikrokontrolera). Następnie długość impulsu zwrotnego czujnika jest rejestrowana w zmiennej time przy pomocy instrukcji PULSIN (składnia: PULSIN pin, state, variable). Prędkość dźwięku w powietrzu jest uzależniona od temperatury, która to zależność jest uwzględniona w zmiennej CmConstant.

---