P RROBOrobak

OJEKTY

ROBOrobak,
cz 1

Postanowiem poczy przyjemne

z poytecznym budujc
zabawk, ktra przysporzy
moim pociechom radoci. I oto
powsta samodzielnie i wedug
swego widzimisi podejmujcy
proste decyzje sympatyczny
ROBOrobak. Jedynym zadaniem
ROBOrobaka jest poruszanie si
po pomieszczeniu, wydawaoby
si, w celach poznawczych,
do momentu wyczerpania si
akumulatorkw.
Rekomendacje:
projekt icie wakacyjny,
dajcy jednoczenie moliwo
poznania od strony praktycznej
podstawowych zagadnie
zwizanych ze sterowaniem
silnikw elektrycznych za
pomoc mikrokontrolerw.

Elektronika Praktyczna 8/2005

Zasada dziaania/zachowanie

Prezentowana zabawka porusza

si na trzech kkach. Pierwsze kko wolne, bez napdu, obraca si
take swobodnie w paszczynie poziomej, pozostae koa s napdzane
bezporednio przez silniki. ROBOrobak jest zaopatrzony w czuki suce do identykowania napotykanych
po drodze przeszkd. Czuek rodkowy ma za zadanie wykrywanie
przeszkd umiejscowionych niej
i centralnie. Po napotkaniu przeszkody ROBOrobak wycofuje si
a nastpnie wykonuje obrt w ty,
po czym kontynuje przez jaki czas
jazd w przd lub pozostaje jaki
czas w bezruchu, wydajc charakterystyczne dwiki pomrukiwanie.
Czasy: jazdy, postoju, czstotliwoci pomrukiwania, ktw obrotu,
s czasami w okrelonym zakresie
wyznaczonymi przez generator liczb
losowych s, wic przypadkowe.
Przypadkowy jest te kierunek oraz
kt obrotu, jaki obiera ROBOrobak
po upywie czasu postoju. Zachowanie zabawki jest, wic w duym
stopniu nieprzewidywalne.
W trakcie postoju ROBOrobak
jest wraliwy jest na to, co si
z nim dzieje. Dotknicie czuka,
powoduje natychmiastow reakcj:
w wypadku czuka skrajnego wycofanie i obrt w ty, w wypadku
rodkowego tylko wycofanie. Po
parokrotnych tego rodzaju manewrach, zaniepokojony owad zamiera
na jaki czas w bezruchu.
Przemieszczenie ROBOrobaka
powodujce obrt jednego z k,
w trakcie postoju, wymusza wyjcie
z tego stanu i wznowienie ruchu
(ucieczk). Zarwno w trakcie ruchu
jak i postoju, ROBOrobak kontroluje

kontakt z podoem. Funkcj czujnika spenia przednie kko. Pierwszy

styk tego specycznego czujnika stanowi kocwka jego pionowej oki,
pracujcej w tulei, drugi paska
spryna znajdujca si nad ni.
Po podniesieniu zabawki z podoa,
oka opada w tulei pod wpywem
cikoci, tracc tym samym kontakt ze spryn i przerywajc obwd (odcinajc potencja masy od
ukadu sterowania). Po kilku sekundach, jeli stan ten utrzymuje
si ROBOrobak zamiera w bezruchu i nie wydaje dwikw. To zachowanie przeniosem na zabawk
ze wiata owadw. Niektre z nich
(np. biedronka siedmiokropka), niepokojone przez intruzw, posuwaj
si do zrcznych forteli udajc
martwe. Mao sensowne wydaje si
poza tym, by napdy pracoway,
gdy ROBOrobak znajduje si w powietrzu. Gdy ma to miejsce, po zamarciu zabawki, co jaki czas jednak, silniki uruchamiaj si, dajc
tym samym ROBOrobakowi moliwo wznowienia normalnej aktywnoci. Zdarzaj si, bowiem sytuacje, gdy przd zabawki podnosi
si w zetkniciu jej czci z jakimi
przeszkodami. Gdyby nie opisana
zdolno, ROBOrobak pozostawaby
w bezruchu w nieskoczono. A tak,
wycofujc si, po odzyskaniu kontaktu z podoem, nabiera pewnoci
siebie i wznawia swoj aktywno.
Na tor ruchu zabawki skada
si jazda w przd, ty oraz obroty
w prawo i lewo w ty. Zdarza si, e
ROBOrobak natraa na przeszkody,
ktrych nie jest w stanie zidentykowa za pomoc czuek. Co wtedy?
Ruch w przd oraz obroty kontrolowane s przez czujniki optycz-

27

