Régóta tervezem ezt a fejlesztést, mert mérföldkőnek gondolom a távirányítós autóból robottá válás útján. Eddig nem volt lehetséges, hogy az autó saját magát navigálni tudja, mivel semmilyen módon nem érzékelte a környezetét, a fizikai világgal való interakcióit. Az autót játékból vezető gyerekek többsége számára talán fel sem tűnt, hogy a manőverezés és navigálás során ők maguk adják a visszacsatolást egy nagyon érzékeny szabályozási körben.
Odometria
Ahhoz, hogy egy robot navigálni tudjon, tudnia kell, hogy adott parancs hatására mennyit és milyen irányban mozdul el a fizikai térben. A navigálásnak ezt a legegyszerűbb módját talán már mindenki használta gyerekkorában: megjegyezzük, hogy hányat léptünk előre, majd merre fordultunk, még hányat léptünk és így tovább. A végén pedig szerencsés esetben megtaláltuk a kincset. Ha visszafelé is lelépjük a sémát, akkor (ideális esetben) visszatérünk a kiindulópontra.
Kerekes robotok esetén a "lépéseket" kerékfordulatokban, vagy azok törtrészeiben számoljuk. Minél kisebb elfordulásokat tudunk érzékelni, annál pontosabb lesz a navigációnk. A kerekek ilyetén monitorozását nevezi a szakzsargon odometriának. Az itt elérhető Szatmári István féle odometria jegyzet rengeteg további információt ad a témában, taglalva a különböző hajtási módszereket.
Enkóder
Több módja is van annak, hogy egy kerék elmozdulását mérjük: használhatunk optikai, mágneses vagy mechanikus érzékelést, mérhetjük pusztán az elmozdulást, de akár a kerék pontos állását is - az ezt megvalósító érzékelőket gyűjtőnéven enkódereknek nevezzük.
Hall szenzor
A robotkocsihoz a mágneses elvet választottam: a kerékre ragasztott kis mágnesek jelenlétét figyeli egy A3144-es Hall szenzor. A szenzor működése a Hall-effektuson alapszik. Azon túl, hogy két állapotú a kimenete, valamint hiszterézis szerint kapcsol, azt érdemes tudni a szenzorról, hogy érzékeny a mágneses erővonalak irányára, csak az északi pólusával közelítő mágnest érzékeli.
Mágnesek
A kocsi bal hátsó kerekére pici, 3*3 mm-es neodímium mágneskockákat ragasztottam. Először azonban meg kellett keresni a mágnesek északi pólusát. Ehhez egy olyan öntapadós mágneskorongot használtam, aminek ismert a mágnesezettsége (színekkel jelölve).
A mágneskockák a déli pólusukkal feküdtek fel a mágneskorongra, így könnyen megjelölhettem az északi pólusaikat alkoholos filccel. Később, a ragasztás során a jelölés alapján így egyértelmű volt, hogy hogyan kell elhelyezni a mágneseket.
A kerék hat küllője mentén helyeztem el a mágneseket, egymástól egyforma távolságokra, közel a kerék pereméhez, hogy minél nagyobb legyen a távolság a mágnesek között, így biztosan lesz elég hely, ahol a Hall-szenzor nem érzékel mágneses teret, illetve akár további mágnesek beragasztására is lehetőség nyílik.
Ez a hat érzékelhető pozíció kerékfordulatonként egyébként rendkívül kevés. Az autó hajtásának kialakítása azonban nem teszi lehetővé, hogy azt megbontsam, pedig a motor tengelyén – még az áttételek előtt – lenne ideális helyen az enkóder. A finom sebességszabályozáshoz ugyanis egy kerékfordulat alatt rengetegszer kéne mintavételezni az aktuális sebességet, majd azt egy PID-szabályozón keresztül visszacsatolni a hajtás vezérlésébe. Megtett távolság mérésére azonban már ezzel a messze nem optimális kialakítással is van lehetőség, sőt, talán még valamilyen kezdetleges szabályozáshoz is elegendő lesz.
Érzékelők
Hall-szenzorból kettőt építettem be, 30°-os eltolással (mivel a mágnesek 60°-onként helyezkednek el a keréken), így fordulatonként 6 helyett 12-szer fognak jelet érzékelni az érzékelők, ami jobb felbontást tesz lehetővé. A két szenzor alkalmazása ráadásul lehetővé teszi a forgás irányának meghatározását is (lásd a fentebb hivatkozott oldalon).
Az érzékelők precíz elhelyezése miatt egy 3D nyomtatható tartót terveztem. A két szenzort n*30° távolságra kell elhelyezni, ahol n egy páratlan szám, tehát nem muszáj konkrétan 30°-ra tenni őket egymástól. Ezt hasznos tudni, ha a szenzorok fizikailag külső akadályokba ütköznének adott kialakítás mellett. De adott esetben az is motiváció lehet, hogy egyszerűbb 90°-ra elhelyezni a szenzorokat egymástól házilag barkácsolt tartó esetén.
Szenzortartó
A 3D nyomtatott tartónak azonban megvan az az előnye, hogy könnyedén és precízen bárhány fokra helyezhetjük egymástól a szenzorokat (aminek lesz is jelentősége, amikor a pontosabb forgásirány-meghatározáshoz duplázom a mágnesek számát és 15°-ra kell helyezni egymástól a szenzorokat, de erről majd egy újabb poszt fog szólni).
Elővettem tehát a TinkerCAD-et, mint már korábban, és megterveztem a tartót:
Majd jöhett a nyomtatás és az összeszerelés. A hall szenzorokat a meghajított lábaik rögzítik a helyükön, ragasztásra nem volt szükség. A kábelkorbácson közös tápellátás érkezik a szenzorokhoz, a sárga és a fehér vezetéken pedig a szenzorok jele fut vissza az elektronikához.
Összeszerelés
Az autó hátsó hídja rugózni tud a többi részhez képest, így arra kéne felszerelni a szenzortartót, hogy a szenzorok mindig jó pozícióban legyenek. Erre a részre azonban nem nagyon lehet felszerelni semmit.
Szerencsére ennek a rugózó résznek a felfüggesztési pontja - mely egyben forgáspontja is - alkalmas a szenzortartó felszerelésére. A tartó tehát az alvázra rögzül, de itt a hátsó tengely forgáspontjában minimális a mágnesek kitérése a szenzorok elől rugózás esetén, ráadásul ez a kis kitérés is koncentrikus irányban történik. A mágneseknek pedig van akkora hatósugaruk, hogy a rugózás teljes tartományában ugyanúgy érzékelhető a jelenlétük. Tehát ide szerelve a szenzort mindig pontos jelet fogunk érzékelni, nem lesznek fals pozitív tévedések, valamint kihagyások sem.
A fenti képeken látszik a felszerelés helye (kapóra jött a futómű bekötési pontját lezáró elemnek a csavaros rögzítése), lent pedig a szenzorok elhelyezkedése a kerék mellett.
Így már csak az elektromos bekötés és a program megírása van hátra.
Az elektromos bekötés egyszerűen annyit takar, hogy 5V-os tápellátást adtam a szenzoroknak, a kimeneteiket a mikrovezérlő kártya egy-egy GPIO portjára kötöttem:
Hibakeresési és szemléltetési célokra két LED-et is direktben vezérlek a szenzorok kimeneteivel. Ezek segítségével igazán látványos az enkóder működése:
Szoftver
Végül a szoftvermodul megírása maradt hátra. Egy-egy interrupt-kezelő rutint kapcsoltam azon GPIO lábakhoz, melyekre kötöttem a szenzorok kimeneteit. Innentől már csak a fel és lefutó élek sorrendjét kell megfelelően értelmezni. Mivel a mágnesek jelen konfigurációban ritkán vannak, így nincs átfedés a két jel magas állapota között, egy egyszerűsített algoritmust alkottam, aminek megvan az a hátránya, hogy két feltétellel működik jól:
- Az autónak bekapcsolás után először előrefelé kell elindulnia
- Akkor detektálja pontosan az irányváltást, ha mindkét szenzor alacsony jelet ad, amikor ez megtörténik. Ha pont az egyik mágnes jelenlétében történik a váltás, az borítja az odometria állapotát.
void Odometry::hallAISR(void) {
if(lastHall == A)
toggleDir();
if(dir == FW)
odoPos++;
else
odoPos--;
odoCount++;
calcSpeed(A);
lastHall = A;
}
void Odometry::hallBISR(void) {
if(lastHall == B)
toggleDir();
if(dir == FW)
odoPos++;
else
odoPos--;
odoCount++;
calcSpeed(B);
lastHall = B;
}
void Odometry::calcSpeed(HallID actHall) {
// Prevent division by zero -> side effect: measuring limit under 5000 RPM (@12 PPR)
if(timer.read_ms() == previousTime)
return;
// Handle direction change
if(actHall == lastHall) {
currentSpeed = 0;
return;
}
// Current speed calculation
currentSpeed = (WHEELCIRCUMFERENCE / PPR) * 100 / (timer.read_ms() - previousTime);
// Highest speed calculation
if((previousSpeed + currentSpeed)/2 > highestSpeed)
highestSpeed = (previousSpeed + currentSpeed)/2;
// Average speed calculation
speedSum += currentSpeed; // TODO: this will overflow once, far in the future
avgSpeed = speedSum / odoCount;
previousTime = timer.read_ms();
previousSpeed = currentSpeed;
}
void Odometry::toggleDir() {
if(dir == FW)
dir = BW;
else
dir = FW;
}
Emiatt már készült egy 15°-os változata a szenzortartónak, valamint 6 új mágnest is beszereztem, amivel jó eséllyel ki lehet majd küszöbölni ezeket a problémákat.
Addig is zárásként megosztok egy videót a szenzorok nagy sebességű teszteléséről is:
