Robot BOB

Na robotovi se silnou měrou podílel kolega roboťák/fw/sw programátor Czenda.

Robot Bob je/byl semestrální projekt pro předmět Základy Elektromechaniky pro Řízení, a je/byl určen pro soutěž ISTROBOT 2006, přesněji pro kategorie Stopár (Pathfollower) a Myš v bludisku (Micromouse). Kvůli časovému nedostatku a na počátku špatně zvolené koncepci (pohon na základě krokových motorů z 5,25" mechanik) se nestihla realizace pro kategorii Micromouse (i když algoritmus byl vyvinut) a BOB soutěžil jen v kategorii Pathfollower. Dokonce nám 5 dní před soutěží klekl procesor ATmega128 - tak sme narychlo slepili náhradu s ATmega16, takže výmlum máme dost [:]-)

Základem kategorie Pathfollower je jízda po černé čáře, která je vyznačena na bílém podkladu. Na trase jsou tři druhy překážek (pokud se nepočítá vlastní vodící čára jako zdroj komplikace) :

Tunel - nápní činnosti této překážky je omezení světelných podmínek na trase. Avšak při použití optoreflexního principu detekce vodící čáry tato překážka nestanoví fakticky žádny problém.

Přerušená čára - v určitém úseku čára chybí, je nutno tento úsek projet a čáru najít. Předpokladem je, že přerušená čára bude na rovném úseku, proto stačí "jet rovně".

Cihla - asi nejtěžší typ překážky na trase (jedná se opravdu o kus oranžového zdiva). Drtivá většina robotů, kteří trasu neprojeli skončili právě na cihle, v jednom pokusu i BOB! Snad nejprimitivnější způsob objetí cihly je jí jen najít a podle pevně dané trasy ji objet, soutěžní trať se v průběhu soutěže nemění, takže to nestanoví problém.

Senzorika robota je uzpůsobena pro překonání soutěžní trasy. Takže dva hlavní detektory jsou pro sledování čáry a pro detekci cihly.

Snímač pro detekci černé čáry. Základním prvkem je optoreflexní závora s tranzistorovým výstupem CNY70, pracujícím v infračervené oblasti spektra okolo vlnové délky 950nm. CNY70 je složen z infraLED a fototranzistoru.

Celý snímač na čáru je složen z pěti kusů senzorů CNY a jeho šířka odpovídá šířce přední části konstrukce.

Funkce jednotlivých senzorů:

Okrajové senzory OR1 a OR5 jsou určeny k prudké změně směru (zatáčení), kdy jedno kolo jede vpřed a druhé vzad -> otáčení na místě. Hlavní náplní práce těchto senzorů je aby robot neztratil čáru v relativně prudkých zatáčkách (malý poloměr zatáčení), a současně aby případně ztracenou čáru nalezl. OR2 a OR4 jsou čidla určena pro jízdu po čáře, jejich vzdálenost je přibližně rovna tloušťce čáry. Tyto středová čidla jen seřizují směr jízdy po čáře, příslušné kolo totiž jen zpomalí svojí rychlost oproti kolu druhému. Nedochází tudíž k otáčení na místě, ale k zatáčení s určitým poloměrem. Z toho je jasné, že hlavní náplní funkce je rychlá jízda po rovném úseku. Středový senzor OR3 je na samotnou detekci čáry to rozhoduje zda-li se po čáře jede (rychle) a nebo se čára vyhledává (menší rychlost), popřípadě se zrovna zatáčí.

Modul elektroniky snímače čáry zde:

InflaLED všech senzorů jsou řazeny sériově a jsou napájeny proudem cca 20mA (závislé na aktuálním stavu akupacku - 9 x 1,2V NiMH). Výstupy z jednotlivých CNY70 jsou odebírány z jejich kolektorů, jako pull-upy jsou řazeny odpory 22k, kterých hodnota byla optimalizována pro tento daný typ aplikace. Jedná se hlavně o logickou kompatibilitu s MCU. Při odrazu od bílé desky je hodnota výstupního napětí cca 150mV a při detekované čáře je cca 4,25V -> což MCU AVR spolehlivě interpretuje jako L a H.

Dálkoměry / proximitní čidla. Pro detekci překážky (cihla) a stěn byl vytvořen jednoduchý IR dálkoměr z inflaLED HSDL4420 a IR optického přijímače SFH5110-36.

Modul dálkoměru / proximitního snímače zde:

Dálkoměr pracuje na principu pseudo-změny citlivosti IR optického přijímače SFH5110-36, který má nejvyšší citlivost na IR paprsek modulovaný nosným kmitočtem 36kHz, proto změnou frekvence nosné frekvence se dá dosáhnout měření vzdáleností na reflexním principu. Poměr nosné frekvence k základní nosné f/f0 pak odpovídá citlivosti. Pro f/f0 = 1 je citlivost nejvyšší, tudíž lze detekovat předměty, které jsou nejvzdálenější.

Při použití fixní frekvence se dá taky senzor modifikovat na obyčejný proximitní senzor (senzor přiblížení - Near Proximity Sensor), kdy výstup zrcadlí stav: je předmět (je odraz paprsku) nebo není. Jistý problém však nastává při určité vzdálenosti kdy výstup začne kmitat (neexistuje ostrá hranice je odraz není odraz), proto je na desce procesoru přidán filtr 1. řádu v podobě RC integračního článku s frekvencí cca 10Hz a společně s hysterezí Schmittova KLO (při čtení stavu portu MCU se čte příslušný pin přes integrovaný S-KLO) tento neduh značně eliminuje.

Senzory jsou použity tři a to jeden ve předu a dva po stranách.

Pohon. Pro pohyb robota jsou požity dva stejnosměrné motory s ušlechtilým komutátorem a integrovaným kvadraturním dekodérem Faulhaber 2224U006SR IE2-512.

Některé základní technická data:

Výstupní moment z hřídele motoru je přiveden na jednoduchou převodovku sestávající z ozubeného kola na hřídeli motoru (12 zubů) a ozubeného kola fixovaného k pojezdovému kolu se 100 zuby - převod je tedy 12/100. Pohon je koncipován jako diferenciální se dvěma pojezdovými koly a kluznými plochami ve předu a vzadu.

Elektronika pohonu byla koncipována jako samostatný modul pro řízení motorů a zpracování dat z inkrementálních kvadraturních enkoderů otáček. Snímače otáček integrované v motoru Faulhaber poskytují kvadraturní signál v podobě dvou pravoúhlých průběhů posunutých vzájemně o p/2. Tyto signály se jednoduše pomocí D-KLO (74HCT74) převedou na signály směru a frekvence, která odpovídá úhlovému kmitočtu hřídele motoru a počtu drážek inkrementálního senzoru. Obrázek naznačuje převod kvadraturních signálů A a B, kde signál A je přiveden na vstup DATA D-KLO 1/2 74HCT74 a signál B je CLK. Na výstupu Q je pak signál DIR odpovídající směru otáčení.

Pro řízení motorů je použit monolitický H-Bridge Motorola (Freescale) MC33886D s výstupním proudem až 5 A.

U motorů se řídí jejich smysl otáčení a pomocí PWM i jejich otáčky. K nadřazenému systému se budiče motorů připojují pomocí signálů IN1, IN2 a /D2. /D2 je vstup Disable a slouží k zakázání/povolení obvodu. Logická kombinace na vstupech IN1 a IN2 pak určuje směr otáčení nebo elektrodynamickou brzdu. Pokud je IN1 != IN2 pak se motor otáčí určitým smyslem a pokud je IN1 == IN2 pak je motor v elektrodynamické brzdě. Řízení otáček pak zajišťuje PWM signál signálu IN1 => motor se buď otáčí nebo je elektrodynamicky bržděn, při použití dostatečně vysoké frekvence PWM signálu je ono střídání stavů patrné jako pokles otáček. Řízením střídy PWM je tudíž dosaženo řízení otáček. Na desce je ještě lineární stabilizátor 7805 (5V/1,5A), která zajišťuje stabilních 5V pro napájení MCU, 74HCT74 a senzorů atd. Jako zdroj pro celého robota je použito akupacku, sestávajícího s 9ti kusů prizmatických akumulátorů NiMH 880mAh. Což poskytuje napětí cca 12,6V, kterým jsou napájeny budiče, tudíž i motory a IRLED u snímače čáry.

Celý plošný spoj je uzpůsoben k připojení druhého horního plošného spoje. Propojení je pomocí tří kusů jumper lišt, které lze funkčně rozdělit na dvě pro řízení a zpracování dat z motorů a třetí je napájecí. Plošný spoj pak ještě obsahuje konektory na připojení motorů a napájecí konektor.

Schéma desky pohonu zde:

Elektronika procesoru. Jádrem celého robota je RISC MCU Atmel AVR ATmega16-16PI.

Senzor čáry je celý napojen na PORTA kde je jako alternativní funkce připojen multiplexer ADC. Řízení pohonu zajišťují výstupy jednotek OC 16-bit C/T1, které jsou nastaveny jako PWM signály. C/T1 má jako zdroj clk přímo nedělený systémový clk 16MHz. Hodnota maxima čítače je dána hodnotou v registru IC1 = 1000, takže při čítání nahoru a dolu je frekvence PWM signálu dána:

O střídě pak rozhoduje hodnota v registrech OC1A a OC1B, která se pohybuje v intervalu <0,IC1>, při 0 je na výstupu L a při hodnotě IC1 a vyšší ze výstup trvale v H. Při použití takto nízké frekvence PWM (dáno i omezením budiče na max 10kHz), je slyšet pískání rotorů dáno magnetostrikčním jevem.

Sběr dat z inkrementálních čidel zajišťují vstupy externích přerušení INT0 a INT1. Přerušení jsou generovány při náběžných hranách signálu kvadraturního clk QCLKn (jedná se přímo o signál z enkoderu motoru Faulhaber) a v rutině přerušení se načítá hodnota smyslu otáčení DIRn a dle toho se inkrementuje nebo dekrementuje počítací registr, takto je sw emulován vratný čítač. C/T2 je použit pro generování signálu 36kHz, konkrétně jeho funkce CTC (Clear Timer on Compare match - Autoreload). Signály z proximitních čidel jsou pak načítány volnými piny. Pro monitorování stavu je použita jednotka USART a její signály RxD a TxD, komunikace je na 57,6 kbps do PC se spuštěným příslušným sw.

Schéma desky procesoru zde:

Popis algoritmů

Algoritmus měření otáček

Kvadraturní clk jsou přivedeny na vstupy externích přerušení kde je v rutině obsluhy přerušení vytvořen sw-ě jeden čítač, který čítá s vlivem na směr a jeden nezávislý čítač který čítá přicházející impulsy. Počet impulsů za danou dobu je přímo úměrný otáčkám motoru. Kvůli 512 tikům na otáčku je frekvence zjišťování stavu nezávislého čítače 250Hz (dáno C/T0 v režimu CTC - Autoreload). V rutině přerušení C/T0 se proto stav čítače přepočítá pomocí konstant na otáčky a současně se spustí algoritmus řízení otáček.

Algoritmus řízení otáček

Pro základní potřebu jízdy byl vytvořen algoritmus, který reguluje přímo otáčky motoru, nejen jejich PWM. Řízení je založeno na poměru aktuálních skutečných otáček s otáčkami požadovanými, výsledkem čehož je číslo. Pokud ono číslo leží v intervalu 0 až 1 (skutečné otáčky jsou nižší) toto číslo se interpretuje pro nastavení PWM (0 - 100%). A pokud je číslo vyšší než 1 (otáčky jsou vyšší něž požadované) zapne se elektrodynamická brzda Tím je dosaženo neustálé snahy systému regulovat otáčky na požadovanou hodnotu. Praktické testy ukázaly jednu nevýhodu - nikdy není dosaženo oněch požadovaných otáček. A jednu výhodu hodnota otáček na které se motor vyreguluje je relativně stabilní. Prakticky pro požadované 2000rpm jsou skutečné cca 1600rpm (jedná se o otáčky motoru nikoli o otáčky pojezdového kolečka). Robot je proto schopen relativně jezdit rovně, toto je potřeba pro překonání přerušené vodící čáry, u které se předpokládá, že bude přerušena v rovném úseku - pro toto algoritmus vystačuje.

Algoritmus se používá pro nastavení čtyř rychlostí: Rychle - pokud je detekována čára jen na středovém čidlu OR3 otáčí se oba motory touto maximální rychlostí - jede se rychle vpřed. Konkrétní číselné hodnoty jsou variabilní. Standardně - touto rychlostí se jede při vyhledávání čáry, jelikož není algoritmus dokonalý prakticky dochází k zatáčení, proto se jede pomaleji, aby poloměr kružnice zatáčení byl větší. Pomalu - této rychlosti se používá pro jemné dolaďování jízdy po vodící čáře - příslušná strana robota jen zpomalí aby se jízda po čáře srovnala (čára je na středových čidlech OR2 nebo OR4) a při ostrém srovnávání na čáru kdy příslušná strana jede touto rychlostí dozadu (čára je na okrajových čidlech OR1 a OR5).

Algoritmus jízdy po čáře

Je sestaven velmi jednoduše pomocí switch(){}, kde jsou jednotlivým možnostem (case) na snímači čáry přiřazeny odpovídající kombinace rychlostí a smyslu otáčení levého nebo pravého motoru.

a)