ROBOrobak
ne. Jeli nie generuj one impulsw
odzwierciedlajcych obrt k, w wypadku ruchu do przodu nastpuje
zatrzymanie i wycofanie z obrotem
w ty, w wypadku obrotu nastpuje zatrzymanie. Potem ROBOrobak
porusza si do przodu lub pozostaje
jaki czas w bezruchu. Zdarzaj si
sytuacje, e ruch w ty, w zwizku
z powyszymi wypadkami, jest nie
wskazany (np. ROBOrobak wpada
w puapk: porusza si do przodu
i do tyu bez moliwoci obrotu).
Po kolejnym napotkaniu przeszkody,
w trakcie ruchu do przodu, ROBOrobak obrci si wic o 90o w prawo lub w lewo w ty, a nastpnie
ruszy do przodu. O podjciu tej akcji zadecyduje okrelona liczba nieudanych (nie wykonanych do koca) obrotw. Sytuacje takie mog
List. 1.

#includeHead.h
////////////////////////////////////
////////////////////////////////////
/////
int main(void)
{
io_init();
//inicjacja portw
extint_init(); //przerwa od encoderw k
timers_init(); //timerw
sei();
//zezw.glob.na przerwania
wdt_enable(WDTO_500MS);
//zezwolenie dla Watchdog Timer na timeout
= 0,5s
MovRotate = BackRight ;
while(1)
{
refSpeedMotR = refSpeedMotL =
setSpeed;
// nastawa prdkoci
TimerOper = RandGen(1,20);
//oblicz czas trwania ruchu w przd
TimerOper_FlagZero = 0;
//rozpocznij odliczanie
CommMoveRegister = Forward;
//komenda w przd
ControlMove();
//wysteruj
silniki
//////////////////////////////
///////// RUCH W PRZD
do
{
wdt_reset (); //kasuj
Watchdog Timer
if(ReactionFlag)
//reakcja na ag uaktywnion przez kontakt
z przeszkod
ReactionControl();
if(FlagEmergencyMove)
//reakcja na stan awaryjny
MoveEmerg();
ControlGround();
trola kontaktu z podoem

//kon-

}while(!TimerOper_FlagZero);
//ptla ,dopki nie minie czas ruchu
//////////////////////////////
///////// POSTJ
Stopover();
//zatrzymanie
count45 = RandGen(1,3);
//
oblicz czas po jakim ,pierwszy odgos
TimerOper = RandGen(2,10);
//oblicz czas trwania postoju
TimerOper_FlagZero = 0;
//rozpocznij odliczanie
do
{
wdt_reset (); //kasuj
Watchdog Timer
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~//
if(!feelerL)
//jeli
aktywny lewy czuek w czasie postoju
{
++counterIntruderMove;
reaktion(BackRight);
}

28

te mie miejsce w wypadku, gdy

ROBOrobak identykuje przeszkody
za pomoc czuek, lecz nie moe
parokrotnie, po wycofaniu, wykona obrotu w ty.
Wspomniane ju czujniki optyczne zapewniaj rwnie sta prdko ruchu zabawki, wpywajc
tym samym na to, e tor ruchu na
wprost lub w ty jest w miar (wizualnie) liniowy. Sterowanie prdkoci realizowane jest oczywicie
poprzez zmian wypenienia impulsw dostarczanych na uzwojenia
silnikw. Moe si ono zmienia
w zakresie 110. Czstotliwo pracy silnika wynosi 667 Hz.
Wspomniana funkcja (staa prdko) jest realizowana poprzez
kontrol liczby impulsw nadchodzcych z czujnikw optycznych
List. 1. cd
if(!feelerR)
//jeli
aktywny prawy czuek
reaktion(BackLeft);
if (!nose)
//jeli aktywny nos
{
FlagStopp = 0;
++counterIntruderMove;
BackMotion();
//ruch
wstecz
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
ControlGround();
//kontrola kontaktu z podoem

//odgos

if (!count45)
{
FlagStopp = 1;
Tickk(6,20000);

test_battery();
//kontrola poziomu napicia baterii
count45 = RandGen(1,8);
//oblicz czas po jakim ,kolejny odgos
w czasie postoju
}
if(FlagIntruderMove)
//jeli nastpi obrt koa w czasie
postoju wznowienie ruchu(ucieczka)
break;

if(counterIntruderMove == 4)
//jeli 3 krotna,w czasie postoju,
reakcja na uaktywnienie czuek
{
counterIntruderMove = 0;
Delay_ms(20000);
//stan
bezczynnoci
}
}while(!TimerOper_FlagZero);
//ptla ,dopki nie minie czas postoju
FlagIntruderMove = 0;
FlagStopp = 0;
if (randReg & 0x01)
//wybr
midzy :jazda na wprost lub obrt
{
angle_Imp = angle(20,60);
//obliczenie kta obrotu
if (MovRotate == BackLeft)
//jeli poprzednio ,nastpowa obrt
w lewo w ty
MovRotate = BackRight;
//zmiana na kierunek obrotu przeciwny
else
MovRotate = BackLeft;
CommMoveRegister = MovRotate;
//komenda:obrt zgodnie
z powyszym wysterowanie silnikw
Rotate();
}
}
}
/////////////////////////////////////
/////////////////////////////////////
/// END

w jednostce czasu. Zbyt dua,

rzeczywista prdko koa w stosunku do prdkoci zadanej, powoduje
zmniejszenie wypenienia impulsw
(zmniejszenie prdkoci koa) i na
odwrt.
Na uwag zasuguje to, w jaki
sposb, w wypadku zetknicia czuek z przeszkod (jazda w przd) wyhamowywane s silniki. Gwatowne
hamowanie odbywa si na skutek
krtkotrwaego ruchu wstecz (!).
Kontrola akumulatorkw, podczas
postoju, po kadym mrukniciu
ROBOrobaka wykonana jest poprzez
zliczenie czasu adowania kondensatora. Jeli czas jest zbyt dugi
(zbyt niskie napicie akumulatorka)
ROBOrobak wchodzi w stan bezczynnoci, sygnalizujc to za pomoc dwiku (dopominajc si o nakarmienie naadowanie)

Budowa

Ko n s t r u k c j n o n z a b a w k i
(rys. 1) stanowi plastikowa rama
(wykonana z pokrywki korytka od
kabli elektrycznych), do ktrej jest
przymocowana platforma gowy
wycity z plastiku gruby krek
podobnie jak koa opasane nastpnie gumkami.
Poniewa zabawka powica sporo czasu na poruszanie si, istnieje
potrzeba uoyskowania k. oyska z osiami oraz nasunitymi na
nie koami, wcinite s do plastikowych rurek a te nastpnie wklejone w boki ramy. W platform gowy
wcinita jest tuleja dla osi pionowej przedniego koa. Na platformie
umocowane s za pomoc zagitych
szpilek czujniki. S to miniaturowe
spryny stykowe (wymontowane
z mechanizmw magnetofonowych).
Trzy spryny zostay odpowiednio
sprzone z czukami wykonanymi
z rozwinitej czciowo linki stalowej (bardzo wane by czuki byy
spryste i odpowiednio elastyczne). Czwarty czujnik (do kontroli kontaktu z podoem) zosta ju
opisany. Od czujnikw do ukadu
sterowania biegn cienkie przewody odzyskane z uzwojenia anteny
ferrytowej.
Drug cz ROBOrobaka stanowi platforma z silnikami, akumulatorami i elektronik. Jest ona
przytwierdzona do przodu ramy
za porednictwem zawiasu. Kolejne
punkty jej kontaktu z doln czci
zabawki stanowi punkty oparcia
osi na koach. Sia docisku tych

Elektronika Praktyczna 8/2005

ROBOrobak
osi do k zwikszana jest przez
tyln spryn. Poza tym do tyu
platformy przylutowany jest koek,
ktry ustala jej pooenie wzgldem
ramy (koek z platform jest wzgldem ramy ruchomy).
W skad opisanej powyej platformy wchodz pytki drukowane.
Na dolnej pytce (stanowicej podstaw tej platformy) znajduje si
m.in. driver silnikw oraz od jej
spodu czujniki optyczne. Grna
pytka to ukad sterowania z procesorem. Pytka ta poczona jest

z doln za pomoc szpilek (najlepiej dugich goldpinw) wtykanych

w dwa gniazda.

Opis programu gwnego

Na pocztku programu gwnego (list. 1) nastpuje inicjacja portu, zewntrznych przerwa oraz
timerw cznie z watchdogiem.
Po ustawieniu prdkoci, generator
liczb losowych zbudowany w oparciu o rejestr przesuwny i Timer0,
generuje liczb, z ustalonego przez
programist zakresu, stanowic

Rys. 1. Budowa mechaniczna ROBOrobaka

Elektronika Praktyczna 8/2005

czas ruchu ROBOrobaka po prostej

(czas ten nie bdzie wyduany
przez czas ruchw, jakie wykona
na skutek kontaktu z przeszkodami).
Po rozpoczciu odliczania nastpuje
ustawienie wyj procesora w kierunku drivera silnikw i wejcie do
ptli ruchu.
Tutaj skanowane s cigle dwie
agi: detekcji przeszkody oraz ruchu awaryjnego (inicjowanego na
skutek parokrotnego zatrzymania
przez przeszkod podczas ruchu
w przd bez moliwoci penego
obrotu w ty). Nastpna czynno to
kontrola kontaktu
z podoem.
Ptla wykonywana jest do momentu ustawienia agi
(w programie obsugi timera) wskazujcej na to, i upyn czas ruchu. Po
wyjciu z ptli nastpuje wystawienie
odpowiednich sygnaw STOP w kierunku drivera. ROBOrobak zatrzymuje
si. Teraz obliczane s przy pomocy
wspomnianego generatora dwa czasy:
postoju oraz czas,
po ktrym nastpi
pierwsze mruknicie (o zakresie czasw rwnie decyduje programista).Po
rozpoczciu odliczania nastpuje wejcie w ptl postoju.
Teraz sprawdzane
s stany wej czujnikw sprzonych
z czukami i jeli
ktre bdzie ak tywne, wywoana
bdzie odpowiednia procedura reakcji. Po sprawdzeniu
kontaktu z podo em, jeli min
czas do pierwszego
mruknicia, nastpuje wygenerowanie
go, przeprowadzenie
kontroli baterii oraz
obliczenie przerwy
do nastpnego odg o s u . Te r a z s ka nowana jest aga

29

ROBOrobak
ustawiana w podprogramie obsugi
przerwa zewntrznych, a sygnalizujca prb przesuwania ROBOrobaka w czasie postoju. Jeli prba
taka miaa miejsce (nastpi obrt
koa) nastpuje wyjcie z ptli postoju. Jeli nie, sprawdzany jest
licznik reakcji na uaktywnianie
czuek w czasie postoju. Jeli reakcji takich byo kilka, nastpuje wejcie w ptl czasow ROBOrobak
wchodzi w stan bezczynnoci.
Po wyjciu z ptli postoju, jeli warto rejestru generatora jest
w danej chwili nieparzysta, po wygenerowaniu wartoci kta, nastpi
skok do podprogramu obrotu w ty
(kierunek obrotu bdzie przeciwny
do poprzedniego). Po wykonaniu
obrotu lub te nie, nastpuje przejcie do pocztku ptli gwnej.

Co si dzieje, gdy ROBOrobak,

podczas ruchu w przd,
napotyka czukiem na
przeszkod?

Co 125 ms nastpuje przepenienie timera. Jeli podprogram wykryje uaktywniony czuek, ustawia a-

30

g (jeli poprzednio nie bya ustawiona), po czym na 50 ms wysterowuje silniki na prac wstecz, nastpnie je zatrzymuje. Jeli program
gwny wykryje, e wspomniana
aga jest ustawiona, przechodzi do
podprogramu, w ktrym podejmuje
nastpujce dziaanie: jeli nie byo
trzech stanw awaryjnych, w zalenoci od tego, ktry czuek jest
aktualnie uaktywniony, przechodzi
do podprogramu reakcji z odpowiednim parametrem. Oznacza to ruch
w ty przez okrelony czas, obliczenie wartoci kta obrotu, ustawienie jego kierunku (w zalenoci od
wspomnianego parametru) i przejcie
do podprogramu obrotu.

Jak jest wykonywany

podprogram obrotu koa?

Na wstpie zerowane s liczniki k. W zalenoci od wartoci

rejestru kierunku, s wysterowywane odpowiednio silniki. Podprogram
wchodzi w ptl, w ktrej skanowana jest odpowiednia zmienna, ktrej
przypisywana jest warto odpowiedniego licznika (zaley to rwnie od

wartoci rejestru kierunku). Warto

wspomnianej zmiennej to odbicie
tego, co si dzieje z odpowiednim
koem. Stopie jego obrotu ledzony
jest poprzez podprogram przerwania
zewntrznego, odpowiedniego dla
danego koa. Ptla trwa bdzie tak
dugo (tym samym dany obrt), jak
warto zmiennej (tym samym warto licznika odpowiedniego koa)
bdzie mniejsza od obliczonego kta
obrotu lub do momentu, gdy wystpi stan awaryjny, wyniky z unieruchomienia danego koa. Po wyjciu
z ptli, jeli wystpi stan awaryjny,
nastpuje zwikszenie jego licznika.
Kolejny krok to zatrzymanie silnikw i wysterowanie ich we wczeniejszym podprogramie na prac
w przd, nastpnie powrt do programu gwnego, do ptli ruchu.
Reasumujc: po zetkniciu si
czuka z przeszkod, ROBOrobak
gwatownie zatrzymuje si (ruch
wstecz i stop), cofa si przez okrelony czas, zatrzymuje, wykonuje
obrt, zatrzymuje i wznawia ruch
w przd.
Grzegorz Sipiora

Elektronika Praktyczna 8/2005