VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ Diplomová práce

Transkript

VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ
Fakulta strojního inženýrství
Ústav mechaniky těles, mechatroniky a biomechaniky
Diplomová práce
Návrh a realizace autonomního robotu pro kategorii IEEE Micromouse
Diplomant: Tomáš Paseka
Vedoucí práce: Ing. Tomáš Marada, Ph.D.
Brno, květen 2006
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Abstrakt
Diplomová práce je zaměřena na návrh řídicího softwaru pro autonomního robota
třídy IEEE Micromouse. Jejím cílem je navrhnout a ve vhodném programovacím jazyce
implementovat řídící software pro jednotlivé moduly robotu, kterými jsou modul hlavního
CPU, modul senzorů a modul motorů.
Práce se zabývá řízením krokových motorů a odometrií robotu s diferenciálním
podvozkem. Dále měřením vzdálenosti pomocí senzorů pracujících na principu odrazu
infračerveného světla. Práce se také zabývá návrhem a realizací strategie na prohledávání a
mapování bludiště a nalezením optimální cesty bludištěm. Všechny navržené algoritmy
byly realizovaný v programovacím jazyce C.
-2-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Prohlášení
Prohlašuji, že jsem diplomovou práci na téma „Návrh a realizace autonomního
robotu pro kategorii IEEE Micromouse“ vypracoval samostatně bez cizí pomoci,
na základě rad a pokynů vedoucího diplomové práce. Vycházel jsem přitom ze svých
znalostí, odborných konzultací a literárních zdrojů.
…........................................
Tomáš Paseka
-3-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Poděkování
Na tomto místě bych rád poděkoval všem, kteří mi byli nápomocni při tvorbě
diplomové práce. Především bych rád poděkoval vedoucímu diplomové práce Ing. Tomáši
Maradovi Ph.D. za odbornou pomoc a cenné rady.
Dále bych rád poděkoval svým přátelům a především rodině za všestrannou
podporu po celou dobu mého studia.
-4-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
1 Obsah
2 Úvod...............................................................................................................7
2.1 Cíle práce....................................................................................................................7
2.2 O micromouse............................................................................................................7
2.3 Pravidla soutěže.........................................................................................................8
3 Současný stav problematiky ve světě........................................................12
3.1 Robot Edgar B..........................................................................................................12
3.2 Robot Maisie............................................................................................................12
3.3 Robot MITEE 7.......................................................................................................13
3.4 Robot MITEE 8.......................................................................................................13
3.5 Danger Mouse..........................................................................................................14
3.6 AIRAT 2....................................................................................................................14
4 Používané typy robotů v soutěži Micromouse.........................................16
4.1 Robot s diferenciálním podvozkem (differential drive).......................................16
4.2 Robot se synchronním podvozkem (synchro drive)..............................................17
4.3 Robot s tříkolovým uspořádáním (tricykle drive)................................................18
4.4 Robot s Ackermanovým podvozkem (Ackerman steering nebo kingpin
steering)..........................................................................................................................19
4.5 Podvozky se všesměrovými koly.............................................................................21
5 Robot vyvíjený na UAI FSI VUT v Brně..................................................23
5.1 Popis architektury...................................................................................................23
6 Komunikace mezi moduly.........................................................................25
6.1 Popis paketu.............................................................................................................25
6.2 Realizace komunikace pro mikrokontroléry ATMega.........................................26
6.2.1 Popis struktury CommInfo.................................................................................26
6.2.2 Odesílání paketu.................................................................................................29
6.2.3 Přijímání paketu.................................................................................................30
6.2.4 Timeout komunikace..........................................................................................32
6.2.5 Synchronizace....................................................................................................32
6.2.6 Odlišnosti v implementaci pro procesory s dvěma periferiemi USART...........32
6.3 Realizace komunikace pro PC................................................................................33
6.3.1 Popis třídy CSerialPort a CSerialBuffer............................................................34
6.3.2 Popis třídy CSerialPortMM a CPaket................................................................39
7 Modul Senzoriky.........................................................................................42
7.1 Vlastnosti senzoru GP2D120..................................................................................42
7.2 Zapojení senzoru GP2D120....................................................................................43
7.3 Software modulu senzorů........................................................................................43
7.3.1 Měření napětí.....................................................................................................44
7.3.2 Převod napětí na vzdálenost...............................................................................44
7.4 Popis všech paketů modulu.....................................................................................45
7.5 Testovací software na PC........................................................................................48
8 Modul Motoru.............................................................................................50
-5-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
8.1 Hardware motoru....................................................................................................50
8.2 Odometrie manévrů modulu..................................................................................51
8.2.1 Akcelerace motorů.............................................................................................52
8.2.2 Ujetí požadované vzdálenosti............................................................................52
8.2.3 Otočení na místě o zadaný úhel.........................................................................53
8.2.4 Zatáčka o 90˚ během jízdy.................................................................................54
8.3 Software modulu......................................................................................................56
8.3.1 Řízení krokových motorů..................................................................................56
8.3.2 Výpočet nové rychlosti a nastavení Output Compare registru .........................57
8.3.3 Hlídání ujeté vzdálenosti....................................................................................59
8.3.4 Řízení jízdy zatáčkou.........................................................................................59
8.5 Testovací software modulu pro PC.........................................................................65
9 Modul hlavního CPU..................................................................................66
9.1 Hardware..................................................................................................................66
9.2 Software....................................................................................................................66
9.2.1 Základní myšlenka softwaru..............................................................................66
9.2.2 Hlavní řídící proměnné modulu.........................................................................67
9.3 Základní funkce programu.....................................................................................68
9.3.1 Hlídání okamžiku začátku startu do zatáčky.....................................................69
9.3.2 Měření napájení.................................................................................................70
9.3.3 Regulace směru jízdy.........................................................................................71
9.3.4 Mapování bludiště..............................................................................................71
9.4 Řízení jízdy...............................................................................................................75
9.4.1 Prohledávání bludiště.........................................................................................75
9.4.2 Nalezení optimální cesty....................................................................................77
9.4.3 Řízení naplánované jízdy bludištěm..................................................................78
9.5.1 Usart 0................................................................................................................80
9.5.2 Usart 1................................................................................................................80
9.6 Testovací software na PC........................................................................................82
10 Závěr..........................................................................................................83
Použitá literatůra...........................................................................................84
Seznam příloh................................................................................................85
-6-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
2 Úvod
Diplomová je zaměřena na návrh a realizaci autonomního robotu pro kategorii
IEEE Micromouse, který je vyvíjený na Ústavu automatice a informatiky FSI VUT v Brně,
pro ověřování metod umělé inteligence. Při jeho návrhu byl kladen důraz na univerzální
a jednoduchou konstrukci a také na nejnižší cenu.
Robot je složen z několika samostatných modulů, a to z modulu hlavního CPU,
modulu senzorů a modulu motorů. Práce se zabývá návrhem a realizací komunikace mezi
jednotlivými moduly. Dále se zabývá návrhem řídícího softwaru pro každý modul,
odometrií robotu s diferenciálním podvozkem a řízením krokových motorů. Dále se zabývá
měřením vzdálenosti pomocí senzorů pracujících na principu odrazu infračerveného světla.
Další část práce je zaměřena na návrh algoritmů prohledávání a mapování bludiště
a nalezení optimální cesty v bludišti.
Všechny algoritmy jsou realizovány v programovacím jazyce C a jejich funkčnost
je experimentálně ověřena v bludišti.
2.1 Cíle práce
Tato práce si klade za cíl:
●
Ověřit funkčnost a případně navrhnout úpravy hardwarové části robotu.
●
Navrhnout a realizovat komunikaci mezi jednotlivými části robotu.
●
Navrhnout a realizovat software pro modul senzorů.
●
Navrhnout a realizovat software pro modul motorů.
●
Navrhnout a realizovat software pro modul hlavního CPU.
2.2 O micromouse
Micromouse je soutěž autonomních robotů, která probíhá od roku 1970. Princip
soutěže spočívá v prozkoumání bludiště a nalezení optimální trasy ze startovního bodu
do středu bludiště, v nejkratším možném čase. Provedení bludiště i robotu stanovují
pravidla soutěže, která jsou popsána níže.
-7-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
2.3 Pravidla soutěže
Tato kapitola vychází z [8].
Soutěžní úloha
Navrhnout a sestrojit mikropočítačem řízený autonomní robot, který dokáže projet
bludištěm v co nejkratším čase.
Bludiště
Bludiště se skládá ze sítě základních čtverců o velikosti 18 x 18 cm, maximální
rozměr bludiště je omezen na 16 x 16 základních čtverců. Stěny bludiště jsou 5 cm vysoké
a 1,2 cm tlusté a mají povolenou odchylku ±5%. Z toho plyne, že chodbičky bludiště jsou
16,8 cm široké. Vnější stěna uzavírá celé bludiště. Stěny bludiště jsou z boku bílé, horní
strana stěn je červená. Podlaha bludiště je ze dřeva nebo z podobného materiálu, natřená
matnou černou barvou. Povrch bočních stěn bludiště by měl odrážet infračervené světlo
a povrch podlahy by ho naopak měl pohlcovat.
Start se nachází v jednom ze čtyřech rohů bludiště. Ve středu bludiště je otevřená
část, tvořená čtyřmi základními čtverci. Tento centrální čtverec je cílem, vchod do tohoto
čtverce je jen jeden. Je možné, že do cíle povede více než jedna cesta.
V bludišti se dá použit pravidlo pravé respektive levé ruky, oba směry nemusí být
rovnocenné. Zároveň to ale není nejkratší cesta do cíle. Záměrem tohoto zjednodušení
je umožnit účast i začátečníkům, kteří se obvykle potřebují více soustředit na konstrukci
robotu než na algoritmus.
Celkové rozměry bludiště musí být dodržené s přesností 5% nebo 2 mm, vždy
menší z obou hodnot. Spoje na podlaze nesmí vytvářet schůdky větší než 1 mm. Změna
sklonu nesmí být větší než 4 stupně. Mezery mezi souvisejícími stěnami jsou menší
než 2 mm.
Robot - myš
Myš musí být autonomní. Nesmí používat zdroj energie, který využívá spalovací
proces. Délka a šířka nesmí překročit 25 cm. Pokud myš mění v průběhu jízdy svoje
rozměry, v žádném okamžiku nesmí překročit 25 x 25 cm, výška není omezená. Myš nesmí
během cesty bludištěm nic odhodit ani ztratit. Myš nesmí skákat, překračovat nebo lozit
po stěnách. Také nesmí žádnou svou činností poškodit bludiště.
-8-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Senzorika
Na typ a počet senzorů nejsou kladeny žádné speciální omezení, pokud neporušují
jiná pravidla. Soutěžící nesmí použít žádné vnější pomůcky pro zlepšení navigace
(nálepky, značky, zrcadla).
Elektronika
Žádná část robotu nesmí pracovat s napětím vyšším než 24V. Celková spotřeba
by neměla překročit víc než 20A. Vyjímky z tohoto pravidla povoluje porota.
Klimatické podmínky
Soutěž probíhá v běžných klimatických podmínkách (T = 270-310K, p = 90 –
120kPa, 0-90%RH). Není možné předem zaručit úroveň osvětlení, ale před každou soutěží
je vyhrazený čas, který soutěžící může využít na optimální nastavení citlivosti snímačů.
Hodnocení
Základní úlohou je dojet ze startovního čtverce do cílového. Tuto cestu nazveme
„pokus“ a čas, který zabere nazveme „čas pokusu“. Cesta zpět z cílového čtverce
do startovního se nepovažuje za pokus. Měří se i celkový čas strávený v bludišti, který je
pro každého soutěžícího omezený na 10 minut. Pokud bude myš v průběhu jízdy
vyžadovat zásah soutěžícího, bude to považováno za „dotknutí“.
Soutěžní čas myši v jednom pokusu se vypočítá jako součet času pokusu, 1/30
času stráveného v bludišti a třísekundové penalizace za jakékoliv dotknutí. Hodnotí
se pouze nejkratší soutěžní čas ze všech pokusů, který daná myš dosáhne.
Když myš dosáhne středu bludiště (cíl), může ji soutěžící zdvihnout a restartovat,
nebo se může samostatně vrátit na start. Zdvihnutí je samozřejmě považováno za dotknutí
a soutěžící dostane tří sekundovou penalizaci. Po dosáhnutí cíle může myš pokračovat
ve zkoumání bludiště a hledat optimální cestu.
Soutěžící nesmí mít při aktivaci myši možnost volit ani ovlivnit strategii.
Po odkrytí bludiště ani při restartu nesmí vložit myši žádnou informaci.
Čas pokusu bude naměřený od okamžiku, kdy myš opustí startovní čtverec,
po okamžik, kdy vjede do cílového čtverce. Celkový čas bludiště bude měřený
od okamžiku aktivování myši. Myš se nemusí začít okamžitě po aktivaci pohybovat, ale
musí být položená do startovního čtverce a připravená na pokus. Pokud se myš vrátí
na start bez dosáhnutí cíle, tento pokus se zastaví a po opakovaném startu začne měření
dalšího pokusu.
-9-
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Čas potřebný na průzkum bludiště bude měřený buď ručně organizátorem, nebo
infračervenými senzory na startu a v cíli. Infračervený paprsek bude vodorovný, na hranici
mezi prvním a druhým čtvercem a při vchodu do cílového čtverce cca 10 mm
nad podlahou.
Pokud myš přestane správně fungovat, může soutěžící požádat porotu o možnost
zrušit pokus a restartovat myš, ne však v případě že myš udělala špatný obrat, nebo
se vydala špatnou cestou.
Pokud soutěžící v průběhu soutěže vymění libovolnou část myši (baterie,
EPROM, atd.), nebo provede jinou podstatnou úpravu, musí vymazat všechny informace
o bludišti, které myš do té doby získala. Malé úpravy (např. nastavení citlivosti snímačů)
jsou povolené pod dohledem poroty. Úpravy rychlosti nebo strategie jsou bez vymazání
informaci o bludišti zakázané.
Žádná část myši s výjimkou baterií nesmí být použita na jinou myš. Například
pokud je použito jedno šasi a dva různé řídící obvody, potom se považují za jednu myš
a nesmí překročit 10 minutový čas bludiště. Před výměnou řídící jednotky je nutno
vymazat paměť. Myš nesmí být v průběhu soutěže odlehčována, např. odstraněním
nepotřebných senzorů po prozkoumání bludiště.
Mezi dokončeným pokusem a startem dalšího pokusu musí myš zůstat alespoň
1 sekundu na startovním čtverci. Během této sekundy nesmí robot zakrývat infrasenzor
pro měření času pokusu.
Hodnocení
Vítězem se stane robot s nejnižším dosaženým soutěžním časem. Pokud
v průběhu soutěže nedosáhne žádná myš cíle, porota určí vítěze na základě celkové
úspěšnosti, např. jak blízko k cíli se podařilo dostat, nebo jestli byl pohyb v bludišti
koordinovaný nebo náhodný apod.
Diskvalifikace
Všeobecně platí, že při každém porušení předpisů je robot vyloučený ze soutěže.
To platí zejména v těchto situacích:
•
nebezpečné chování
•
poškození dráhy
- 10 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Bezpečnost
Robot musí dodržovat tři zákony robotiky:
1. Robot nesmí ublížit člověku, nebo svojí nečinností dopustit, aby bylo člověku
ublíženo.
2. Robot musí poslechnout příkaz člověka, s vyjímkou pokud je příkaz v rozporu
s prvním zákonem.
3. Robot musí chránit sám sebe před zničením, s vyjímkou rozporu s prvním
a druhým zákonem.
- 11 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
3 Současný stav problematiky ve světě
Tato kapitola vychází z [9] a [10].
3.1 Robot Edgar B
Edgar je mikromyš postavená Petem
Harrisonem. Myš má klasický dvoukolový
podvozek, který je poháněný dvojicí krokových
motorů, které jsou řízeny relativně výkoným
procesorem 80C196.
V současné době je myš vybavena
třemi detektory pro měření vzdálenosti, které
jsou umístěny po jednom na každé straně
Obr. 3.1: Robot Edgar
a jeden vpředu. Senzory jsou schopny detekovat
zeď v rozmezí 2 – 13cm. Senzory jsou závislé na odrazu aktuálního povrchu a proto musí
byt zkalibrované pro každé bludiště.
3.2 Robot Maisie
Obr. 3.3: Robot Maisie
Obr. 3.2: Robot Maisie
Masie je robot, kterého postavil C. Kanesalingham z Univerzity of East London.
Masie je od počátku velmi úspěšným robotem v soutěži, který nevyniká vysokými
rychlostmi, ale vysokou spolehlivostí.
Masie je založena na diferenciálním podvozku, který je osazen dvěma krokovými
motory. Pro spolehlivou detekci zdí, autor vybavil myš šesti infračervenými snímači, které
jsou umístěný po dvou na každém z bočních ramen a dvě vpředu a dvěma mikrospínači pro
- 12 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
detekci kolizí. Velkou zvláštností je použití olověné baterie, která je umístěna vzadu nad
motory, díky niž robot vyniká velkou hmotností.
Během soutěže Masie jednoduše jezdí podél zdí, dokud se nedostane do středu
bludiště. Tím často poráží své podstatně sofistikovanější soupeře, kteří se často ztratí nebo
uváznou.
3.3 Robot MITEE 7
MITEE 7 je robot kterého postavil
Dave
Otten's
z Massachusetts
a
Institute
Tony
Caloggero
of
Technology.
Podvozek robotu je tvořen čtyřmi poháněnými
a zároveň řiditelnými koly, konstrukce je dobře
vidět na Obr. 3.4.
Toto řešení je poněkud složitější
než diferenciální nebo tříkolové uspořádání,
ale přináší sebou několik výhod. Při akceleraci
Obr. 3.4: MITEE 7
je váha robotu přenášena na zadní nápravu, to u dvoukolového uspořádání vede
k odlehčení nápravy a tím k možnému prokluzu, u čtyřkolového uspořádání se všemi
hnanými koly je možnost prokluzu minimální. Další výhodou je natáčení všech čtyř kol,
díky němuž je myš schopna projíždět zatáčky o velmi malém poloměru poměrně vysokou
rychlostí.
Robot je vybaven osmi DC motory, čtyři slouží k pohonu kol a zbylé čtyři k jejich
natáčení. Pro orientaci v bludišti jsou použity infračervené senzory pro měření vzdálenosti
v každém směru.
3.4 Robot MITEE 8
MITEE
8
je
další
robot
vyvinutý
na Massachusetts Institute of Technology stejnými
autory jako MITEE 7, ale na rozdíl od svého
předchůdce má klasické dvoukolové diferenciální
uspořádání.
Myš je poháněna dvěma DC motory Obr. 3.5: MITEE 8
- 13 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
se šesti 225mAh NiCd akumulátory, díky ním je hmotnost myši jen 200g. Pro určování
polohy v bludišti je myš vybavena infračervenými senzory pro měření vzdálenosti.
Na spodní straně je umístěn inkrementální čítač, který je spojený s koly robotu a vytváří
tak zpětnou vazbu pro řízení motorů.
3.5 Danger Mouse
DangerMouse je robot, kterého postavil Finbarr Mc Carthy student University
of Limerick.
Podvozek
má
klasické
dvoukolové uspořádání a pro pohon jsou
použity dva krokové motory typu H546,
které jsou použity u komerční mikromyši
AIRAT. Pro řízení motorů je použito obvodů
ULN5804.
Pro orientaci v bludišti je myš vybavena
infračervenými snímači QRB1134 od firmy
Obr. 3.6: DangerMouse
Fairchild Semiconductor, které jsou umístěny
na dvojici ramen, vždy čtyři senzory na jednom rameni. Další senzor je umístěn vpředu,
pro měření vzdálenosti ke zdi. Myš je osazena procesorem Analog Devices ADuC831.
3.6 AIRAT 2
Robot AIRAT 2 je komerčně prodávaná
mikromyš, velice pokročilé konstrukce. Robot
je navržen jako nástroj pro výzkum metod
umělé inteligence. Myš má standardizované
rozměry, které stanovil Institute of Electricial
and Electronic Engineers (IEEE).
Robot je postaven na diferenciálním
podvozku, který je vyroben z hliníku a je
poháněn dvojicí krokových motorů typu H546. Obr. 3.7: Robot AIRAT 2
Myš je vybavena procesorem AT89C51 od firmy Atmel, který běží na frekvenci
22,11MHz, procesor disponuje 16KB flash paměti. Pro orientaci v bludišti myš používá
šest infračervených senzorů, dva směřují dopředu, dva do stran pod úhlem 90° a dva také
do stran, ale pod úhlem 45°. Senzory umístěné pod úhlem 45° umožňují spolehlivou jízdu
- 14 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
diagonálním směrem. Pro komunikaci s uživatelem je myš vybavena LCD displayem
a třemi ovládacími tlačítky.
K robotu je dodáváno CD se simulačním programem, jak pro simulování různých
operací myši tak pro simulování jízdy bludištěm.
- 15 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
4 Používané typy robotů v soutěži Micromouse
Tato kapitola vychází z [3] a [11]
I když pravidla soutěže přímo neurčují typ podvozku robotů, v soutěži
se používají výhradně kolové podvozky. Je to především díky jejich snadné mechanické
konstrukci, snadného řízení a tím pádem finančně nejméně náročné. Z tohoto důvodu
se dále budu zabývat pouze kolovými podvozky.
4.1 Robot s diferenciálním podvozkem (differential drive)
Jedná se patrně o nejjednodušší a nejpoužívanější typ podvozku pro mobilní
roboty, který je často používán u malých a levných strojů. Je to asi nejoblíbenější řešení
u amatérů
a nadšenců,
především
díky
jednoduché
konstrukci a snadnému řízení. Schématické uspořádání je
dobře vidět na obrázku Obr. 4.1
Jak je vidět na Obr. 4.1 podvozek obsahuje dvě
aktivní kola s jedním stupněm volnosti. Jako pohon jsou
nejčastěji používány malé stejnosměrné motory s enkodéry
Obr. 4.1: Schématický nákres
diferenciálního podvozku
či
krokové
motory.
mechanické
Při
konstrukci
nesmí být opomenuty opěrné body, nejčastěji třecí elementy
nebo kola s jedním čí lépe se dvěma stupni volnosti.
Mezi největší výhody diferenciálního podvozku
patří kromě jednoduché a robustní konstrukce a nízké ceny
také velmi jednoduchá a přesná odometrie pro navigaci
robotu. Tato výhoda je často používána i u poměrně
sofistikovaných robotů, kde je však odometrie doplňována
nějakou
další
metodou
bez
chyby
s
integračním
charakterem. Další velkou výhodou tohoto podvozku je
velmi dobrá manévrovatelnost. Robot s diferenciálním
podvozkem je schopen otočit se na místě, tato vlastnost
bývá ještě posílena tím, že roboty s diferenciálním
Obr. 4.2: Model diferenciálního
podvozkem mívají válcovitý tvar, což eliminuje možnost robotu
uvíznutí robotu v rozích místnosti a podobně.
- 16 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Odometrie robotu s diferenciálním podvozkem
Základem řízení diferenciálního robotu je rozdíl rychlosti jednotlivých kol. Pokud
se obě kola otáčí stejnou rychlostí a stejným směrem, robot jede rovně a naopak, pokud
se kola otáčí opačným směrem, robot se otáčí na místě kolem středu nápravy, tzv.
referenční bod.
Pokud je rozdíl rychlostí kol nenulový a obě nápravy se točí stejným směrem,
robot se pohybuje po kružnici, jejíž poloměr je dán poměrem rychlostí obou kol. Vše je
popsáno následujícími rovnicemi.
Poloměr kružnice zatáčky je dán Rov. 4.1, kde je R – poloměr zatáčky [m], b –
rozchod robotu [m], vl – rychlost levého kola [m/s], vr – rychlost pravého kola [m/s].
b v l v r 
R= ⋅
2 v l −v r 
Rov. 4.1
Úhel natočení robotu je dán Rov. 4.2, kde je φ – úhel natočení robotu [rad], Δsl –
dráha ujetá levým kolem [m], Δsr – dráha ujetá pravým kolem [m].
=
 s l  s r 
2
Rov. 4.2
Dráha ujetá robotem je popsána Rov. 4.3, kde je s – ujetá dráha [m], sl – dráha
ujetá levým kolem, sr – dráha ujetá pravým kolem [m].
s=
 sl s r 
2
Rov. 4.3
Rychlost referenčního bodu je vyjádřena rovnicí Rov. 4.4, kde je vref – rychlost
referenčního bodu [m/s], vl – rychlost levého kola [m/s], vr – rychlost pravého kola [m/s].
v ref =
v l v r 
2
Rov. 4.4
4.2 Robot se synchronním podvozkem (synchro drive)
U synchronního podvozku má každé kolo dva stupně volnosti. Typická
konfigurace tohoto podvozku obsahuje tři kola uspořádaná do tvaru rovnostranného
trojúhelníku. Tvar robotu bývá obvykle válcový, uspořádání je znázorněno na Obr. 4.3.
Všechna kola se vždy otáčí stejným směrem a stejnou rychlostí a míří také vždy na stejnou
stranu.
- 17 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Synchronizace pohybů jednotlivých kol lze
dosáhnout buď elektronicky pomocí vhodně navrženého
senzoricko-regulačního systému, nebo častěji pomocí
vhodného
mechanického
Pochopitelnou
nevýhodou
v
uspořádání
podvozku.
případě
elektronické
synchronizace je potřeba dvou motorů na každé kolo.
Při mechanickém provázání pohybů kol je sice složitější
synchronního podvozku
mechanická konstrukce, postačí však pouze dva motory
pro libovolný počet kol.
Odometrie se synchronním podvozkem
Odometrie robotu se synchronním podvozkem je velice jednoduchá, protože
natočení všech kol je stejné, je i stejná jejich ujetá vzdálenost, vše je popsáno rovnicemi
Rov. 4.5 a Rov. 4.6, kde je βi – úhel natočení i-tého kola [°], Δβ – změna orientace [°], Δd i –
vzdálenost uhetá i-tým kolem [m], Δd – celková ujetá vzdálenost[m].
1=2=3= 
Rov. 4.5
 d 1= d 2= d 3= d
