Ebben a részben a már jól működő odometriára építve valósítok meg egy nagyon egyszerű aktív fékezést végrehajtó algoritmust. Ahogy az alcím is sugallja, arra törekedtem, hogy minél egyszerűbben belehekkeljek a motorvezérlő ciklusba.
Szabályozásnak nem nevezném, bár mégiscsak egy visszacsatolt rendszerről van szó, ráadásul – bár durván, de – működik. Komolyabb (PID) szabályozást majd egy következő részben fogok bemutatni. Érdemes lesz az eredményeket is összehasonlítani.
Eddigi motorvezérlés
Az alábbi kódrészlet mutatja, hogy miből indultam ki:
// 100 Hz control loop
while (true) {
// main drive control
if(self->f_forward) {
if(forwardPower < 0.1f)
forwardPower = 0.7f; // initial torque
else {
if(forwardPower > self->driveSpeed)
forwardPower -= 0.1f; // decreasing torque to a constant 30%
}
} else
forwardPower = 0.0f;
if(self->f_backward) {
if(backwardPower < 0.1f)
backwardPower = 0.7f; // initial torque
else {
if(backwardPower > self->driveSpeed)
backwardPower -= 0.1f; // decreasing torque to a constant 30%
}
} else
backwardPower = 0.0f;
self->po_forward->write(forwardPower);
self->po_backward->write(backwardPower);
Thread::wait(10); // 100 Hz
}
A változók neveiben felfedezhető inkonzisztencia ne zavarjon össze senkit: nem teljesítményt hasonlítok sebességgel, pusztán a folyamatos fejlesztés (és a lustaság) velejárója, hogy vannak változók, amik már nevükben a jövőbeli szerepüket tükrözik.
Röviden: az látható, hogy egy egyszerű teljesítménygradienst megvalósítva indítom az autót adott irányba, majd egy konstans teljesítménnyel hajtom tovább. A számítások elvégzése után állítom be a PWM jeleket a po_... (PwmOut) objektumok write metódusának hívásával.
Egyszerű fékezés
A számítások és a PWM jelek beállítása közötti helyre betenni a fékezésért felelős kódrészletet kézenfekvőnek tűnt, így az alábbi sorokkal egészült ki a fenti ciklus:
if(self->f_brake) {
float speed = self->odometry->readValue(Odometry::CurrentSpeed);
float brakingPower = (fabs(speed) > 9)?0.3f:(fabs(speed) / 1000 );
if(speed > 7) {
forwardPower = 0.0f;
backwardPower = brakingPower;
} else if(speed < -7) {
forwardPower = brakingPower;
backwardPower = 0.0f;
} else {
forwardPower = 0.0f;
backwardPower = 0.0f;
}
}
Ha fékezünk (az ezt jelző f_brake flag igaz), akkor elkérem az odometriát kezelő objektumtól az aktuális sebességértéket, majd ez alapján számítom ki a fékezés erejét (vagyis, hogy mekkora teljesítménnyel kezdem el ellenkező irányú forgásra késztetni a kerekeket).
Ha a sebesség nagyobb 9 cm/s-nél, akkor 30 %-os teljesítményt állítok be, ha kisebb, akkor pedig a sebességgel arányos és jóval kisebb értéket.
Ezután meghatároztam egy alsó sebességhatárt, aminél kisebb sebesség esetén egyáltalán nem fékezek. Ez a határ végül meglepően magas, 7 cm/s lett, de empirikus úton keresve a megfelelő értéket erre jutottam. Ha ugyanis akkor is próbálnék ellenkező irányba teljesítményt adni, amikor az autó már szinte áll, akkor egy vad vibrálás, ide-oda "hintázás" lesz az eredmény.
Teszt
Az elméletről többet nem is mondanék, annyira egyszerű, lássuk inkább az eredményt. Előtte azonban egy gyors eszmefuttatás arról, hogyan is teszteljünk jól:
Minden teszt esetén kulcsfontosságú a reprodukálhatóság. Értem ez alatt a környezet változatlanságát, valamint a az egyes tesztesetek végrehajtásának módját is. Manuális tesztelés esetén nincs értelme pl. féktávról, annak a mértékéről beszélni, mert nem tudjuk garantálni, hogy minden esetben ugyanannyi ideig gyorsítjuk az autót, ahogyan azt sem, hogy mindig ugyanakkora reakcióidővel "lépünk rá" a fékre.
Írtam hát egy rövid forgatókönyvet az autó "Demo" moduljában, aminek a segítségével automatikusan tesztelhetjük a gyorsítást, majd a megállást:
while(self->f_run) {
self->lights->headLightOn();
self->drive->steerStraight();
Thread::wait(1000);
self->drive->forward();
Thread::wait(1500);
self->drive->stop();
Thread::wait(2000);
self->drive->backward();
Thread::wait(1500);
self->drive->stop();
Thread::wait(2000);
self->lights->headLightOff();
Thread::wait(3000);
self->lights->headLightOn();
self->drive->steerStraight();
Thread::wait(1000);
self->drive->forward();
Thread::wait(1500);
self->drive->brake();
Thread::wait(2000);
self->drive->releaseBrake();
self->drive->backward();
Thread::wait(1500);
self->drive->brake();
Thread::wait(2000);
self->drive->releaseBrake();
self->lights->headLightOff();
Thread::wait(1000);
}
A forgatókönyv a következő: előrefelé gyorsítunk, majd kigurulva megállunk, majd hátrafelé gyorsítunk és ugyanígy állunk meg; ezután ugyanezt eljátszuk, de kigurulás helyett aktív fékezéssel.
Ahogy a videókon is látható, a fenti, fékerőt számító algoritmus még nem tökéletes. Az algoritmust küszöbölt arányos (P) szabályozásnak lehetne nevezni, hiszen a fékerő egy tartományban a sebességgel arányos, de egy kezdeti, küszöb feletti tartomány is, nagyobb fékerővel. Az ilyen típusú szabályozásoknak tipikus jelensége a túllövés és az ebből következő remegés.
A helyzet azonban nem olyan rossz, mert hátramenetben bizonyos esetekben egész jól blokkol a kerék, köszönhetően annak, hogy ilyenkor a hátsó tengelyre kerül nagyobb terhelés.
Úgy gondolkodtam, hogy ha növelnéma szabályozási kör időállandóját (azaz 100 Hz helyett mondjuk 20 Hz-es gyakorisággal futna le a fékerőszámítás és korrekció), akkor lenne ideje a rendszernek arra, hogy a ciklusonként egyre csökkenő sebességgel arányos, a valóságban szükségeshez közelebb álló fékerőt fejthessen ki, valamint a megállást, azaz a 0 sebességű esetet is legyen ideje felismerni, mielőtt kompenzálni próbálna. Ezt segíti a fentebb már említett 7 cm/s alsó küszöbérték is.
Betettem tehát egy számlálót a ciklusba, valamint egy feltételt, hogy csak minden 5. alkalommal frissüljön a fékerő:
unsigned int timer = 0;
while (true) {
...
if(self->f_brake && (timer % 5 == 0)) {
...
}
...
timer++;
}
Mivel a timer változó unsigned int típusú, nem kell a túlcsordulásától tartani. A modulo 5-ös osztás mindig jó eredményt fog adni. Lássuk az eredményt:
A videókon látható, hogy a remegés megszűnt, nagyon jól sikerült kiküszöbölni a szabályozás lassításával. Ez azonban nem jelenti azt, hogy ez egy tökéletes szabályozási kör lenne. A P betű a PID rövidítésből a proportional-t, vagyis arányost jelöli. Ez a legritkább esetekben a leghatékonyabb megoldás. Az Integrált és Derivált tagok implementálásával el lehet érni, hogy gyorsabb legyen a beavatkozás, mégse legyen túllövés. Esetünktben ez azt jelenti, hogy csökkenteni lehet a féktávot anélkül, hogy a káros remegést tapasztalnánk. Ezzel a témakörrel egy következő posztban fogok foglalkozni.
FRISSÍTÉS:
Mondhatnám úgy is: spoiler a következő rész tartalmából. A fenti kódrészletben hagytam egy hibát, ami csak némi próbálgatás után jelentkezett. Ez újfent rávilágít a tesztelés fontosságára. Házi feladatként lehet ezen gondolkodni, mert csak a következő részben fogom elárulni, mi volt a hiba.
