Az előző részben bemutatottam az aktív fékezést. Időközben egy ultrahangos távolságérzékelő/szonár is felkerült az autó elejére, így most arról lesz szó, hogy hogyan is gyúrhatjuk össze ezt a két funkcionalitást egy vészfékaszisztensé. A motiváció már adott egy ideje, hogy ezt a funciót kifejlesszem, mert a rendezvényeken a közönség akaratlanul is gyakran vezette neki valaminek a kocsit.
A szonár
Az autó elejére egy standard(-nak nevezhető) HC-SR04 ultrahangos távolságérzékelőt szereltem fel.
Ez úgy működik, hogy a "trigger" lábára adunk egy rövid impulzust, majd várjuk, hogy az "echo" lábon megjelenjen egy válasz-impulzus. Közben számoljuk az eltelt időt. A visszaérkező impulzust akkor küldi ki a szonár, ha a "trigger" jelünkre kibocsájtott ultrahangos hullám visszaverődik valahonnan és ez a visszhang visszaérkezik a szenzorba.
Tehát az eltelt idő arányos a szonár előtti szabad távolsággal. Innen már csak egy kis matematika és programozás kérdése az egész (szinte ugyanezt a kódot használtam fel).
A szonár rögzítése
A rögzítéshez a már sokszor taglalt és bevált 3D nyomtatáshoz folyamodtam: TinkerCAD-ben megterveztem a tartót, amit egyszerűen bepattinthatok az autó elejére, majd gyorsan ki is nyomtattam.
 |
 |
Vészfékezzünk
De előtte egy kis kitérő az előző részben felvezetett hibához: A szabályozást úgy lassítottam a káros remegések elkerülése érdekében, hogy léptettem egy számlálót a ciklusban, és amikor 5-tel osztható az értéke, csak akkor frissítettem a fékerőt. Ezzel nincs is gond addig, amíg az autó nem kap új forward vagy backward parancsot.
Ekkor ugyanis a motorvezérlő ciklus 5 lefutásából négyszer a motorerő értékét felülírja a haladáshoz szükséges motorerőt számító rész. Így az autó próbált "kitörni a ketrecből", mocorgott, néha, ha megfelelően rezonált a két algoritmus, akkor neki is lódult.
Ezt elkerülendő felhasználtam a fékezést jelző logikai változót, ami a fékezés teljes időtartama alatt jelzi, hogy jelenleg fékező üzemmódban vagyunk, és egy ezt felhasználó elágazás megakadályozza a fékerő felülírását. Na de a kódot egyszerűbb megérteni, mint magyarázni:
// 100 Hz control loop
while (true) {
// brake persistence (needed to prevent altering braking power, as it is updated only in every 5th cycle)
if(self->f_brake) {
self->f_forward = false;
self->f_backward = false;
}
// main drive control
if(self->f_forward) {
...
}
...
}
A teljes kód egyébként elérhető GitHubon: https://github.com/dralisz82/robotkocsi_OS
Most már tényleg vészfékezzünk
A szonár kezelését végző osztályt integráltam az autó szenzorkezelő alrendszerébe (részleteg a fenti GitHub linken láthatók), majd a motorvezérlő kódba integráltam magát a vészfékezést.
Felvettem új változókat és a konstruktort kiegészítettem az alábbi módon:
if(sensors != NULL) {
this->odometry = sensors->getSensor("odo");
this->frontSonar = sensors->getSensor("sonFront");
} else {
this->odometry = NULL;
this->frontSonar = NULL;
}
// black box data
unacknowledgedEmergencyBraking = false;
emergencyBrakingDistance = 0;
emergencyBrakingSpeed = 0;
A motorvezérlő ciklus elejére pedig az alábbi kódrészlet került:
// 100 Hz control loop
while (true) {
// emergency braking
if((self->frontSonar->readValue() < 70 && self->odometry->readValue(Odometry::CurrentSpeed) > 250) ||
(self->frontSonar->readValue() < 40 && self->odometry->readValue(Odometry::CurrentSpeed) > 200) ||
(self->frontSonar->readValue() < 30 && self->odometry->readValue(Odometry::CurrentSpeed) > 0)) {
self->f_forward = false;
self->f_brake = true;
self->lights->brakeLightOn();
self->lights->hazardLightsOn();
if(!self->unacknowledgedEmergencyBraking) {
self->unacknowledgedEmergencyBraking = true;
self->emergencyBrakingDistance = self->frontSonar->readValue();
self->emergencyBrakingSpeed = self->odometry->readValue(Odometry::CurrentSpeed);
}
}
...
}
A kódrészlet három sebességtartományhoz három különböző trigger távolságot jelöl meg. Erre azért volt szükség, mert túl nagy határ lehetetlenné teszi a kissebességű manőverezést szűk helyen, túl kicsi határ pedig nem lenne elegendő (kisebb lenne, mint a féktáv) nagyobb sebességeknél.
A feltétel teljesülése esetén a "self->f_brake = true;" parancs lefutása után a korábban ismertetett módon zajlik a fékezés.
Fékezés után egy releaseBrake() hívással lehet "kiengedni a féket" (például a fék gombot megpöccinteni a távirányítón), ez után lehet újra mozgásra bírni az autót.
Maga a vészfékezés ilyen egyszerű. A többi sor pusztán diagnosztikai célokat szolgál. A fékezési adatokhoz getter függvényekkel lehet hozzáférni, azokat a parancsértelmezőből, egy újabb parancs érkezése esetén íratom ki a Bluetoothos terminálra.
Tesztelés
Lássuk mindezt a gyakorlatban (először a videón, majd akár élőben is, az autó ugyanis, ahogyan az elmúlt években is, idén is megtekinthető lesz a Kutatók Éjszakáján, amihez a lenti videó egyúttal kedvcsinálóként is készült.