Rov. 4.6
4.3 Robot s tříkolovým uspořádáním (tricykle drive)
U tohoto podvozku jsou zpravidla hnaná zadní
kola a přední kolo je motoricky pouze natáčené, zatímco
ve směru pohybu je obvykle pasivně odvalováno.
Výhodou podvozku je jednoduché řízení, opět ovládáme
jedním motorem směr pohybu a druhým motorem
rychlost jízdy. Typické uspořádání je znázorněno na Obr.
4.4
Tento podvozek je již možno používat v těžším
tříkolového podvozku
terénu a navíc umožňuje použití poměrně jednoduché
odometrie. Je však nutné počítat s poměrně značnou
chybou odometrického měření v případě pohybu v terénu, nárůst chyby způsobí i použití
nafukovacích pneumatik. Další nevýhodou tříkolového podvozku je, že není možná rotace
na místě, jako u předchozích typů. Hrozí tedy uvíznutí robotu v úzkých a komplikovaných
prostorách, rovněž autonomní řízení v takových prostorách představuje komplikovanou
úlohu.
- 18 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Odometrie robotu s tříkolovým uspořádáním
V závislosti na natočení řiditelného kolečka se tříkolka pohybuje buď po přímce
nebo po kružnici, pro zjištění pozice je
třeba dvou údajů – orientace řiditelného
kolečka a jeho ujetá vzdálenost. Nechť
je referenčním
bodem
střed
nápravy
pevných kol. Předpokládáme-li nenulové
natočení řídícího kolečka a tedy pohyb
po kružnici, bude střed této kružnice ležet
na ose hnaných kol, jak je vidět na Obr.
Obr. 4.5: Model tříkolového podvozku
4.5.
Poloměr kružnice je dán rovnicí Rov. 4.7, kde je R – poloměr kružnice [m], l –
vzdálenost náprav [m], ω -úhel natočení řídícího kolečka [˚].
R=
l
tan 
Rov. 4.7
Úhel, který tříkolka urazí po kružnici, a který bude zároveň odpovídat i změně její
orientace, se spočte podle Rov. 4.8, kde je φ – změna orientace [˚], dF – vzdálenost ujetá
řiditelným kolečkem [m], ω – úhel natočení řídicího kolečka [˚], R – poloměr zatáčky [m].
=
dF⋅cos 
R
Rov. 4.8
Změna pozice referenčního bodu je dána Rov. 4.9, kde je dx – změna pozice v ose
x [m], dy – změna pozice v ose y [m], φ – změna orientace [˚].
dx= R− R⋅cos  , dy =R⋅sin  
Rov. 4.9
4.4 Robot s Ackermanovým podvozkem (Ackerman steering nebo
kingpin steering)
Jde o typ známý z automobilů. V robotice bývá nejčastější uspořádání, kdy jsou
hnaná pouze zadní kola a přední jsou natáčena každé jiným úhlem, protože každé kolo
opisuje kružnici o jiném poloměru. Schématický nákres je na Obr. 4.6. Je možno použít
i podvozek se stejně natáčenými koly, je však nutno počítat s většími prokluzy při zatáčení.
Problémy se objeví zejména při vyšších rychlostech robotu.
- 19 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Ackermanův podvozek bývá používán zejména
u větších vozidel u kterých se předpokládá činnost
na běžných silnicích s požadovanou velkou nosností nebo
činnost v těžším terénu. I zde je možno použít odometrii,
v praxi však její použití nebývá příliš běžné. Nevýhodou
tohoto podvozku je nemožnost otáčky na místě.
Odometrie robotu s Ackermanovým podvozkem
Pokud má být střed otáčení dobře definován (tj. Obr. 4.6: Schématický nákres
žádné kolečko nebude ve smyku), je nutné, aby vnitřní Ackermanova podvozku
kolo zatáčelo více než vnější. Úhly natočení kol popisují rovnice Rov. 4.10 pro vnější kola
a Rov. 4.11 pro vnitřní kolo, kde je βin – úhel natočení vnitřního kola [°], βout – úhel
natočení vnějšího kola [°], R – poloměr zatáčky [m], l – vzdálenost náprav [m], b –
rozchod robotu [m].
l
R
Rov. 4.10
l
R−b
Rov. 4.11
tan out =
tan in =
Pokud nahradíme přední kola tzv.
virtuálním kolem, můžeme použít odometrii
pro tříkolový podvozek. Virtuální kolo je
umístěno ve středu přední nápravy (viz Obr.
4.7), jeho úhel natočení je závislý na natočení
kol přední nápravy a spočte se podle rovnice
Rov. 4.12, kde je βvr – úhel natočení virtuálního
kola [°], βin – úhel natočení vnitřního kola [°],
Obr. 4.7: Model Ackermanova řízení
βout – úhel natočení vnějšího kola [°], b –
rozchod robotu [m], l – vzdálenost náprav [m].
cotan vr =cotan out −
b
b
=cotanin 
2⋅l
2⋅l
- 20 -
Rov. 4.12
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
4.5 Podvozky se všesměrovými koly
Obr. 4.9: Všesměrové kolo
Obr. 4.8: Schéma robotu s
všesměrovými koly
Jako alternativa k běžně používaným kolům vznikla tzv. všesměrová či složená
kola, která umožňují pohyb ve dvou osách. Při vhodném použití takovýchto kol, je možno
zkonstruovat tzv. všesměrové mobilní platformy. Všesměrová kola jsou v zásadě běžná
kola, která mají na svém obvodu řadu pasivních válečků (viz Obr. 4.9). Kdyby byly dané
válečky zablokovány chovalo by se takové kolo jako běžné kolo s jedním stupněm
volnosti. Kdyby byla naopak zablokovaná hlavní osa kola, kolo by se mohlo pohybovat
pouze v kolmém směru. Kombinací těchto mechanizmů a vhodným uspořádáním
podvozku vzhledem k danému typu kol, je možno
dosáhnout
libovolného
pohybu
podvozku,
tj.
podvozek se může pohybovat jedním libovolným
směrem a libovolně rotovat, případně vykonávat oba
tyto pohyby současně. Každé kolo je poháněno
jedním motorem, ale celkově je nutné použít alespoň
tři motory. Jde o obecný princip – chceme-li
při pohybu
měnit
tři
prostorové
souřadnice,
potřebujeme k tomu alespoň tři zdroje pohybu.
Odometrie podvozku s všesměrovými koly
Obr. 4.10: Model všesměrového podvozku
Informace potřebné k výpočtu změny pozice obsahují jednotlivé enkodéry
a vzdálenost kol od středu robotu. Potom změnu pozice lze vypočítat ze soustavy rovnic
Rov. 4.13, kde je di – ujetá vzdálenost i-tého kola [m], dx – změna polohy v ose x, dy –
změna polohy v ose y, φ -změna úhlu natočení robotu [°], R – vzdálenost kola od středu
robotu [m].
- 21 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
d 1=d x ⋅R
d 2=−d x⋅sin 30d y⋅cos30⋅R
d 3 =−d x⋅sin 30−d y⋅cos 30⋅R
Rov. 4.13
Analogicky lze spočítat rychlost pole soustavy rovnic Rov. 4.14, kde je vi –
rychlost i-tého kola [m], vx – změna rychlosti v ose x, vy – změna rychlosti v ose y,
φ - změna úhlu natočení robotu [°], R – vzdálenost kola od středu robotu [m].
v 1=v x ⋅R
v 2 =−v x⋅sin 30v y⋅cos30⋅R
v 3=−v x⋅sin 30−v y⋅cos 30⋅R
- 22 -
Rov. 4.14
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
5 Robot vyvíjený na UAI FSI VUT v Brně
Jak již bylo popsáno výše, robot slouží k ověřování metod umělé inteligence.
Robot je navržen tak, aby ho bylo možno použít v soutěži kategorie IEEE Micromouse,
případně po výměně desky senzorů v kategorii Path-Follower. Při návrhu robotu byl
největší důraz kladem na univerzální a jednoduchou konstrukci a co nejnižší cenu.
Obr. 5.2: Pohled na robot zepředu
Obr. 5.1: Pohled na robot zezadu
5.1 Popis architektury
Robot je postaven na diferenciálním podvozku, pro pohon je použita dvojice
krokových motorů. Změna směru pohybu je realizována různou rychlostí otáčení kol. Aby
se robot nepřevrátil, je vybaven dvěma kulovými podpěrami. Základní technické parametry
robotu jsou v Tab 5.1.
Parametr
Hodnota
Podvozek
Diferenciální
Dvě hnaná kola
Dvě podpěry
Pohon
2x krokový motor TEAC KP39HM2-025
Senzory
3x senzor SHARP GP2D120
Napájení
4x akumulátor LiIon CGR18650/4,2V
Komunikace
2x RS323/TTL/38400Bd
Délka
130mm
Šiřka
105mm
Výška
80mm
Rozteč kol
95mm
- 23 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
Parametr
Hodnota
Hmotnost
990g
TOMÁŠ PASEKA
Tab 5.1: Základní technické parametry robotu
Řídící systém se skládá z jednotlivých modulů, které spolu komunikují
po společné sběrnici. Díky tomuto řešení lze snadno modifikovat robot pro danou úlohu.
V současné době je robot vybaven třemi základními moduly a to modul hlavního CPU,
modul senzoriky, modul řízení motorů, dále je možno připojit modul displeje a tlačítek,
pro zobrazování informací a nastavování parametrů robotu. Do budoucna se plánuje ještě
modul bezdrátového přenosu dat mezi robotem a PC, pro ověřování algoritmů umělé
inteligence, které vyžadují výkonnější CPU. Blokové schéma robotu je na Obr. 5.3
Moduly používají mikrokontroléry ATMega od firmy Atmel. Jedná se o procesory
s osmi bitovou architekturou, které mohou pracovat v rozsahu frekvencí 1-16MHz.
Tyto mikrokontroléry jsou dobrým kompromisem mezi výkonem a cenou, jsou k dostání
v mnoha provedeních, které se liší velikostí programové paměti a počtem periferií.
Proto je možné zvolit vždy optimální procesor pro daný modul.
Obr. 5.3: Blokové schéma robotu
Napájení všech modulů je zajištěno čtyřmi články LiIon CGR18650/4,2V
zapojených sériově, výsledné napětí je tedy cca. 16,8V. Napětí z baterií je přímo připojeno
do modulu hlavního CPU, který ho ještě stabilizuje na napětí 3V a 5V a rozvádí
do ostatních modulů pomocí jednotného konektoru.
- 24 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
6 Komunikace mezi moduly
Komunikace mezi moduly je založena na posílání paketů pomocí rozhraní
USART, které obsahují procesory ATMega (viz [5] a [6]). Jedná se o rozhraní pro sériový
přenos dat. Toto rozhraní lze snadno připojit na port RS232. Pro připojení stačí pouze
napěťový převodník, který převádí 5V logiku na 12V.
Architektura jednotlivých modulů je typu master/slave. Modul hlavního CPU
je typu master a všechny ostatní moduly (modul senzoriky, modul řízení motorů a modul
tlačítek a displeje) jsou typu slave. Drobnou nevýhodou této architektury může být fakt,
že moduly typu slave spolu nemohou komunikovat přímo, ale veškerá komunikace musí
nejprve směřovat do modulu master a teprve odtud do modulu slave. To může neúměrně
vytěžovat linku i CPU mastera. Protože ale většina komunikace v našem případě probíhá
jen mezi mastrem (modul hlavního CPU) a slavem (ostatní moduly), je pro náš případ
vyhovující.
6.1 Popis paketu
Pakety jsou složeny ze čtyř hlavních částí:
•
startovní byty
•
hlavička
•
data
•
CRC součet
Jsou použity dva startovní byty, jako rozumný kompromis mezi pravděpodobností
výskytu a jejich velikostí. Hlavička obsahuje adresu příjemce, adresu odesílatele, velikost
přenášených dat a typ dat. Každá z těchto informací má velikost 1 byt, tudíž hlavička
má celkem 4 byty. Za hlavičkou již přímo následují posílaná data. Konec paketu uzavírá
kontrolní součet o velikosti 2 byty, slouží k odhalení případné chyby v paketu. Schéma
paketu je na Obr. 6.1
Obr. 6.1: Schéma paketu
- 25 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Celková velikost paketu včetně startovních bytů, hlavičky a CRC součtu je
omezena na 256 bytů. Při velikosti paketů je nutno brát v úvahu velikost paměti
jednotlivých mikrokontrolérů, protože většina modulů používá ATMegu 8, která
má jen 1KB SRAM paměti.
6.2 Realizace komunikace pro mikrokontroléry ATMega
Komunikace je nezávislá na konkrétní aplikaci, její kód se nachází
v samostatných souborech, které stačí vždy přilinkovat k aplikaci. Komunikace využívá
dvě přerušení USART Recieve Complete a USART Transmit Complete (viz [5] a [6]).
Komunikace je řízena strukturou CommInfo, která je globální proměnou celé
aplikace. Struktura uchovává všechny potřebné informace a statistiku komunikace.
Kód komunikace je obsažen v souborech:
•
„Communic.h“ - hlavičkový soubor pro mikrokontroléry s jednou periferií
USART (např. ATMega 8, ATMega 16, ATMega 32)
•
„Communic.c“ - implementační soubor pro mikrokontroléry s jednou periferií
USART
•
„Communic2.h“ - hlavičkový soubor mikrokontroléry se dvěma periferiemi
USART (např. ATMega 64, ATMega 128)
•
„Communic2.c“ - hlavičkový soubor pro mikrokontroléry se dvěma periferiemi
USART
6.2.1 Popis struktury CommInfo
Struktura je deklarována takto:
struct CommInfo
{
volatile ComState_t
pData_t
pData_t
Size_t
Size_t
Adr_t
Adr_t
Adr_t
DataType_t
RecComplete_t
SendComplete_t
uint16_t
ComState_t
uint8_t
State;
pBuf;
pRecBuf;
nBytes;
nTotalBytes;
nCPUAdr;
nAdr;
nLastSender;
nLastType;
pfRecComplete;
pfSendComplete;
iLastCRC;
nTimeOutState;
nCRCCount;
- 26 -
ÚMTMB VUT V BRNĚ
uint8_t
volatile uint8_t
};
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
nTimeOutCount;
nSynchro;
State
Proměnná je typu uint8_t, popisuje stav komunikace. Pomocí této proměnné
se řídí příjímání a odesílání paketu viz kap. 6.2.2 a 6.2.3. Může nabývat hodnot:
•
COM_ST_READY – linka je volná, neprobíhá příjem ani odesílání paketu
•
COM_ST_RECIEVING – probíhá příjem dat
•
COM_ST_TRANSMITING – probíhá odesílání dat
•
COM_ST_WAIT_SEC_ST – příjem druhého startovního bytu
•
COM_ST_WAIT_REC_ADR – příjem adresy příjemce paketu
•
COM_ST_WAIT_SEN_ADR – příjem adresy odesílatele
•
COM_ST_WAIT_SIZE – příjem bytu reprezentující velikost dat, která budou
poslána
•
COM_ST_WAIT_TYPE – příjem bytu reprezentující typ dat
•
COM_ST_REC_PROCESS – zpracování dat
•
COM_ST_REC_CRC1 – příjem dolního bytu CRC součtu
•
COM_ST_REC_CRC2 – příjem horního bytu CRC součtu
pBuf
Proměnná typu char*. Dynamicky alokovaný buffer pro odesílaný paket. Alokuje
se při volaní SendData(...) viz kap. 6.2.2 a uvolňuje se po odeslání paketu.
pRecBuf
Proměnná typu char*. Dynamicky alokovaný buffer pro přijímaný paket. Alokuje
se při začátku příjmu bytu reprezentujícího velikost přijímaných dat a uvolňuje
se po návratu z funkce pfRecComplete, viz kap. 6.2.3.
nBytes
Proměnná typu uint8_t. Počet aktuálně přijatých nebo odeslaných bytů.
nTotalBytes
Proměnná typu uint8_t. Celkový počet bytů pro odeslání nebo přijímání.
nCPUAdr
Proměnná typu uint8_t. Adresa mikrokontroléru.
- 27 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
nAdr
Proměnná typu uint8_t. Proměnná pro dočasné uložení adresy příjemce paketu,
viz kap. 6.2.3.
nLastSender
Proměnná typu uint8_t. Proměnná pro dočasné uložení adresy odesílatele,
viz kap. 6.2.3.
nLastType
Proměnná typu uint8_t. Proměnná pro dočasné uložení typu paketu,
viz kap. 6.2.3.
pfRecComplete
Ukazatel na funkci, která se má vykonat po úspěšném přijetí paketu,
viz kap. 6.2.3. Funkce má prototyp:
void _název_(Adr_t From, DataType_t Type, pData_t pData, Size_t Size);
pfSendComplete
Ukazatel na funkci, která se má vykonat po úspěšném odeslaní paketu,
viz kap. 6.2.2 Funkce má prototyp:
void _název_(void);
iLastCRC
Proměnná typu uint16_t. Proměnná pro dočasné uložení CRC součtu,
viz kap. 6.2.3.
nTimeOutState
Proměnná typu uint8. Proměnná pro detekci timeoutů v průběhu komunikace.
nCRCCount
Proměnná typu uint8_t. Počet CRC chyb v průběhu celé komunikace.
nTimeOutCount
Proměnná typu uint8_t. Počet timeoutů v průběhu celé komunikace.
nSynchro
Proměnná typu uint8_t. Proměnná pro potřeby synchronizace komunikace,
viz kap. 6.2.5.
- 28 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Nastavení komunikační struktury lze provést pomoci funkce:
void SetCommInfoStruct(Adr_t Adr, RecComplete_t pfRecComplete, SendComplete_t,
pfSendComplete);
kde:
•
Adr – adresa mikrokontroléru
•
pfRecComplete – ukazatel na funkci, která se má volat po úspěšném přijetí paketu
•
pfSendComplete – ukazatel na funkci, která se má volat po úspěšném odeslání
paketu
Pozn. Tuto funkci je nejlepší volat na začátku běhu programu.
6.2.2 Odesílání paketu
Odesílání je založeno na přerušení USART Transmit Complete (viz [5] a [6]), které
nastává vždy po odeslání registru UDR. Odesílání paketu zajišťuje funkce:
void SendData(Adr_t Rec, DataType_t Type, pData_t pData, Size_t Size, uint8_t wait);
kde:
•
Rec - adresa příjemce paketu
•
Type – typ paketu
•
pData – ukazatel na buffer obsahující data, která mají být poslána
•
Size – velikost posílaných dat v bytech
wait – pokud je parametr roven 1, funkce čeká dokud nedojde k odeslání
předchozího paketu. Pokud se nerovná 1, funkce skončí aniž by paket byl odeslán
•
Funkce SendData() nejdříve otestuje, zda neprobíhá jiný přenos pomocí proměnné
CommInfo.nState. Pokud zjistí že přenos probíhá, rozhodne podle parametru wait,
zda bude čekat na jeho ukončení nebo se ukončí aniž by odeslala paket. Pokud je linka
volná, vypočítá CRC součet, alokuje buffer pro paket a do něj vloží startovní byty,
hlavičku, data a CRC součet. Dále vyplní do struktury CommInfo informace nezbytné
pro přenos a zahájí přenos, vložením prvního bytu do registru UDR. Vše je znázorněno
na zjednodušeném vývojovém diagramu Obr. 6.2.
Odesílaní dalších znaků probíhá v přerušení USART Transmit Complete, vždy
se otestuje zda byly odeslány všechny byty, pokud ne, vloží do registru UDR další byt
paketu. Pokud už byly odeslány všechny byty, uvolní se buffer a volá se funkce
- 29 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
CommInfo->pfSendData. Průběh přerušení popisuje Obr. 6.3.
Funkce CommInfo->pfSendData musí mýt prototyp:
void _název_(void);
Obr. 6.2: Zjednodušený vývojový diagram funkce SendData()
Obr. 6.3: Zjednodušený vývojový diagram přerušení USART Transmit
Complete
6.2.3 Přijímání paketu
Příjímání paketu je realizováno pomocí přerušení USART Recieve Complete (viz
[5] a [6]), které nastává při přijetí bytu do registru UDR.
Na začátku přerušení se vyhodnotí proměnná CommInfo.nState. Pokud proměnná
CommInfo.nState je rovna COM_ST_READY, což znamená, že neprobíhá žádný přenos
a byt v registru UDR je roven prvnímu startovnímu bytu, nastaví proměnou
CommInfo.nState na COM_ST_WAIT_SEC_ST a ukončí se obsluha přerušení. Jakmile
- 30 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
se přijme další byte opět ho otestuje, zda odpovídá druhému startovnímu bytu, pokud ano,
nastaví se proměnná CommInfo.nState na COM_ST_WAIT_REC_ADR. Dále se přijímají
jednotlivé byty hlavičky paketu a ukládají se do struktury CommInfo, jakmile je přijata
hlavička, přijmou se vlastní data paketu a CRC součet. Vše je popsáno na zjednodušeném
vývojovém diagramu Obr. 6.4.
Obr. 6.4: Zjednodušený vývojový diagram přerušení USART Transmit Complete
Jakmile se přijmou všechny byty paketu, zkontroluje se adresa, když nesouhlasí
paket se zahodí. Pokud souhlasí, provede se kontrola dat pomocí CRC součtů. Když byl
paket přijat v pořádku, volá se funkce CommInfo->pfRecComplete. Funkce musí být
deklarována takto:
void _název_(Adr_t From, DataType_t Type, pData_t pData, Size_t Size);
- 31 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
kde:
•
From – je adresa odesílatele paketu
•
Type – typ paketu
•
pData - ukazatel na pole přijatých dat
•
Size – velikost přijatých dat v bytech
Jakmile je funkce vykonána, uvolní se buffer CommInfo.pRecBuf a proměnná
CommInfo.nState se nastaví zpět na COM_ST_READY, to znamená že komunikace je opět
připravená na příjem nebo odeslání paketu.
Pokud CRC součet nesouhlasí, tzn. že paket nebyl přijat v pořádku, uvolní
se buffer aniž by se volala funkce CommInfo->pfRecComplete a přičte se jednička
k proměnné CommInfo.nCRCCount, která vyjadřuje počet chybně přijatých paketů.
6.2.4 Timeout komunikace
Timeout komunikace zajišťuje funkce ComTimeOut(void). Funkci je nutno volat
pravidelně např. v časovači. Funkce porovnává, zda od minulého průběhu došlo ke změně
proměnné CommInfo.nState, pokud ne, uvolní všechny použité buffery, nastaví proměnnou
CommInfo.nState
na
COM_ST_READY
a
přičte
jedničku
k
proměnné
CommInfo.nTimeOutCount.
Interval volání funkce ComTimeOut() musí být delší než čas potřebný k přenosu
paketu s maximální možnou délkou, jinak by docházelo k timeoutu dřív, než byl paket
přijat nebo odeslán.
6.2.5 Synchronizace
Synchronizaci zajišťují dvě funkce:
•
void CommWaitForReady(void) - tato funkce čeká, dokud není komunikace
volná, resp. dokud není proměnná CommInfo.nState rovna COM_ST_READY.
•
uint8_t CommWaitForRecieve(uint16_t MaxCount) – funkce čeká, dokud není
přijatý další paket, např odpověď na dotaz. Parametr MaxCount určuje maximální
dobu čekaní krát 10ms. Pokud vyprší čas čekání, aniž by byl přijat paket funkce
vrací 0 jinak 1.
- 32 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
6.2.6 Odlišnosti v implementaci pro procesory s dvěma periferiemi USART
Algoritmus funkcí je stejný jako v předchozím případě, liší se pouze jména funkcí
příp. jejich parametry tak, aby bylo možno určit se kterou periferií USART pracují.
Struktura CommInfo obsahuje informace pro každou periferii zvlášť, jména
proměnných se liší pouze tím, že na konci mají číslo periferie, ke které se vztahují. Funkce
pro nastavování informační struktury je deklarována takto:
void SetCommInfoStruct(Adr_t Adr_0, Adr_t Adr_1, RecComplete_t pfRecComplete_0,
RecComplete_t pfRecComplete_1, SendComplete_t pfSendComplete_0, SendComplete_t
pfSendComplete_1);
kde:
•
Adr_0 – adresa periferie USART0
•
Adr_1 – adresa periferie USART1
•
pfRecComplete_0 - ukazatel na funkci, která se má volat po úspěšném přijetí
paketu na USART0
•
pfRecComplete_1 - ukazatel na funkci, která se má volat po úspěšném přijetí
paketu na USART1
•
pfSendComplete_0 - ukazatel na funkci, která se má volat po úspěšném odeslání
paketu na USART0
•
pfSendComplete_1 - ukazatel na funkci, která se má volat po úspěšném odeslání
paketu na USART1
Funkce pro odesílaní paketu SendData() má navíc jeden parametr, který určuje
číslo periferie, která se má použít pro odeslání. Funkce je deklarována takto:
void SendData(Adr_t Rec, DataType_t Type, pData_t pData, Size_t Size, uint8_t wait,
uint8_t USART)
•
vše jako v kap. 6.2.2.
•
uint8_t USART – číslo periferie USART, po které se mají data odeslat.
Funkce pro synchronizaci mají stejný význam, parametry a návratové hodnoty
jako v 6.2.5, mají pouze jiná jména:
•
CommWaitForReady_0 – pro USART0
•
CommWaitForReady_1 – pro USART1
- 33 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
•
CommWaitForRecieve_0 – pro USART0
•
CommWaitForRecieve_1 – pro USART1
TOMÁŠ PASEKA
6.3 Realizace komunikace pro PC
Na PC se pro přijímání dat používá sériový port RS232, protože ten ale pracuje
s 12V logikou je nutné použít napěťový převodník. V našem případě je použit integrovaný
obvod MAX232 od firmy MAXIM.
Pro obsluhu portu je použito přímo Win32 API (application programming
interface), jež zapouzdřuje třída CSerialPort, která využívá MFC verze 7 (Microsoft
Foundation Class).
Třída CSerialBuffer, byla navržena jako základní třída pro práci se sériovým
portem, proto nepracuje přímo s pakety. Slouží jako rodičovská třída pro třídu
CSerialPortMM, která již pracuje přímo s pakety.
Třída CSerialPort zajišťuje veškerou práci s portem tzn. otevření portu, nastavení
parametrů, příjem dat, odesílání dat a jejich ukládání do vstupního bufferu. Příjem dat lze
provádět dvěma způsoby. Buď přijmout předem zadaný počet bytů nebo spuštěním
threadu, automaticky přijímat příchozí byty, tyto byty jsou ukládány do třídy CSerialBuffer.
Třída CSerialBuffer obsahuje vždy všechny přijaté byty, ale uchovává
si informace o tom, které byty už byly přečteny. Dále zajišťuje synchronizaci mezi
zapisováním a čtením dat, které nesmí nikdy probíhat současně.
Výše uvedené třídy byly navrženy pouze pro potřebu testovaní jednotlivých
modulů pomocí PC, proto se nehodí k zajištění nepřetržité komunikace, např. pro přímé
řízení jízdy bludištěm. Ale jsou navrženy tak, aby je bylo možno rozšířit. Jedná
se především o třídu CSerialPortMM, ve které nejsou implementovány všechny nezbytné
funkce.
6.3.1 Popis třídy CSerialPort a CSerialBuffer
Třída CSerialPort je deklarována takto:
class CSerialPort
{
private:
CString
DWORD
BYTE
m_sPort;
m_dwBaudRate;
m_nStopBits;
- 34 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
HANDLE
m_hPort;
HANDLE
m_hRecThread;
HANDLE
m_evEndRecTh;
static UINT __stdcall
RecieveThread(LPVOID pParam);
public:
CSerialBuffer
m_sbBuffer;
CSerialPort(void);
~CSerialPort(void);
inline DWORD ReadAll(LPVOID pBuffer, DWORD dwMaxLen);
inline DWORD ReadAvailable(LPVOID pBuffer, DWORD dwMaxLen);
inline size_t GetBufferSize();
inline size_t GetBufferAvailable();
inline void ClearBuffer();
inline void ResetBufReadStatus();
inline void LockBuffer();
inline void UnLockBuffer();
LPCSTR GetPort();
DWORD GetBaudRate();
BYTE GetStopBits();
DWORD OpenPort(LPCSTR sPort,DWORD dwBaudRate, BYTE nStopBits)
bool ClosePort();
bool IsOpen();
DWORD Transmit(LPVOID pData, DWORD dwLength,DWORD dwTimeOut =
1000 )
DWORD Receive(char* pBuffer, WORD dwMaxLen, LPDWORD
pdwReadBytes);
DWORD ReceiveW(char* pBuffer, DWORD dwLength, LPDWORD
dwReadBytes, DWORD dwTimeOut = 2000);
void StartAutoReceive();
void StopAutoReceive();
void ClearRXBuffer();
};
m_sPort
Název portu, ke kterému je třída připojena (com1, com2, atd.).
m_dwBaudRate
Rychlost přenosu dat v baudech.
m_nStopBits
Počet stop bitů.
m_hPort
Handle otevřeného portu.
m_hRecThread
Handle threadu pro automatické přijímání.
m_evEndRecTh
Handle události zajištující ukončení threadu m_hRecThread.
- 35 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
m_sbBuffer
Vstupní buffer.
static UINT __stdcall RecieveThread(LPVOID pParam)
Pracovní funkce threadu pro automatický příjem.
CSerialPort(void)
Konstruktor.
~CSerialPort(void)
Destruktor.
LPCSTR GetPort()
Funkce vrátí ukazatel na CString s názvem otevřeného portu.
DWORD GetBaudRate()
Funkce vrátí rychlost přenosu v baudech.
BYTE GetStopBits();
Funkce vrátí počet stop bitů, otevřeného portu.
DWORD OpenPort(LPCSTR sPort, DWORD dwBaudRate, BYTE nStopBits)
Funkce otevře port:
•
sPort – název portu (com1, com2, atd.)
•
dwBaudRate – rychlost přenosu dat v baudech
•
nStopBits – počet stop bitů
návratové hodnoty:
•
R_OK – vše proběhlo v pořádku
•
R_COM_NOT_FOUND – port nenalezen
•
R_COM_ACC_DENIED – port není přístupný, nejspíše ho používá jiná aplikace
•
R_SET_COM_TIMEOUTS_FAILED – selhalo nastavení timeoutů
•
R_GET_COM_STATE_FAILED – selhalo nastavení parametrů portu
•
R_ERR_SET_COMM_MASK – selhalo nastavení událostí portu
•
R_ERR_SETUP_COM – nezdařila se fuknce SetupComm viz [12]
bool ClosePort();
Funkce zavře port, vrací true pokud vše proběhlo v pořádku.
- 36 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
bool IsOpen();
Funkce testuje zda je port otevřený, pokud ano vrací true jinak false.
DWORD Transmit(LPVOID pData, DWORD dwLength, DWORD dwTimeOut =
1000 )
Funkce odešle byty na sériový port.
•
pData – Ukazatel na buffer bytů pro odeslání
•
dwLength – velikost bufferu pData v bytech
•
dwTimeOut – čas ve kterém se musí byty odeslat v ms
Návratové hodnoty:
•
R_OK – odeslání proběhlo v pořádku
•
R_ERR_TIMEOUT – nezdařilo se odeslat data v zadaném čase
•
R_ERR – odeslání selhalo
DWORD Receive(char* pBuffer, WORD dwMaxLen, LPDWORD
pdwReadBytes)
Funkce přijme byty z bufferu sériového portu.
•
pBuffer – ukazatel na buffer pro přijaté byty
•
dwMaxLen – maximální počet přijatých bytů
•
pdwReadBuffer – ukazatel na proměnnou, do které se uloží počet přijatých bytů
•
R_OK – funkce proběhla bez chyb
•
R_BUFFER_IS_SMALL – buffer je malý, na port bylo přijato více bytů
než je velikost buffer pBuffer
DWORD ReceiveW(char* pBuffer, DWORD dwLength, LPDWORD
dwReadBytes, DWORD dwTimeOut = 2000)
Přijme data ze sériového portu, funkce na rozdíl od předchozí skončí buď
po přijetí dwLength bytů, nebo po vypršení času dwTimeOut.
•
pBuffer – ukazatel na buffer pro přijatá data
•
dwLenght – počet bytů kolik bude přijato
•
dwReadBytes – ukazatel na proměnou do které se uloží počet přijatých bytů
•
dwTimeOut – čas v ms ve kterém musí být data přijata
- 37 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
•
R_OK – funkce proběhla bez chyb
•
R_ERR_TIMEOUT – vypršel čas pro příjem dat
void StartAutoReceive()
Zahájí automatické příjímání dat z otevřeného portu, přijatá data ukládá
do m_sbBuffer.
void StopAutoReceive()
Zastaví automatické příjímání dat z otevřeného portu.
void ClearRXBuffer()
Vymaže vstupní buffer portu.
Následující funkce zprostředkovávají pouze volání funkcí proměnné m_sbBuffer:
•
inline DWORD ReadAll(LPVOID pBuffer, DWORD dwMaxLen)
•
inline DWORD ReadAvailable(LPVOID pBuffer, DWORD dwMaxLen);
•
inline size_t GetBufferSize();
•
inline size_t GetBufferAvailable();
•
inline void ClearBuffer();
•
inline void ResetBufReadStatus();
•
inline void LockBuffer();
•
inline void UnLockBuffer();
Třída CSerialPort je deklarována takto:
class CSerialBuffer
{
private:
CRITICAL_SECTION
m_csLock;
DWORD
m_dwReadBytes;
DWORD
m_dwSize;
char*
m_pBuffer;
public:
CSerialBuffer(void);
~CSerialBuffer(void);
inline void Lock();
inline void UnLock();
size_t
GetSize();
size_t GetAvailable();
DWORD ReadAll(LPVOID pBuffer, DWORD dwMaxLen);
DWORD ReadAvailable(LPVOID pBuffer, DWORD dwMaxLen);
bool Add(LPVOID pBuffer, DWORD dwLength);
void ResetReadBytes();
- 38 -
ÚMTMB VUT V BRNĚ
void
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Clear();
};
m_csLock
Kritická sekce pro synchronizaci čtení a zápisu dat.
m_dwReadBytes
Počet přečtených bytů z m_pBuffer.
m_dwSize
Celkem bytů v m_pBuffer.
m_pBuffer
Ukazatel na buffer s přijatými daty.
CSerialBuffer(void)
Konstruktor.
~CserialBuffer(void)
Destruktor.
inline void Lock()
Uzamče kód při ukládání dat do bufferu pomocí m_csLock.
inline void UnLock()
Odemkne kód po uložení dat do bufferu pomocí m_csLock.
size_t GetSize()
Vrátí počet bytů v m_pBuffer.
size_t GetAvailable()
Vrátí počet nepřečtených bytů z m_pBuffer.
DWORD ReadAll(LPVOID pBuffer, DWORD dwMaxLen)
Zkopíruje byty z m_pBuffer do pBuffer, ale maximálně dwMaxLen.
DWORD ReadAvailable(LPVOID pBuffer, DWORD dwMaxLen)
Zkopíruje doposud nepřečtené byty z m_pBufferu, ale maximálně dwMaxLen,
a aktualizuje proměnnou m_dwReadBytes.
bool Add(LPVOID pBuffer, DWORD dwLength)
Připojí dwLength bytů z pBufferu do m_pBufferu.
- 39 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
void ResetReadBytes()
Vynuluje počet přečtených bytů, tzn. nastaví m_dwReadBytes = 0.
void Clear()
Vymaže m_pBuffer.
6.3.2 Popis třídy CSerialPortMM a CPaket
Třída CSerialPortMM je deklarovaná takto:
class CSerialPortMM: public CSerialPort
{
public:
char m_cStartBytes[2];
CSerialPortMM(void);
~CSerialPortMM(void);
DWORD TransmitPaket(Adr_t From, Adr_t To, Size_t Size,
DataType_t Type, pData_t pData, DWORD dwTimeOut =
1000);
DWORD ReceiveWPaket(CPaket* paket, Size_t Size, DWORD
dwTimeOut = 2000);
};
m_cStartBytes[2]
Startovní byty.
CSerialPortMM(void)
Konstruktor.
~CSerialPortMM(void)
Destruktor.
DWORD TransmitPaket(Adr_t From, Adr_t To, Size_t Size, DataType_t Type, pData_t
pData, DWORD dwTimeOut = 1000)
Odešle paket na otevřený port.
•
From - adresa odesílatele
•
To - adresa příjemce
•
Size - kolik bytů odeslat
•
Type - typ odeslaných dat
•
pData - ukazatel na data, která odeslat
dwTimeOut – čas, do kterého musí být data odeslána, jinak se odesílaní přeruší
•
Návratové hodnoty jsou stejné jako v CSerialPort.Transmit().
- 40 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
DWORD ReceiveWPaket(CPaket* paket, Size_t Size, DWORD dwTimeOut = 2000)
Příjme paket určité velikosti
•
paket - ukazatel na proměnnou typu CPaket pro uložení přijatých dat.
•
Size - velikost přijímaných dat v bytech
•
dwTimeOut - doba v ms, do které se data musí přijmout
Návratové hodnoty jsou stejné jako v CSerialPort.RecieveW()
Třída CPaket je deklarovaná takto:
class CPaket
{
private:
Size_t
m_Size;
public:
Adr_t
m_From;
Adr_t
m_To;
DataType_t m_Type;
pData_t
m_pData;
inline void AllocData(Size_t Size);
inline void FreeData();
inline Size_t GetSize()
CPaket(void);
~CPaket(void);
};
m_Size
Počet bytů alokovaných pro m_pData.
m_From
Adresa odesílatele.
m_To
Adresa příjemce.
m_Type
Typ dat.
m_pData
Ukazatel na pole bytů obsahující data.
inline void AllocData(Size_t Size)
Alokuje pamět pro m_pData o velikosti Size.
inline void FreeData()
Uvolní paměť alokovanou pro m_pData.
- 41 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
inline Size_t GetSize()
Vrátí proměnou m_Size.
CPaket(void)
Konstruktor.
~CPaket(void)
Destruktor.
- 42 -
TOMÁŠ PASEKA
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
7 Modul Senzoriky
Pro zjednodušení stavby řídícího programu s ohledem na časování, byla zvolena
koncepce samostatného systému senzorů s vlastním procesorem. Jako řídící procesor
je použit ATMega8, který pracuje na frekvenci 11,0592MHz. Frekvence krystalu byla
zvolena tak, aby časovač periferie USART měl nulovou odchylku od požadované
frekvence. Jako senzory jsou použity tři kusy senzorů GP2D120 od firmy SHARP. Senzory
jsou rozmístěny tak, aby jeden měřil vzdálenost překážky před robotem a zbylé dva
po stranách.
7.1 Vlastnosti senzoru GP2D120
Senzor GP2D120 používá pro měření vzdálenosti metodu optické triangulace, kdy
světelný paprsek vytváří na měřeném objektu nepatrný
světelný bod. Detekcí úhlu odrazu tohoto světelného
bodu je vypočtena vzdálenost senzorů od měřeného
objektu. Princip této metody je na Obr. 7.1. Velkou
výhodou této metody je malý rozdíl v měření při bílé
nebo černé překážce. Parametry senzoru jsou shrnuty
Obr. 7.1: Metoda optické triangulace
v Tab 7.1.
Parametr
Hodnota
Výrobce
SHARP
Název
GP2D120
Napájecí napětí
4,5 až 5,5V
Výstupní napětí
1,95 až 2,55V
Pracovní teplota
-10 až +60°C
Rozsah měřené vzdálenosti
4 až 30cm
Doba měření
38,3±9,6ms
Tab 7.1: Technické parametry senzoru SHARP – GP2D120
Závislost výstupního napětí na měřené vzdálenosti je zobrazena na Obr. 7.2.
Překážka byla vyrobena z bílého materiálu (kancelářský papír). Senzor vykazuje pro malé
vzdálenosti cca od 3 do 10 cm velmi vysoké přesnosti měření. Pro větší vzdálenosti
cca od 30 cm je změna výstupního napětí senzoru příliš malá, proto na větší vzdálenosti
není tento senzor vhodný. Protože v našem případě potřebujeme přesně měřit hlavně
- 43 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
vzdáleností do 20 cm, je senzor vyhovující. Pro měření větších vzdáleností by bylo možné
modul vybavit ještě senzorem SHARP GP2Y0A02YK, který měří v rozsahu od 20 do 120
cm. Nebo je možno použít ultrazvukové senzory.
Obr. 7.2: Závislost výstupního napětí na měřené vzdálenosti senzoru GP2D120
7.2 Zapojení senzoru GP2D120
Senzor
je
připojen
na
A/D
převodník
mikrokontroléru, podle doporučení výrobce. I když výrobce
uvádí výstupní napětí na senzoru 1,95 až 2,55V, měření
ukázalo že v našem případě se pohybuje v podstatně vyšším
rozmezí, a to od 0,5 do 3V, jak je vidět na Obr. 7.2. Protože
vnitřní referenční napětí A/D převodníku je 2,56V, bylo
nutné použít odporový dělič napětí. Kompletní zapojení
senzoru k A/D převodníku je vidět na Obr. 7.3.
Obr. 7.3: Zapojení senzoru
GP2D120
7.3 Software modulu senzorů
Software je navržen tak, aby neustále měřil napětí na všech třech senzorech
a výsledek na požádání převedl na mm nebo kroky motoru a poslal paket s naměřenými
vzdálenostmi. Modul posílá buď přímo hodnotu z ADC registru, nebo převádí napětí
na milimetry, nebo kroky motorů.
- 44 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
K měření napětí je použit A/D převodník, který má šest přepínatelných kanálů.
Zapojení senzorů na kanály převodníku je v Tab 7.2. Převodník je nastaven tak,
aby používal vnitřní referenční napětí, které je 2,56V. Převodník má nastavenou
předděličku na 128, takže jeden převod trvá přibližně 150μs.
Senzor
Kanál
pravý
ADC0
přední
ADC1
levý
ADC2
Tab 7.2: Zapojení senzorů
7.3.1 Měření napětí
Měření napětí probíhá v přerušení ADC Conversion Complete, které nastává
po dokončení
převodu,
více
viz
[5]. A/D
převodník běží v tzv. Free Running modu,
kdy se další
převod
zahájí
automaticky
po zpracování přerušení.
Na začátku přerušení se vyhodnotí
proměnná
nAdcChannel,
která
vyjadřuje,
který senzor se právě měří. Naměřená hodnota
napětí se uloží do příslušné globální proměnné.
Přepne se kanál A/D převodníku na další senzor
a ukončí
se
zpracovávání
přerušení,
čimž
se automaticky zahájí další převod. Algoritmus je
znázorněn na vývojovém diagramu Obr. 7.4
Obr. 7.4: Vývojový diagram přerušení ADC
Conversion Complete
7.3.2 Převod napětí na vzdálenost
Převod napětí na vzdálenost je z důvodů nelineární závislosti realizován pomocí
tabulky. Řádky tabulky jsou tvořeny dvojicí hodnot napětí a jemu odpovídající vzdálenosti.
Tabulka je kvůli velikosti uložena v programové paměti. Nevýhodou použití programové
paměti je několikanásobně delší doba přístupu, ale protože senzory mají nízkou vzorkovací
frekvenci, je toto řešení vyhovující.
- 45 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Hodnoty tabulky i jejich počet je volen tak, aby co nejlépe popisovaly závislost
z Obr. 7.2. Při volbě počtu hodnot musel být brán ohled na dobu prohledávání tabulky.
Jako kompromis mezi přesností popisu a dobou prohledávání bylo zvoleno 36 hodnot.
Převod realizuje funkce:
uint16_t GetDistanceMM(uint16_t Adc)
kde parametr Adc je hodnota A/D převodníku získaná z registru ADC, funkce vrací
vzdálenost v milimetrech.
Funkce
prohledává
tabulku,
která
je seřazena od nejmenšího napětí po největší.
Vývojový diagram je znázorněn na Obr. 7.5.
Prohledávání zajišťuje cyklus, který postupně načítá
hodnoty z programové paměti. Jakmile je nalezena
hodnota,
která
je
větší
než
proměnná Adc,
prohledávání se ukončí. Načte se z programové
paměti vzdálenost, která ji odpovídá. Dále se načte
předchozí napětí, a jemu odpovídající vzdálenost.
Poté se provede výpočet vzdálenosti, odpovídající
napětí Adc, pomocí lineární interpolace viz Rov. 7.1,
kde je A – aktuálně změřené napětí, A1 – napětí i-tého
řádku z tabulky, A2– napětí (i-1)-tého řádku z
tabulky, d1 – vzdálenost odpovídající napětí A1, d2 –
Obr. 7.5: Vývojový diagram funkce pro
převod napětí na vzdálenost
vzdálenost odpovídající napětí A2, d – vzdálenost
odpovídající napětí A.
d=
d 1−d 2 ⋅ A−A2 
d 2
A1−A2
Rov. 7.1
7.4 Popis všech paketů modulu
DT_ECHO
Při přijetí tohoto paketu modul vrátí stejný paket.
DT_GET_STATUS
Žádost o poslání statistiky komunikace. Modul vrátí paket typu DT_STATUS,
velikost dat je 3 byty.
- 46 -
ÚMTMB VUT V BRNĚ
•
byte 0 – rezervovaný
•
byte 1 – počet CRC chyb
•
byte 2 – počet TimeOutů
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
DT_SEN_GET_LEFT
Žádost o poslání výstupního napětí pravého senzoru. Modul vrátí paket typu
DT_SEN_LEFT, velikost dat je 2 byty.
•
byte 0 – horní byte 16-bit čísla
•
byte 1 – dolní byte 16-bit čísla
DT_SEN_GET_RIGHT
Žádost o poslání výstupního napětí levého senzoru. Modul vrátí paket typu
DT_SEN_RIGHT, velikost dat je 2 byty
•
•
DT_SEN_GET_CENTER
Žádost o poslání výstupního napětí prostředního senzoru. Modul vrátí paket typu
DT_SEN_CENTER, velikost dat je 2 byty
•
•
DT_SEN_GET_ALL
Žádost o poslání výstupního napětí všech senzorů. Modul vrátí paket typu
DT_SEN_ALL, velikost dat je 6 bytů.
•
byte 0 – horní byte 16-bit čísla, levý senzor
•
byte 1 – dolní byte 16-bit čísla, pravý senzor
•
byte 2 – horní byte 16-bit čísla, přední senzor
•
byte 3 – dolní byte 16-bit čísla, přední senzor
•
byte 4 – horní byte 16-bit čísla, pravý senzor
•
DT_SEN_GET_LEFT_MM
Žádost o poslání naměřené vzdálenosti na levém senzoru v milimetrech. Modul
vrátí paket typu DT_SEN_LEFT_MM, velikost dat je 2 byty.
- 47 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
•
•
TOMÁŠ PASEKA
DT_SEN_GET_RIGHT_MM
Žádost o poslání naměřené vzdálenosti na pravém senzoru v milimetrech. Modul
vrátí paket typu DT_SEN_RIGHT_MM, velikost dat je 2 byty.
•
•
DT_SEN_GET_CENTER_MM
Žádost o poslání naměřené vzdálenosti na prostředním senzoru v milimetrech.
Modul vrátí paket typu DT_SEN_CENTER_MM, velikost dat je 2 byty.
•
•
DT_SEN_GET_ALL_MM
Žádost o poslání naměřené vzdálenosti na všech senzorech. Modul vrátí paket
typu DT_SEN_ALL_MM, velikost dat je 6 bytů.
•
•
•
•
•
•
DT_SEN_GET_CENTER_ST
Žádost o poslání naměřené vzdálenosti na prostředním senzoru v krocích motoru.
Modul vrátí paket typu DT_SEN_CENTER_ST, velikost dat je 2 byty.
•
•
DT_SEN_GET_LEFT_ST
Žádost o poslání naměřené vzdálenosti na v levém senzoru v krocích motoru.
Modul vrátí paket typu DT_SEN_LEFT_ST, velikost dat je 2 byty.
•
- 48 -
ÚMTMB VUT V BRNĚ
•
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
DT_SEN_GET_RIGHT_ST
Žádost o poslání naměřené vzdálenosti na v pravém senzoru v krocích motoru.
Modul vrátí paket typu DT_SEN_RIGHT_ST, velikost dat je 2 byty.
•
•
DT_SEN_GET_ALL_ST
Žádost o poslání naměřené vzdálenosti na všech senzorech v krocích motoru.
Modul vrátí paket typu DT_SEN_ALL_ST, velikost dat je 6 bytů.
•
•
•
•
•
•
7.5 Testovací software na PC
Pro možnost testování správné funkce hardwaru modulu senzorů a pro měření
jejich charakteristik byly vytvořeny dva programy na PC:
•
Senzors Test – testovaní funkce senzorů
•
Senzors Char – měření charakteristiky senzorů (vyžaduje i modul motorů)
Senzors Test
Modul využívá pro komunikaci třídu CSeriálPortMM popsanou v kap. 6.3.
Aplikace posílá modulu všechny podporované pakety popsané v kap. 7.4 a zobrazuje jejich
odpovědi. Lze ji použít na získání statistiky komunikace, nebo na ověřování přesnosti
měření senzorů.
Tato aplikace nevyžaduje, aby byl modul připevněn k podvozku a spojen
s modulem hlavního CPU (lze jej připojit naprosto nezávisle).
Senzors Char
Tato aplikace opět využívá CSeriálPortMM popsanou v kap. 6.3. Aplikace
vyžaduje, aby byl modul připevněn k podvozku robotu a podvozek byl také osazen
- 49 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
modulem motorů. K PC je nutno připojit oba moduly, proto musí být PC vybaveno dvěma
sériovými porty. Aplikace podle nastavení provede několik měření vzdálenosti
před předním senzorem, ty zprůměruje a uloží. Potom pošle modulu motorů příkaz, aby
ujely nastavenou vzdálenost a měření opakuje, dokud není dosaženo požadovaného počtu
měření.
Naměřené hodnoty ukládá do textového souboru typu csv, kde jsou jednotlivé
hodnoty odděleny středníkem. Tento soubor lze snadno importovat to tabulkových
procesorů, jako je Microsoft Excel nebo OpenOffice Calc.
- 50 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
8 Modul Motoru
Pro řízení krokových motorů, byla opět zvolena koncepce samostatného systému
motorů s vlastním procesorem. Jako řídící procesor je použit ATMega8 s krystalem
14,7456 Mhz. Frekvence krystalu byla zvolena tak, aby časovač periferie USART měl
nulovou odchylku od požadované frekvence. Pohon robotu je zajištěn dvěma krokovými
motory TEAC KP39HM2-025 z disketových mechanik. Řízení motorů je bez zpětné
vazby, tudíž se jedná o řízení v otevřené smyčce.
8.1 Hardware motoru
Ovládání motorů zajišťují integrované obvody SLA7051 firmy ALLEGRO, které
umožňují jedno-fázové i dvou-fázové řízení unipolárních krokových motorů. Obvod dále
umožňuje řídit i proud do vinutí krokového motoru PWM modulací. Samozřejmostí
je i použití polovičního kroku. Základní technické parametry obvodu SLA7051 jsou v Tab
8.1 a technické parametry motoru jsou v Tab 8.2
Paramter
Hondnota
Výrobce
ALLEGRO
Název
SLA7051
Max výstupní napětí
46V
Max proud
2A
Max logické napětí
7V
Pracovní teplota
-20 až 85˚C
Tab 8.1: Zakladní tech. parametry obvodu SLA7051
Parametr
Hodnota
Výrobce
TEAC
Název
KP39HM2-025
Typ motoru
Unipolární
Délka kroku
1,8“
Počet kroků na otáčku
200
Počet vinutí
4
Napájecí napětí
12V
Jmenovitý proud
0,2A
- 51 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Parametr
Hodnota
Opor vinutí
75Ω
Indukčnost cívky
70mH
Rozměry
39x39x33mm
Hmotnost
130g
Tab 8.2: Technické parametry krokových motorů TEAC - KP39HM2-025
8.2 Odometrie manévrů modulu
Modul nezajišťuje jen samotné řízení rychlosti krokových motorů. Aby
nedocházelo k nadměrnému zatěžování hlavního modulu, umožňuje provádět základní
manévry:
•
ujetí požadované vzdálenosti
•
otočení na místě o zadaný úhel
•
zatáčka o 90˚ během jízdy
Dále měří ujetou vzdálenost každého kola a zajišťuje regulaci směru na příkaz
hlavního modulu.
Protože řízení probíhá bez zpětné vazby, u všech níže popsaných manévrů, musí
být zrychlení i rychlost taková, aby nedocházelo k prokluzu. Parametry, které limitují
rychlost a zrychlení, jsou součinitel tření mezi vozovkou a pneumatikou, kroutící moment
motoru (nesmí dojít ke ztrátě kroku), a hmotnost robotu. Hmotnost robotu ovlivňuje třecí
sílu a setrvačnost .
Maximální hodnoty parametrů jsou shrnuty v Tab 8.3, tyto parametry byly
zjištěny experimentálně a jsou spíše orientační, protože výrazně závisí na povrchu
vozovky.
Parametr
Hodnota
Max rychlost
0,5 ms-1
Max zrychlení
1ms-2
Max rychlost průjezdu zatáčkou o
poloměru 70mm
0,12ms-1
Max rychlost průjezdu zatáčkou o
poloměru 100mm
0,2ms-1
Tab 8.3: Orientační maximální hodnoty rychlosti a zrychlení
- 52 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
8.2.1 Akcelerace motorů
Pro potřeby akcelerace krokových motorů je nutno odvodit vztah pro novou
rychlost v závislosti na změně ujeté vzdálenosti, která reprezentuje jeden krok.
Při odvozování vyjdeme ze základní rovnice:
a=
Rov. 8.1
ds
ds
rovnici rozšíříme zlomkem
a=
protože
dv
dt
dv ds
⋅
dt ds
Rov. 8.2
ds
=v můžeme psát
dt
dv
a=v⋅
ds
Rov. 8.3
Po dosazení mezí a integrací získáme následující vztah, který upravíme tak, aby
v2 bylo na levé straně a všechno ostatní na straně pravé:
s2
v2
s1
v1
Rov. 8.4
∫ sds=∫ vdv
v 22−v 21
a  s 1−s 2=
2
Rov. 8.5
v 2 =2a  sv 1
Rov. 8.6
2
Rov. 8.6 již vyjadřuje naši hledanou závislost rychlosti na změně dráhy, kde Δs je
ujetá dráha na jeden krok motoru.
8.2.2 Ujetí požadované vzdálenosti
Tento manévr lze rozdělit na tři části,
jak je vidět na Obr. 8.1:
•
zrychlování
•
jízda konstantní rychlostí
•
brzdění
Obr. 8.1: Závislost rychlosti na dráze
- 53 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Nejdříve je potřeba vypočítat teoretickou maximální možnou rychlost vmaxt, kterou
lze na dané vzdálenosti dosáhnout. K tomu lze využít Rov. 8.6, kde za Δs dosadíme
požadovanou vzdálenost a v1 = 0. Protože ale musíme počítat také s bržděním je třeba
požadovanou vzdálenost podělit dvěmi:

v maxt = 2 a
s poz
2
Rov. 8.7
Námi spočítaná rychlost vmaxt může být větší než maximální rychlost motorů vmaxm,
proto maximální rychlost, kterou lze na požadované vzdálenosti dosáhnout se rovná:
v max=min v maxt , v maxm 
Rov. 8.8
Dále je potřeba vypočítat dráhu potřebnou k zrychlení na vmax. K tomu lze opět
využít Rov. 8.6. Kterou upravíme tak, aby na levé straně byla dráha s a na pravé zbytek:
s zrych=
v 22−v 21
2a
Rov. 8.9
Protože ale v našem případě se v1 = 0 lze psát:
v 2max
s zrych=
2a
Rov. 8.10
Dráha potřebná pro zabrzdění je stejná jako dráha pro zrychlení. Proto dráha
po které se robot pohybuje konstantní rychlostí je:
s konst =s poz −2 s zrych
Rov. 8.11
8.2.3 Otočení na místě o zadaný úhel
Jedná se o stejný problém jako v předešlé kapitole, pouze se každé kolo musí
otáčet jiným směrem. Pro tento problém je nutné vypočítat, jakou vzdálenost musí každé
kolo urazit, aby se robot otočil o požadovaný úhel.
Při tomto manévru se robot otáčí okolo středu nápravy a kola opisují kružnici
o poloměru r, který lze vypočítat:
r=
H
2
kde H je rozchod robotu.
- 54 -
Rov. 8.12
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Ujetá vzdálenost každého kola je rovna kruhové výseči o požadovaném úhlu,
to lze vypočítat podle vztahu:
s=
2 r
⋅
360
Rov. 8.13
kde je s – vzdálenost, kterou musí urazit každé kolo, φ – úhel, o který se má robot
otočit.
8.2.4 Zatáčka o 90˚ během jízdy
Zatáčka o 90˚ během jízdy je nejsložitějším manévrem, který modul realizuje. Lze
ji rozdělit do pěti úseků:
•
Nájezd do zatáčky (A)– obě kola se otáčí stejnou rychlostí, robot jede rovně.
•
Změna rychlosti kol při nájezdu (B)– vnější kolo zrychluje a vnitřní zpomaluje,
robot se začíná otáčet.
•
Jízda zatáčkou (C)– obě kola se otáčí konstantní rychlostí, robot se pohybuje
po kružnici.
•
Změna rychlosti kol při výjezdu (D)– vnější kolo zpomaluje a vnitřní zrychluje,
robot se přestává otáčet.
•
Výjezd ze zatáčky (E)– obě kola se otáčí stejnou rychlostí, robot se pohybuje
rovně.
Vše ze znázorněno na Obr. 8.2 a Obr. 8.3.
Obr. 8.2: Rychlosti otáčení kol během průjezdu
zatáčkou
Obr. 8.3: Úseky průjezdu zatáčkou
- 55 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Nejdříve je potřeba spočítat dráhu, kterou musí urazit střed nápravy a jednotlivá
kola v průběhu celé zatáčky:
2R
4
Rov. 8.14
2  R−w
4
Rov. 8.15
2 Rw 
4
Rov. 8.16
s str =
s in=
s out =
kde je sstr – dráha, kterou urazí střed nápravy, sin – dráha, kterou urazí vnitřní
kolo, sout – dráha, kterou urazí vnější kolo, R – poloměr zatáčky a w – je vzdálenost kola
od středu nápravy.
Čas potřebný k průjezdu zatáčkou se vypočítá podle následujícího vztahu:
t=
s str
v str
Rov. 8.17
kde je sstr – dráha středu nápravy, vstr – námi zvolená rychlost středu nápravy,
jedná se vlastně o rychlost kterou najíždíme do zatáčky.
Protože obě kola musí projet zatáčku ve stejném čase, lze pomocí času t spočítat
rychlost vnějšího a vnitřního kola v úseku C.
v in =
s in
t
Rov. 8.18
v out =
s out
t
Rov. 8.19
kde je vin – rychlost vnitřního kola, vout – rychlost vnějšího kola, sin – dráha
vnitřního kola, sout – dráha vnějšího kola.
Pokud je změna rychlosti v úsecích B a D dostatečně malá, lze ujetou dráhu
v těchto úsecích zanedbat. Dráha ujetá v úseku C se potom rovná celkové dráze. Pokud
je ale změna rychlosti taková že ji nemůžeme zanedbat, dráha ujetá v úseku C se rovná:
sCstr =s str −s Bstr −s Dstr
Rov. 8.20
s Cin=s in −s Bin −s Din
Rov. 8.21
s Cout=s out−s Bout−s Dout
Rov. 8.22
- 56 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
2
TOMÁŠ PASEKA
2
v str −v in
s Bin =s Din =
2a
Rov. 8.23
s Bin s Bout
2
Rov. 8.24
v 2out −v 2str
s Bout =s Dout =
2a
Rov. 8.25
s Bstr =s Dstr =
kde je sCstr - dráha středu nápravy v úseku C, sBstr - dráha středu nápravy v úseku
B, sDstr - dráha středu nápravy v úseku D, sCin - dráha vnitřního kola v úseku C, sBin dráha vnitřního kola v úseku B, sDin - dráha vnitřního kola v úseku D, sCout - dráha
vnějšího kola v úseku C, sBout - dráha vnějšího kola v úseku B, sDout - dráha vnějšího kola
v úseku D.
8.3 Software modulu
Aby došlo k ušetření výpočetního času, modul používá jako jednotku vzdálenosti
místo milimetrů kroky motoru, stejně tak rychlost je krok/s a zrychlení je krok/s2.
Délka jednoho kroku závisí na obvodu kola:
s kt =
2 r
nkr
Rov. 8.26
kde je skr - délka jednoho kroku, r - poloměr kola robotu, nkr - počet kroků
na otáčku.
Po dosazeni dostaneme:
s kt =
2 r 2 60
=
=0,942 mm
nkr
200
Rov. 8.27
z toho plyne že 1krok = 0,942mm.
8.3.1 Řízení krokových motorů
Řízení rychlosti krokových motorů je realizováno pomocí časovače Timer1, viz
[5]. Jedná se o 16-bit časovač, který umožňuje vykonávat mimo jiné dvě přerušení Timer1
Compare A a Timer1 Compare B. Přerušení nastávají, když je registr časovače roven
registru Output Compare 1A resp. 1B.
Pro řízení pravého motoru je použito přerušení Timer1 Compare A, a pro levý
motor přerušení Timer1 Compare B. Dále uvedu popis řízení pouze jednoho motoru,
protože druhý motor je naprosto totožný.
- 57 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Průběh přerušení je vidět na Obr. 8.4.
Nejdříve se nastaví příslušný pin do logické
jedničky,
do řídícího
čímž
se
obvodu
začne
vysílat
SLA7051.
impulz
Provede
se
výpočet nové rychlosti podle vztahů uvedených
v kap. 8.2.1. Po výpočtu rychlosti se nastaví
příslušný pin do logické nuly, čímž je ukončeno
vysílání impulzu a motor provede krok. Poté se
provede výpočet hodnoty, kterou je nutno přičíst
k Output Comapre Registru. Jedná se vlastně
o dobu, za kterou se má vyslat další impulz.
Potom v závislosti na typu manévru, který je Obr. 8.4: Zjednodušený vývojový diagram
realizován, se může provádět hlídání ujeté přerušení Timer Compare
vzdálenosti nebo řízení jízdy zatáčkou.
8.3.2 Výpočet nové rychlosti a nastavení Output Compare registru
Rychlost se nastavuje vždy v proměnné nTargetSpeed, která vyjadřuje cílovou
rychlost. Ostatní manévry pouze hlídají ujetou vzdálenost a podle ní nastavují
nTargetSpeed, tímto je vždy zaručena správná dynamika motorů.
Obr. 8.5: Vývojový diagram řízení rychlosti
- 58 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Algoritmus řízení je zjednodušeně zobrazen na Obr. 8.5. Na začátku řízení
rychlosti se porovná proměnná nCurSpeed, která vyjadřuje aktuální rychlost motorů,
s cílovou rychlostí (nTargetSpeed). Pokud je cílová rychlost menší než aktuální rychlost
motorů, provede se výpočet nové rychlosti pomocí funkce Deccel(), která vypočítá novou
rychlost při zrychlení nAccel za vzdálenost jednoho kroku. Analogicky vše probíhá pokud
je cílová rychlost větší než aktuální, pouze se použije funkce Accel(). Funkce pro výpočet
rychlosti používají vztahy, které byly odvozeny v kap. 8.2.1.
Výpočty nové rychlosti jsou velice náročné na výpočetní čas CPU. Pro zrychlení
výpočtu jsou simulována čísla s pevnou desetinnou čárkou, s přesností na tří desetinná
místa. To se ukázalo jako dostačující přesnost. Díky tomu je výpočet nové rychlosti 5 až 6
krát rychlejší než za použítí čísel s plovoucí desetinou čárkou. I přesto, je ale maximální
rychlost motorů omezena na 700 kroků/s, při překročení této rychlosti už nelze zajistit,
že výpočet bude proveden v potřebném čase. Řešením by mohlo být použití krystalu
o větší frekvenci.
Jakmile je vypočítána nová rychlost, je nutno nastavit Output Compare Register.
Výpočet doby, za kterou je nutno vykonat další krok motorů se provádí podle vzorce:
comp=
f tim
v kr
Rov. 8.28
kde je comp - hodnota přičítaná ke Output Compare registru, ftim - frekvence
časovače, vkr - rychlost v krocích za sekundu.
Řízení rychlosti nijak nekontroluje překročení maximální dovolené rychlosti
nastavené pomocí proměnné nMaxSpeed. Tato kontrola se provádí už při nastavování
proměnné nTargetSpeed, např. po přijetí paketu DT_MOT_SPEED.
Nastavování všech parametrů pro řízení rychlosti jízdy, zrychlení a maximální
rychlosti, se děje pomocí paketů popsaných v kap. 8.4, defaultní nastavení je uvedeno
v Tab 8.4.
Parametr
hodnota
Zrychlení [krok/s2]
1027
Maximální rychlost [krok/s]
200
Tab 8.4: Defaultní parametry řízení rychlosti
- 59 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
8.3.3 Hlídání ujeté vzdálenosti
Hlídání ujeté vzdálenosti je využíváno pro dva manévry, a to ujetí požadované
vzdálenosti a otáčku na místě.
Hlídání ujeté vzdáleností využívá dvou proměnných nTargetDis a nTargetBrDis.
Proměnná nTargetDis vyjadřuje celkovou vzdálenost, která má být ujeta. Pokud se rovná
nule, hlídaní ujeté vzdálenosti se neprovádí. Proměnná nTargetBrDis vyjadřuje vzdálenost,
za kterou je nutno začít brzdit, aby došlo k zastavení na požadované vzdálenosti.
nTargetDis je buď přímo zadaná pomoci paketu DT_MOT_GO_DISTANCE, nebo
vypočítaná při otáčce na místě za pomoci vztahů odvozených v kap. 8.2.3. Proměnnou
nTargetBrDis vypočítá, podle vztahů v kap. 8.2.2,
funkce CalcDisToBreak(). Cílovou rychlost, kterou
je možno na daném úseku dosáhnout, počítá funkce
CalcMaxSpeed(), opět podle vztahů odvozených
v kap. 8.2.2, takto vypočítaná rychlost je dosazena
do
proměnné
nTargetSpeed.
Funkce
počítají
z okamžitou rychlostí, proto nemusí být na začátku
robot v klidu.
Hlídání ujeté vzdálenosti probíhá podle
algoritmu na Obr. 8.6. Pokud se proměnná
nTargetDis
nerovná nule,
přičte se jednička
k proměnné nDistance, která vyjadřuje ujetou
Obr. 8.6: Zjednodušený vývojový diagram
hlídání ujeté vzdálenosti
vzdálenost.
Dále se testuje,
zda byla ujeta
vzdálenost, po které se má začít brzdit. Pokud ano,
nastaví se nTargetSpeed na nulu, což znamená, že motor začne zpomalovat dokud
nezastaví. Jakmile se ujede celá požadovaná vzdálenost, jsou vynulovány použité
proměnné a motor stojí.
Pokud je v průběhu hlídání ujeté vzdálenosti obdržen jiný příkaz, např.
DT_MOT_SPEED nebo jiný, jsou proměnné vynulovány a manévr je ukončen.
8.3.4 Řízení jízdy zatáčkou
Jízda zatáčkou požívá několik parametrů, které jsou vypočítány pomocí vztahů
odvozených v kap. 8.2.4. Tyto parametry jsou bud přednastaveny nebo se počítají po přijetí
- 60 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
paketu DT_MOT_SET_TURN_PARAM, který je popsaný v kap. 8.4. Defaultní hodnoty
parametrů jsou v Tab 8.5, pro rychlost nájezdu 100 krok/s a poloměr 75 kroků.
Parametr
Proměnná
Hodnota
Rychlost středu nápravy
nTurnVcen
100 krok/s
Rychlost vnitřního kola
nTurnVin
32 krok/s
Rychlost vnějšího kola
nTurnVout
168 krok/s
Dráha vnitřního kola
nDisInner
28 kroků
Dráha vnějšího kola
nDisOuter
180 kroků
Tab 8.5: Defaultní parametry jízdy zatáčkou
Jízda zatáčkou je odstartována po přijetí paketu DT_MOT_TURN, který je
popsaný v kap. 8.4. Podle směru zatáčky, se do proměnné nTargetSpeed příslušného
motoru vloží rychlost vnějšího nebo vnitřního kola. Analogicky se do proměnné nTurnDis
příslušného motoru vloží dráha vnitřního nebo vnějšího kola. Dále se nastaví proměnná
nRouteType na hodnotu MRT_TURN_DECCEL, která vyjadřuje, že se motory nachází
v nájezdu do zatáčky. Vlastní řízení probíhá jen v přerušení Timer1 Compare.
Proměnná nRouteType vlastně popisuje, ve které oblastí podle Obr. 8.2 a Obr. 8.3
se motory nachází. Oblasti A a E odpovídá hodnota MRT_DIRECTLY, oblasti B
MRT_TURN_DECCEL, oblasti C MRT_TURN a oblasti D MRT_TURN_ACCEL.
Algoritmus řízení je zobrazen na Obr. 8.7. Pokud je proměnná nRouteType rovna
MRT_TURN_DECCEL, testuje se, jestli bylo dosaženo požadované rychlosti kola. Pokud
bylo dosaženo požadované rychlosti, proměnná nRouteType se nastaví na MRT_TURN,
to znamenná že motory se nachází v oblasti s konstantní rychlostí a provádí se test ujeté
vzdálenosti.
Každý
krok
se
inkrementuje
proměnná
nDistance
a
porovnává
se z požadovanou vzdáleností nTurnDis. Jakmile je požadovaná vzdálenost ujetá,
nTargetSpeed se nastaví na rychlost středu nápravy, která byla zároveň vstupní rychlostí
do zatáčky, proměnná nRouteType se nastaví na MRT_TURN_ACCEL, která znamená,
že motory se nachází na výjezdu ze zatáčky. Jakmile motory dosáhnou zadaných rychlostí,
proměnná nRouteType se nastaví na hodnotu MRT_DIRECTLY, která vyjadřuje, že zatáčka
byla ukončena a motory se pohybují rovně.
- 61 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Obr. 8.7: Zjednodušený vývojový diagram řízení jízdy zatáčkou
Je nezbytné, aby v době kdy modul obdrží příkaz k vykonání zatáčky během jízdy
(paket DT_MOT_TURN), byla rychlost obou motorů rovna zadané rychlosti středu
nápravy, resp. rychlosti nájezdu do zatáčky, pokud tomu tak není, rychlost je změněna
skokově.
Pokud
je
v
době
vykonávání
manévru
obdržen
jiný
příkaz
např.
DT_MOT_SPEED, je manévr přerušen.
DT_ECHO
Pří přijetí tohoto paketu modul vrátí stejný paket
DT_GET_STATUS
Žádost o poslání statistiky komunikace. Modul vrátí paket typu DT_STATUS,
•
byte 0 – rezervovaný
•
byte 1 – počet CRC chyb
•
byte 2 – počet TimeOutů
- 62 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
DT_MOT_EN
Zapne/Vypne motory.
Data:
•
byte 0 - 0 - vypne, vše ostatní zapne
DT_MOT_DIR
Nastaví směr otáčení motorů.
Data:
•
byte 0 - 1- vpřed, 2 vzad.
DT_MOT_STEP
Nastaví krok motoru
byte 0 - 0 - Full step, 1 - Half step.
DT_MOT_SPEED
•
Nastaví rychlost motorů.
Data:
•
byte 0 - horní byte 16-bit čísla, rychlost pravého motoru
•
byte 1 - dolní byte 16-bit čísla, rychlost pravého motoru
•
byte 2 - horní byte 16-bit čísla, rychlost levého motoru
•
byte 3 - dolní byte 16-bit čísla, rychlost levého motoru
DT_MOT_GO_DISTANCE
Nastaví vzdálenost, kterou mají motory urazit.
Data:
•
byte 0 - horní byte 16-bit čísla, vzdálenost pro pravý motor
•
byte 1 - dolní byte 16-bit čísla, vzdálenost pro pravý motor
•
byte 2 - horní byte 16-bit čísla, vzdálenost pro levý motor
•
byte 3 - dolní byte 16-bit čísla, vzdálenost pro levý motor
DT_MOT_ACCEL
Nastaví zrychlení motoru.
Data:
•
byte 0 - horní byte 16-bit čísla, zrychlení
•
byte 1 - dolní byte 16-bit čísla, zrychlení
- 63 -
TOMÁŠ PASEKA
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
DT_MOT_MAX_SPEED
Nastaví maximální rychlost motorů
Data:
•
byte 0 - horní byte 16-bit čísla, max. rychlosti.
•
byte 1 - dolní byte 16-bit čísla, max. rychlosti.
DT_MOT_GET_INFO
Žádost o poslání parametrů moru. Modul vrátí paket DT_MOT_INFO, velikost dat
je 27 bytů.
•
byte 0 - rezervováno
•
byte 1,2 – 16-bit číslo, aktuální rychlost pravého motoru
•
byte 3,4 – 16-bit číslo, aktuální rychlost levého motoru
•
byte 5,6 – 16-bit číslo, cílová rychlost pravého motoru
•
byte 7,8 – 16-bit číslo, cílová rychlost levého motoru
•
byte 9,10 – 16-bit číslo, konstanta pro výpočet zrychlení
•
byte 11,12 – 16-bit číslo, zrychlení motoru
•
byte 13,14 – 16-bit číslo, maximální rychlost motorů
•
byte 15,16 – 16-bit číslo, cílová vzdálenost pro pravé kolo
•
byte 17,18 – 16-bit číslo, cílová vzdálenost pro levé kolo
•
byte 19,20 – 16-bit číslo, cílová vzdálenost, po které začne brždění pravého
motoru
•
byte 21,22 – 16-bit číslo, cílová vzdálenost po které začne brždění levého motoru
•
byte 23,24 – 16-bit číslo, vzdálenost ujetá pravým motorem
•
byte 25,26 – 16-bit číslo, vzdálenost ujetá levým motorem
Pozn. tento paket slouží především pro testování a ladění modulu
DT_MOT_EMER_STOP
Nouzové zastavení. Motory se okamžitě zastaví, ale nevypnou se.
DT_MOT_TURN_PLACE
Otáčka na místě.
Data:
•
byte 0 – horní byte 16-bit čísla, úhel otočení
•
byte 1 - dolní byte 16-bit čísla, ůhel otočení
- 64 -
ÚMTMB VUT V BRNĚ
•
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
byte 2 – směr otáčení, 0-doleva/1-doprava
DT_MOT_TURN
Zatáčka o 90° během jízdy
Data:
•
byte 0 – směr zatáčky, 0 – doleva/1 - doprava
DT_MOT_GET_TURN_PARAM
Žádost o poslání parametrů zatáčky. Modul vrátí paket DT_MOT_TURN_PARAM,
velikost dat je 10 bytů.
•
byte 0,1 – 16-bit číslo, rychlost průjezdu zatáčkou
•
byte 1,2 – 16-bit číslo, rychlost vnitřního kola
•
byte 3,4 – 16-bit číslo, rychlost průjezdu zatáčkou
DT_MOT_SET_TURN_PARAM
Nastavení parametrů zatáčky.
Data:
•
byte 0 – horní byte 16-bit čísla, rychlost průjezdu zatáčkou
•
byte 1 – dolní byte 16-bit čísla, rychlost průjezdu zatáčkou
•
byte 2 – horní byte 16-bit čísla, poloměr zatáčky
•
byte 3 – dolní byte 16-bit čísla, poloměr zatáčky
DT_MOT_GET_MAIN_INFO
Žádost o poslání informací pro hlavní modul. Modul vrátí paket typu
DT_MOT_MAIN_INFO, velikost dat je 5 bytů.
v levé kol v pravé kol
)
2
•
byte 0 – horní byte 16-bit čísla, výsledná rychlost (
•
byte 1 - dolní byte 16-bit čísla, výsledná rychlost
•
byte 2 - horní byte 16-bit čísla, výsledná ujetá vzdálenost (
•
byte 3 - dolní byte 16-bit čísla, výsledná ujetá vzdálenost
•
byte 4 – 8-bit číslo, typ cesty (proměná nRouteType)
DT_MOT_NULL_COVER_DIS
Nuluje ujetou vzdálenost.
- 65 -
s levé kol s pravé kol
)
2
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
DT_MOT_GET_COVER_DIS
Žádost
o
poslání
ujeté
vzdálenosti.
Modul
vrátí
paket
typu
DT_MOT_COVER_DIS, velikost dat je 4byty.
•
byte 0 – horní byte 16-bit čísla, vzdálenost ujetá levým kolem
•
byte 1 – dolní byte 16-bit čísla, vzdálenost ujetá levým kolem
•
byte 2 – horní byte 16-bit čísla, vzdálenost ujetá pravým kolem
•
byte 3 – dolní byte 16-bit čísla, vzdálenost ujetá pravým kolem
DT_MOT_DIR_REG
Regulace směru jízdy
Data:
•
byte 0 – horní byte 16-bit znaménkového čísla, velikost akčního zásahu
•
byte 1 – dolní byte 16-bit znaménkového čísla, velikost akčního zásahu
Pokud je parametr kladný regulace na pravou stranu jinak na levou.
8.5 Testovací software modulu pro PC
Tento modul má také svoji testovací aplikaci. Ta opět využívá pro komunikaci
třídy CSeriálPortMM. Aplikace se jmenuje Motor Test a umožňuje posílat modulu
všechny pakety popsané v kap. 8.4 a zobrazovat jejich odpovědi.
Pokud je modul připevněný na podvozek robotu a jsou k němu připojeny motory,
lze
testovat
jednotlivé
manévry
s
různými
parametry
bez
nutností
zapojení
a přeprogramování hlavního modulu. To je velice užitečné k nalezení správného nastavení,
při kterém nedochází k prokluzu kol.
- 66 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
9 Modul hlavního CPU
Modul hlavního CPU je navržen tak, aby co nejlépe využíval dostupný prostor
robotu. Modul zajišťuje především vlastní navigaci v bludišti a prohledávací strategii.
Další jeho funkcí je výroba stabilizovaného napětí pro ostatní moduly, také hlídá aby
napětí v bateriích nekleslo pod stanovenou mez a nedošlo ke zničení baterií.
9.1 Hardware
Jako řídící procesor byl zvolen ATMega 64 s krystalem 11,0592 Mhz. Frekvence
krystalu byla zvolena tak, aby časovač periferie USART měl nulovou odchylku
od požadované frekvence. Procesor je vybaven 4KB pamětí SRAM a 64KB paměti
EPROM pro programy. Velikost této paměti se jeví jako plně dostačující pro všechny
programy. Procesor je také vybaven dvěma periferiemi USART. Kdy jedna slouží pro
komunikaci s ostatními moduly a druhá je vyhrazena pro připojení k PC.
Modul je vybaven třemi dvoubarevnými LED diodami, které slouží k signalizaci
činností robotu, signalizaci chybových stavů a vybité baterie. Měření napětí v bateriích
je realizováno pomocí A/D převodníku procesoru ATMega. Převodník používá vnitřní
referencí 2,56V, proto jsou k němu baterie připojeny přes odporový dělič napětí.
9.2 Software
9.2.1 Základní myšlenka softwaru
Software je možné rozdělit do dvou nezávislých částí. Jedna zajišťuje základní
funkce a druhá řízení jízdy. Každá část programu je vykonávána při jiném přerušení,
základní funkce při přerušení Timer0 Overflow a řízení jízdy při Timer1 Overflow.
Mezi základní funkce programu patří aktualizace stavových proměnných,
mapování bludiště, hlídání okamžiku startu do zatáčky, regulace jízdy rovně a měření
napětí baterií. Stavové proměnné jsou aktuální rychlost středu nápravy, ujetá vzdálenost
a naměřené vzdálenosti překážek od senzorů. Tyto hodnoty jsou uloženy v globálních
proměnných programu. Další funkci je regulace směru jízdy pomocí PID regulátoru.
Řízení jízdy obstarává jen vlastní rozhodování, kterou cestou se robot vydá.
Rozhoduje se podle typu pole, na kterém se robot nachází. Příkazy ke změně směru
se předávají prostřednictvým proměnné g_nRouteType. Vlastní odeslaní paketu s příkazem
se již provádí v základních funkcích, které hlídají správný okamžik startu.
- 67 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Tato koncepce byla zvolena proto, aby se daly jednoduše vytvářet nové strategie
pro prohledávání a projíždění bludiště, aniž by bylo nutné řešit problémy spjaté
se správným najetím do zatáček, mapováním a pod.
9.2.2 Hlavní řídící proměnné modulu
Základní činnost hlavního modulu je řízena proměnou g_nStrategy, která popisuje
právě prováděnou činnost. Proměnná může nabývat hodnot:
•
ST_TEST – modul je v režimu testovaní připravenosti před jízdou
•
ST_SCAN – modul je v režimu prohledávání bludiště
•
ST_ERROR – modul je v nějakém chybovém stavu
•
ST_ROUTE – modul jede už prohledaným bludištěm na střed
Na startu programu je proměnná g_nStrategy rovna ST_TEST. V tomto případě
se robot nepohybuje, motory jsou vypnuté a neprobíhá aktualizace stavových proměnných.
Každému modulu je poslán pouze jeden paket DT_ECHO, kterým se testuje, zda je modul
připojen. Pokud se zjistí, že jsou připojeny všechny moduly, proměnná g_nStrategy
se nastaví na ST_SCAN a zapnou se motory. V tomto okamžiku je robot připraven
na prohledávání bludiště. Pokud některý z modulů není správně připojen, je nastaven
chybový stav ST_ERROR a LED2 se rozsvítí červeně.
Pokud je proměnná g_Strategy rovna ST_SCAN, probíhá mapování bludiště.
Stavové proměnné jsou pravidelně aktualizovány a robot prohledává bludiště zadaným
algoritmem. Jakmile robot dosáhne opět startovního pole, je celé bludiště zmapované.
Proměnná g_nStrategy je nastavena na ST_ROUTE. V tomto režimu již robot jede znáním
bludištěm do cílového pole, umístěného ve středu bludiště. Jakmile ho dosáhne, soutěžní
úloha skončila.
Další důležitou proměnnou je g_nRouteType, která vyjadřuje manévr, který
se realizuje nebo který se má realizovat. Právě na základě této proměnné a typu pole,
na kterém se robot nachází rozhoduje řízení jízdy, zda se má zatočit nebo jet rovně.
Proměnná může nabývat hodnot:
•
RT_STOP – robot stojí
•
RT_DIRECTLY – robot se pohybuje rovně ( pouze v tomto režimu probíhá
regulace jízdy rovně )
- 68 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
•
RT_TURN – robot zatáčí, kontrolu nad robotem má modul motorů, po dokončení
zatáčky je automaticky nastaven stav RT_DIRECTLY
•
RT_FOR_BARRIER – bariéra těsně před robotem
•
RT_START_TURN_LEFT – robot bude v aktuálním poli zatáčet doleva, pouze
se čeká na správný okamžik startu, jakmile nastane je poslán příkaz motorům a je
nastaveno RT_TURN
•
RT_START_TURN_RIGHT – totéž co předchozí, ale do leva
•
RT_WAIT – robot čeká na další činnost. Tento stav je nastaven při otočení
ve slepé uličce
•
RT_TURN_PLACE – probíhá otáčka na místě, kontrolu nad robotem má modul
motorů, po skončení zatáčky je nastaven stav RT_WAIT
9.3 Základní funkce programu
Obr. 9.1: Vývojový diagram Timer0
Základní funkce programu jsou vykonávány při přerušení časovače Timer0
Overflow, viz [6]. S ohledem na vzorkovací frekvenci senzorů a rychlost komunikace
- 69 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
je optimální frekvence aktualizace stavových proměnných od 10 do 20 Hz. Protože
časovač Timer0 je jen 8-bitový, nelze dosáhnout menší frekvence než 42Hz. Proto při
každém přerušení proběhne aktualizace jen některých stavových veličin. Vše popisuje
vývojový diagram na Obr. 9.1.
9.3.1 Hlídání okamžiku začátku startu do zatáčky
Správné načasovaní startu do zatáčky je pro její přesné projetí velice důležité.
Při časování startu se nelze řídit bočními senzory, protože v době kdy detekují zatáčku,
se kola robotu nachází daleko před zatáčkou viz Obr. 9.2a.
Správný okamžik startu je také závislí na poloměru zatáčky. V ideálním případě
je poloměr roven polovině délky pole, jak je vidět na Obr. 9.2b. Tento případ je ale velice
náchylný na přesnost nájezdu. Pokud se robot nepohybuje ve středu pole, dochází
na výjezdu k velkým nepřesnostem, to způsobuje velké problémy pokud na sebe navazují
dvě zatáčky. Lepších výsledku je dosaženo pokud je poloměr zatáčky menší viz Obr. 9.2c.
Poloměr musí být zvolen tak, aby bylo zaručeno, že zatáčka vždy skončí na stejném poli,
na kterém začala.
Obr. 9.2: Start zatáčky
Start do zatáčky lze provádět dvěma způsoby, první je na základě ujeté
vzdáleností od místa detekce zatáčky a druhý na základě vzdálenosti ke zdi před robotem.
Start na základě ujeté vzdálenosti lze použít vždy, ale nedosahuje takové přesnosti
jako druhý způsob. Největší slabinou tohoto způsobu jsou nepřesnosti při detekci zatáček.
Dalším problém je sčítání chyb, pokud na sebe navazuje více zatáček .
Start na základě vzdálenosti zdi před robotem, je velice přesný. Nedochází
ke sčítání chyb při navazujících zatáčkách. Při experimentech bylo dosaženo podstatně
spolehlivějších průjezdů než v předchozím případě. Velkou nevýhodou této metody je,
že nelze použít na křižovatkách typu T, kde se před robotem nenachází žádná zeď.
- 70 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Alogritmus startu do zatáčky je zjednodušeně znázorněn na Obr. 9.3.
Pokud je proměnná
g_nRouteType
rovna
RT_START_TURN_LEFT
nebo
RT_START_TURN_RIGHT, rozhodne se podle typu pole, na kterém se robot nachází,
zda se použije startování podle ujeté vzdáleností nebo startování podle vzdálenosti robotu
od zdi. Potom pokud robot ujel potřebnou vzdálenost nebo je dostatečně blízko zdi,
se pošle příkaz motorům a proměnná g_nRouteType se nastaví na RT_TURN. Jakmile
je zatáčka
skončena
proměnná
g_nRouteType
je
automaticky
nastavena
na RT_DIRECTLY.
Obr. 9.3: Zjednodušený vývojový diagram startu do zatáčky
9.3.2 Měření napájení
Měření napájení je realizováno pomocí A/D převodníku mikrokontroléru
ATMega64. Pro měření je použito vnitřní referenční napětí 2,56V. A/D převodník běží
v tvz. Single Conversion Modu, viz [6], kdy je převod odstartován, zapsáním bitu ADCS
v řídícím registru převodníku. Po ukončení měření je vyvoláno přerušení ADC Conversion
Complete.
Při přerušení je hodnota registru ADC přepočtena na napětí pomocí funkce
CalcAdcToMv(..). Ale protože jsou baterie k A/D převodníku připojeny pomocí
odporového děliče napětí, je nutno ještě spočítat výsledné napětí. Přepočet zajišťuje funkce
CalcResDivU1(..).
- 71 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Pokud napětí klesne pod minimální hodnotu, stanovenou pomocí proměnné
g_nBatteryMinADC, rozsvítí se LED1 červeně, jinak svítí zeleně.
Aktuální napětí lze také získat pomocí paketu DT_MAIN_GET_BATTERY
popsaného níže.
9.3.3 Regulace směru jízdy
K regulaci jízdy rovně je použit softwarově řešený PID regulátor. Regulační
odchylka je rozdíl naměřené vzdálenosti levého a pravého senzoru a akční zásah
je parametr pro paket DT_MOT_DIR_REG, popsaného v 8.4.
Obr. 9.4: Regulace směru
případ 2
Obr. 9.5: Regulace směru
případ 1
Regulaci směru můžeme rozdělit do dvou případů, jak ukazují obrázky Obr. 9.4
a Obr. 9.5. První případ je, když jsou zdi po obou stranách. Regulační odchylka se počítá
jako:
e=l P−l L
Rov. 9.1
V druhém případě, je zeď jen na jedné straně. Potom je vzdálenost k chybějící zdi
nahrazena ideální vzdáleností, která se vypočítá podle:
l ideal =
H pole
−h senz
2
Rov. 9.2
kde je lideal – ideální vzdálenost od zdi, Hpole – šířka pole, hsenz – vzdálenost senzoru
od osy robotu.
Pro zrychlení výpočtu jsou simulována čísla s pevnou desetinou čárkou s přesností
na dvě desetinná místa. To se ukázalo jako dostačující přesnost.
Protože není znám přenos soustavy, nastavení PID regulátoru bylo provedeno
experimentálně.
- 72 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
9.3.4 Mapování bludiště
Pro mapování bludiště je nejdříve nutno popsat jeho uložení v paměti. Bludiště
je uloženo jako dvourozměrné pole, jehož prvky jsou tvořeny 8-bitovým číslem
bez znaménka. Zdi jsou potom reprezentovány příslušným bitem. Počátek souřadného
systému je umístěn v levém horním rohu. Bludiště je uloženo v proměnné g_arMaze.
Způsob uložení bludiště, orientace globálních směrů a označení zdí je zobrazen na Obr.
9.6.
Bity reprezentující zdi:
•
MAZE_GL_TOP – bit 0
•
MAZE_GL_RIGHT – bit 1
•
MAZE_GL_BOTTOM – bit 2
•
MAZE_GL_LEFT – bit 3
•
MAZE_GL_UNKNOWN – bit 4, nezmapované pole.
Obr. 9.6: Způsob uložení bludiště
Dále bylo nutné definovat globální směry v bludišti, ty vyjadřují směr pohybu
robotu v bludišti. Směr se vždy aktualizuje po dokončení zatáčky, pomocí funkce
UpdateGlobalDir(..).
Globální směr je uložen v proměnné g_nGlobDir, jedná se o 8-bitové bez
znaménkové číslo, nabývá hodnot:
•
GL_RIGHT = 1
•
GL_BOTTOM = 2
•
GL_LEFT = 4
•
GL_TOP = 8
- 73 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Na začátku programu je pro všechny prvky pole nastaven bit DIR_UNKNOWN
a pro krajní pole jsou nastaveny bity reprezentující okraj bludiště.
Dalším klíčovým prvkem pro mapování je zjištění typu pole, na kterém se robot
nachází. Typ pole, které je možno detekovat je omezen množstvím senzorů, v aktuální
konfiguraci, lze detekovat zeď vpředu a
po stranách robotu, případně zeď vpředu
na následujícím poli, jak ukazuje Obr. 9.7.
Typ aktuálního pole je uložen v proměnné
g_nCellType, která je typu 8-bitové bez
znaménkové
číslo,
kde
jsou
zdi
opět
reprezentovány příslušným bitem. Umístění
zdí je v tomto případě popsáné vzhledem
k souřadnému systému, který je spojený
s robotem.
Obr. 9.7: Typ pole na kterém se robot nachází
•
DIR_LEFT – bit 0, zeď na levé straně
•
DIR_RIGHT – bit 2, zeď na pravé straně
•
DIR_FORWARD - bit 1, zeď před robotem
•
DIR_NEXT_FOR – bit 4, zeď před robotem na následujícím poli.
•
DIR_BARRIER – bit 5, překážka těsně před robotem (používá se jako pojistka
proti nárazu robotu)
Pro mapování se používají pouze první tři bity, ostatní slouží k navigaci jízdy
bludištěm.
Transformaci typu pole ze souřadného systému robotu do globálního souřadného
systému bludiště, zajišťuje funkce TranCellLcToGL(..).
Algoritmus vlastního mapování lze rozdělit do dvou častí. Mapování rovného
úseku a mapování zatáčky. Mapování rovného úseku je prováděno na základě ujeté
vzdálenosti. Mapování zatáčky se děje vždy při jejím startu. Úkolem mapování je také
aktualizovat polohu robotu v bludišti. Ta je uchovávána v proměnných g_nMousePos_x
a g_nMousePos_y.
Algoritmus mapování rovného úseku je zjednodušeně zobrazen na Obr. 9.8.
Nejdříve se provede test zda byla ujeta minimální vzdálenost, kdy budou senzory již
- 74 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
v novém poli. Pokud ano musí se otestovat, zda pole již nebylo mapováno. Pokud nebylo,
namapuje se pomocí funkce MapRoute(..). Potom se čeká dokud se neujede maximální
vzdálenost pro dosažení nového pole. Pokud je ujeta, vynuluje se ujetá vzdálenost a proces
se opakuje.
Funkce MapRoute(..) nejdříve vypočítá novou polohu robotu na základě směru,
kterým se robot pohybuje, pomocí proměnné g_nGlobDir, která byla popsána výše. Takto
vypočítaná poloha je vložena do proměnných g_nMousePos_x a g_nMousePos_y. Potom
vybere první tři bity z proměnné g_nCellType, která vyjadřuje typ pole na kterém se robot
nachází, takto získané informace transformuje do globálních souřadnic a uloží
je do proměnné g_arMaze.
Obr. 9.8: Zjednodušený algoritmus mapování rovného úseku
Význam minimální a maximální vzdálenosti je zobrazen na Obr. 9.9. Toto rozmezí
vzdáleností zajistí správné mapování při nepřesných nájezdech nebo výjezdech ze zatáček
nebo vjetí do slepé uličky, kdy robot nemusí vždy urazit vzdálenost celého pole. Volba
těchto vzdáleností je poměrně složitou úlohou. Zejména je třeba dbát na správné nastavení
minimální vzdálenosti. Pokud je minimální vzdálenost příliš velká, dojde k tomu,
že senzory robotu se nachází v jiném poli než kola robotu, to vede k namapování
následujícího pole na aktuální. Maximální vzdálenost by v ideálním případě měla být
doplňkem do velikosti pole, ale experimenty ukázaly, že se dosahuje lepších výsledků
pokud je o několik procent menší ( asi 5÷8% ). Pokud je maximální vzdálenost příliš malá
dochází na dlouhých rovinkách ke špatnému počítání ujetých polí.
- 75 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Obr. 9.9: Význam max. a min. vzdálenosti pro mapovaní
bludiště
Mapování zatáčky je nutné proto, že v době okamžiku startu do zatáčky nebyla
uražena minimální vzdálenost, ale přitom je zaručeno že robot se již nachází na novém
poli. Mapování je prováděno pouze funkcí MapRoute(..), která vypočítá novou polohu
a namapuje pole. Při výjezdu ze zatáčky se již vše mapuje předchozím algoritmem.
9.4 Řízení jízdy
Řízení jízdy je vykonáváno při přerušení Timer1 Overflow. Algoritmus jízdy
se vybírá podle proměnné g_nStategy, která byla popsaná výše.
9.4.1 Prohledávání bludiště
Jak již bylo popsáno v pravidlech soutěže, bludiště nesmí obsahovat osamocené
ostrůvky, tudíž ho lze projet pravidlem pravé nebo levé ruky. Zjednodušený algoritmus
prohledávání je na Obr. 9.10.
Pokud
se
robot
pohybuje
rovně
(proměnná
g_nRouteType
se
rovná
RT_DIRECTLY), vyhodnotí se podle proměnné g_nCellType tvar pole na kterém se robot
nachází. Pokud je před robotem překážka, robot zastaví a proměnná g_nRouteType
je nastavena na RT_FOR_BARRIER. Pokud se robot nachází na poli na kterém může jet
doprava, proměnná g_nRouteType se nastaví na RT_START_TURN_RIGHT. Start
do zatáčky potom proběhne při přerušení Timer0 Overflow. Jakmile je zatáčka dokončena
je opět automaticky nastaven stav RT_DIRECTLY. Pokud se robot nachází na poli,
na kterém nemůže jet doprava ani rovně, zatočí se obdobným způsobem doleva.
- 76 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Když se robot pohybuje v zatáčce, tzn proměnná g_nRouteType se rovná
RT_TURN, hlídá se pouze aby robot nenarazil do zdi. Pokud se k ní přiblíží příliš, je poslán
příkaz k zastavení.
Obr. 9.10: Zjednodušený vývojový diagram algoritmu prohledávání
- 77 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Pokud se nachází před robotem zeď a robot stojí, tzn. že g_nRouteType je rovna
RT_FOR_BARRIER, testuje se zda jde o slepou uličku. Pokud ano pošle se příkaz
pro otočení na místě.
Vše probíhá tak dlouho dokud se robot nevrátí zpět na místo startu.
9.4.2 Nalezení optimální cesty
Po prohledání bludiště je třeba nelézt optimální cestu. Pro nalezení optimální
cesty existuje několik algoritmů, v našem případě je použit algoritmus, který je popsaný
na Obr. 9.11.
Algoritmus spočívá v ohodnocení polí bludiště, kdy ohodnocování probíhá
od cílového pole ke startovnímu. Cílové pole ohodnotíme jedničkou viz Obr. 9.11a,
v našem
případě
v prostředním
cílové
čtverci
pole
bludiště.
leží
Potom
ohodnotíme sousední pole, na které je
možné dojet číslem větším o jedničku, viz
Obr. 9.11b. Takto postupujeme tak dlouho,
dokud nedosáhneme startovního pole které
je v našem případě v levém horním rohu,
viz Obr. 9.11c. Nejkratší cestu potom
nalezneme tak, že od startovního pole
vybíráme vždy následující pole, které je
menší než aktuální, viz Obr. 9.11d.
Tento algoritmus nám zajistí vždy
nalezení nejkratší cesty, pokud nějaká Obr. 9.11: Nalezení optimální cesty bludištěm
existuje. Pokud existují dvě totožné cesty,
jsou nalezeny obě. Tento algoritmus je rychlý a nenáročný na pamět. Požadavky na paměť
jsou závislé na velikosti bludiště. V případě že je bludiště 16x16 polí, potřebuje pouze 256
bytů.
Ohodnocení bludiště zajišťuje funkce SolveMaze(), která na základě zmapovaného
bludiště (g_arMaze), ohodnotí pole reprezentované proměnnou g_arMazeSol. Vývojový
diagram funkce je na Obr. 9.12.
- 78 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Obr. 9.12: Vývojový diagram nalezení optimální cesty
bludištěm
Na začátku funkce je nastavené cílové pole na hodnotu 1. K procházení
dvourozměrného pole je použito dvou cyklů. Pokud je nalezeno pole, které se právě
expanduje, provede se ohodnocení sousedních polí. Vše se opakuje dokud není
ohodnoceno startovní pole.
9.4.3 Řízení naplánované jízdy bludištěm
V případě, že máme nalezenou optimální cestu bludištěm, je nutno vytvořit mapu,
podle které se robot bude pohybovat. Mapa je také tvořena dvourozměrným polem.
Na základě hodnot jednotlivých prvků pole se rozhoduje, zda robot pojede rovně nebo
zatočí. Směr zatáčky je v lokálních souřadnicích robotu. Prvky pole mohou nabývat
hodnot:
- 79 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
•
0 – pokračovat ve směru jízdy
•
1 – zatočit doleva
•
2 – zatočit doprava
TOMÁŠ PASEKA
Mapa pro případ bludiště, které bylo popsané v kap. 9.4.2.
je na Obr. 9.13.
Vytvoření
mapy
zajišťuje
funkce
CreateRoute(). Obr. 9.13: Mapa cesty
Pro úsporu paměti lze pro ohodnocené bludiště a mapu použit stejné pole. Ale protože
mikrokontrolér ATMega 64 má paměti dostatek pro snadnější testování se mapa ukládá
do dalšího dvourozměrného pole, které je reprezentováno proměnnou g_arRoute.
Obr. 9.14: Vývojový diagram jízdy podle mapy
Jakmile je mapa vytvořena robot se podle ní pohybuje pomocí algoritmu, který je
na Obr. 9.14. Pokud robot jede přímo, proměnná g_nRouteType se rovná RT_DIRECTLY
- 80 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
a lze zatočit vlevo nebo vpravo, rozhodne se podle pozice robotu v bludišti a podle mapy,
zda se zatočí vlevo nebo vpravo. V okamžiku detekce zatáčky je ale nutné přičíst jedno
pole, podle směru, jízdy k aktuální pozici, protože senzory se již nachází na jiném poli,
než kola robotu. Proces se opakuje dokud není dosaženo cíle. Pokud se robot nachází
v zatáče, proměnná g_nRouteType se rovná RT_TURN, je nutné provádět ochranu proti
nárazu robotu do zdi.
9.5.1 Usart 0
DT_ECHO
9.5.2 Usart 1
DT_ECHO
DT_MAIN_GET_BATTERY
Žádost o poslaní napětí na baterii. Modul vrátí paket typu DT_MAIN_BATTERY
•
byte 0 – horní byte 16-bitového čísla, napětí v mV
•
byte 1 – dolní byte 16-bitového čísla, napětí v mV
DT_MAIN_GET_STATUS
Žádost
o
poslání
statistiky
komunikace.
Modul
vrátí
paket
typu
vrátí
paket
typu
DT_MAIN_STATUS, velikost dat jsou 4 byty.
•
byte 0 – 8-bitové číslo, počet CRC chyb na USART 0
•
byte 1 – 8-bitové číslo, počet timeoutů na USART 0
•
byte 2 – 8-bitové číslo, počet CRC chyb na USART1
•
byte 3 – 8-bitové číslo, počet timeoutů na USART1
DT_MAIN_GET_STATE_VAR
Žádost
o
poslání
stavových
proměnných.
Modul
DT_MAIN_STATE_VAR, velikost dat je 10 bytů.
•
byte 0 – horní byte 16-bitového čísla, rychlost motorů [krok/s]
- 81 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
•
byte 1 – dolní byte 16-bitového čísla, rychlost motorů [krok/s]
•
byte 2 – horní byte 16-bitového čísla, ujetá vzdálenost
•
byte 3 – dolní byte 16-bitového čísla, ujetá vzdálenost
•
byte 4 – horní byte 16-bitového čísla, vzdálenost naměřená levým senzorem
•
byte 5 – dolní byte 16-bitového čísla, vzdálenost naměřená levým senzorem
•
byte 6 – horní byte 16-bitového čísla, vzdálenost naměřená prostředním
senzorem
•
byte 7 – dolní byte 16-bitového čísla, vzdálenost naměřená prostředním
senzorem
•
byte 8 – horní byte 16-bitového čísla, vzdálenost naměřená pravým senzorem
•
byte 9 – dolní byte 16-bitového čísla, vzdálenost naměřená pravým senzorem
DT_MAIN_GET_ERROR_REP
Žádost o poslání hodnot klíčových proměnných. Modul vrátí paket typu
DT_MAIN_ERROR_REP, velikost dat je 6 bytů.
•
byte 0 – 8-bitové číslo, proměnná g_nModulesInfo
•
byte 1 – 8-bitové číslo, proměnná g_nRouteType
•
byte 2 – 8-bitové číslo, proměnná g_nCellType
•
byte 3– 8-bitové číslo, proměnná g_nGlobDir
•
byte 4 – 8-bitové číslo, rezervováno
•
byte 5 – 8-bitové číslo, rezervováno
DT_MAIN_SET_DIR_PID
Nastavení PID regulátoru pro jízdu rovně
Data:
•
byte 0 - horní byte 16-bit čísla, proporcionální složka
•
byte 1 - dolní byte 16-bit čísla, proporcionální složka
•
byte 2 - horní byte 16-bit čísla, integrační složka
•
byte 3 - dolní byte 16-bit čísla, integrační složka
•
byte 4- horní byte 16-bit čísla, derivační složka
•
byte 5 - dolní byte 16-bit čísla, derivační složka
- 82 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
DT_MAIN_GET_DIR_PID
Žádost o poslání nastavení PID regulátoru. Modul vrátí paket typu
DT_MAIN_DIR_PID, velikost dat je 6 bytů.
•
byte 0 - horní byte 16-bit čísla, proporcionální složka
•
byte 1 - dolní byte 16-bit čísla, proporcionální složka
•
byte 2 - horní byte 16-bit čísla, integrační složka
•
byte 3 - dolní byte 16-bit čísla, integrační složka
•
byte 4- horní byte 16-bit čísla, derivační složka
•
byte 5 - dolní byte 16-bit čísla, derivační složka
DT_MAIN_GET_POSITION
Žádost
o
poslání
pozice
robotu
v
bludišti.
Modul
vrátí
paket
DT_MAIN_POSITION, velikost dat je 2 byty.
•
byte 0 – 8-bit číslo, souřadnice x
•
byte 1 - 8-bit číslo, souřadnice y
DT_MAIN_GET_MAZE
Žádost o poslání bludiště. Modul vrátí paket DT_MAIN_MAZE, velikost dat
je závislá na velikosti pole.
DT_MAIN_GET_MAZE_SOL
Žádost
o
poslání
ohodnoceného
bludiště.
Modul
vrátí
paket
DT_MAIN_MAZE_SOL, velikost dat je závislá na velikosti pole.
DT_MAIN_GET_MOT_COMST
Žádost o poslání statistiky komunikace motorů, modul vrátí paket popsaný
v kap. 8.4.
DT_MAIN_GET_SEN_COMST
Žádost o poslání statistiky komunikace senzorů, modul vrátí paket popsaný
v kap. 7.4.
- 83 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
9.6 Testovací software na PC
Modul hlavního CPU má jako všechny ostatní svoji testovací aplikaci. Aplikace
pro komunikaci využívá třídu CseriálPortMM. Jméno aplikace je MainBoard Test.
Aplikace umožňuje posílání všech popsáných paketů a zobrazování jejich
výstupů. Také umožňuje stažení namapovaného a ohodnoceného bludiště a jeho vykreslení
včetně číselných hodnot polí. Lze pomoci ní také nastavovat PID regulátor pro regulaci
jízdy rovně.
- 84 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
10 Závěr
Tento první model autonomního robotu ukázal, že zvolená koncepce je dobrá.
Podařilo se sestavit robot s dobrým poměrem cena / výkon. Jediným problémem který
se zatím vyskytl je schopnost senzorů měřit pouze do vzdálenosti od 4 do 30cm. Tento
nedostatek lze odstranit přidáním senzorů.
Funkčnost jednotlivých řídících programů modulů byla experimentálně ověřena
v bludišti. Navržené algoritmy se ukázaly jako dobré, s výjimkou mapování bludiště, které
někdy vykazuje chyby při detekci typu pole. Chyba je nejspíše způsobena sčítáním
nepřesností při navazujících zatáčkách. Pro odstranění tohoto nedostatku by bylo dobré,
robot vybavit bezdrátovým modulem pro připojení k PC, nebo vytvořit vhodný simulační
software, aby bylo možno sledovat hodnoty stavových veličin v průběhu jízdy.
Poděkování
Tato práce je podporována projektem MSM 0021630518 „Simulační modelování
mechatronických soustav“.
- 85 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Použitá literatůra
[1] David Matoušek: Práce s mikrokontroléry ATMEL AVR, Technická literatura
BEN 2003
[2] Vladimír Váňa: Mikrokontroléry ATMEL AVR a jazyce C, Technická literatura
BEN 2003
[3] Doc. Ing. František Šolc Csc, Ing. Zdeněk Žalud Ph.D.: Robotika, VUT v Brně
FEKT 2002
[4] Dirk Louis, Petr Mejzlík, Miroslav Virius: Jazyky C a C++ podle normy
ANSI/ISO., Grada 1999
[5] Datasheet: ATMega8(L), Atmel Corporation 2004
[6] Datasheet: ATMega64(L), Atmel Corporation 2004
[7] David J. Kruglinski: Mistrovství ve Visual C++, Computer Press 1999
[8] Robotika.sk, http://www.robotika.sk
[9] Peter Harrison: Micromouse Information Centre,
http://micromouse.cannock.ac.uk
[10] Microrobot Corporation, http://www.microrobot.com/
[11] Zbyněk Winkler: Odometrie, http://robotika.cz/guide/odometry/cs
[12] MSDN Library, Microsoft 2003
[13] Ing. Tomáš Marada Ph.D., Ing. Pavel Houška Ph.D., Tomáš Paseka: SMALL
AUTONOMOUS ROBOT FOR PRACTICAL VERIFYING OF ARTIFICIAL
INTELLIGENCE METHODS, Svratka 2006
- 86 -
ÚMTMB VUT V BRNĚ
DIPLOMOVÁ PRÁCE
TOMÁŠ PASEKA
Seznam příloh
Přílohou je CD ROM se zdrojovými soubory, elektronickou podobou této práce a
vývojovými nástroji. CD ROM obsahuje:
Zdrojové soubory
•
pro komunikační protokol
•
pro modul motorů
•
pro modul senzorů
•
pro modul hlavního CPU
•
pro aplikaci Motor Test
•
pro aplikaci Senzors Test
•
pro aplikaci Senzors Char
•
pro aplikaci MainBoard Test
Aplikace
•
Motor Test
•
Senzors Test
•
Senzors Char
•
MainBoard Test
Vývojové nástroje
•
WinAVR
•
PSPad
Pzn. Všechny přiložené vývojové nástroje jsou volně šiřitelné.
- 87 -

VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ Diplomová práce

Transkript

Podobné dokumenty

Dokumentace

Ovladače v OS a v ŘS REX

BEGA220A

Návrh webového systému řízení malé společnosti

31 SCS - České vysoké učení technické v Praze

- Katedra technické a informační výchovy PdF UP v

Zde - Martin Uhlík

Internet Embedded MCU W7200 Datasheet

DOPLŇUJÍCÍ TEXT PRO DĚTI VE VĚKU 15 - 18 LET

Rozbor řešení grantového projektu a celkové shrnutí Mobile robot

Koncept pokročilého návrhu ve VHDL

ABSOLVENTSKÁ PRÁCE

Zkušenosti s instalacemi OBU ETCS

KP ETCS - informace o projektu

Návod pro obsluhu TOPENÍ A OHŘEV TEPLÉ VODY

1/2014