b)

c)

d)

Po aplikaci masky 0b11111000 (dá se použít i log. shift) na PINA (registr ve kterém je hodnota na portu A) se jeví situace a) jako 0b00100000 - což se interpretuje jako nastavení na obou motorech maximální rychlosti (rychle) - jede se po vodící čáře. Situace b) se načte jako 0b00010000 - čára je vpravo, tudíž se musí zatočit mírně doprava. To znamená levý motor plnou rychlostí vpřed (rychle) a pravý motor pomaleji (pomalu). Dle c) je patrné, že chybí relativně málo a čára je ztracena (načtena hodnota je 0b00001000), proto se musí zatočit relativně prudce, levý motor jede rychle vpřed a pravý reverzuje (pomalu) - dochází k mimo středovému otáčení na místě. Situace d) se interpretuje stejně jako situace c) - 0b00011000. Celkově je všeho všudy možno až 32 kombinací ale některé jsou nesmyslné jako např. 0b10101000, atd. takové případy jsou zahrnuty v sekvenci default kde se jede standardní rychlostí vpřed - vyhledávání čáry. A jedna kombinace je velmi důležitá a to 0b11111000, když není ani na jednom senzoru odraz tak se stojí, tohle se velmi hodní při najetí ke hraně stolu - robot ze stolu nespadne.

Algoritmus objetí překážky (cihly)

Zde se využívá dat z počítacích registrů, které počítají tiky jednotlivých kvadraturních enkoderů se vzhledem na jejich směr. Otočka na místě je pak dána určitým počtem tiků na jednom kole plus a na druhém minus. Popojetí o určitý úsek je pak jasný.

Algoritmus je uzpůsoben dané cihle a konstanty délek úseček byly stanoveny experimentálně. Jedná se o dojetí k cihle kde přední proximitní čidlo zaznamená její přítomnost, zastaví se, následuje otočka o 90° doprava popojetí mimo hranu cihly, otočka o 90° doleva, popojetí mimo boční hranu cihly otočka o cca 45°, jízda dokud se nenarazí čidlem OR3 na čáru, otočka o 45° a spuštění algoritmu jízdy po čáře.

Závodní fw: racing.c

Lepší fw, neumí ale objet cihlu: nobrick.c

Konstrukce je ocelová vytvořená z 1mm plechů a jedná se o konstrukci z robofotbalisty, složena je z pěti dílů spojených bodováním. Dole jsou mimoose umístěné motory, které jsou připevněny šrobky M2 k bočním dílům, na bocích jsou i přibodované osy kol. Nad motory je akupack připevněný suchým zipem k dílu který spojuje přední a zadní díl, nejvíc nahoře jsou plošné spoje, které jsou umístěné na distančních sloupcích. Přední a zadní díl jsou na originélním robotu použity na "kopání" do míčku, nám dobře posloužil přední díl na kterém je lepící pistolí připevněn senzor čáry a sekundovým lepidlem přední dálkoměr/proximitní čidlo. Zadní díl nese na bocích boční dálkoměry/proximitní čidla fixnuté zase sekundovým lepidlem. Nějak se nestihla naše konstrukce natřít takže je pěkně zrezlá ;). Další slova nahradí obrázky - viz dole.

Závěrem jen uvedu, že naše vystoupení na soutěži nebylo nijak zázračné, po příjezdu do Bratislavy o den dříve samozřejmně robot nefungoval (teda fungoval ale hodně divně :) avšak bezesné noc vše částečné spravila nakonec až ve třetí jízdě Bob projel celo trasu za cca 42s. Ale takhle drsně jsem ještě neprogramoval!

A pokud to nevíte tak Bob má starou! Jmenuje se Madge a je skvělá, z nuly na šedesát za míň jak deset minut.

Fotky:

Videa:

Na videech je Bob řízen fw "nobrick.c" na mém domácim tankodromu.

Více též na http://www.robotika.sk

Disclaimer: Tato stránka není ekvivalentní k dokumentaci odevzdané jako semestrální projekt!

Aktualizováno 26.06.2006