Reaali Robootika.COM

NXT robotimaailm ja programmeerimine C-keeles

Juhend: Mootorite peamised käsud NXC-s

Alljärgnevalt peamised NXC käsud, mille abil robot liikuma või seisma panna.

OnFwd("ports", "pwr"); OnRev(..)

OnFwd(OUT_A, 75);

Mootor A liigub edasi kiirusega 75


OnFwdSync("ports", "pwr", “turn”); OnRevSync(..)

OnFwdSync(OUT_AB, 75, -100);

Mootorid A ja B liiguvad edasi ning neid hoitakse sünkroonis. Turn ehk keeramine -100 määrab ära kummale poole keeratakse. 0 – sõidab otse, 50 – üks ratas seisab, 100 – rattad liiguvad vastassuundades.


OnFwdReg("ports", "pwr", OUT_REGMODE_SPEED); OnRevReg(..)

OnFwdReg(OUT_AB, 100, OUT_REGMODE_SPEED);

Mootorid A ja B sõidavad otse ning rakendavad lisajõudu kui ette satub takistus.


Off(OUT_AB);

Off(OUT_AB);

Lülitab mootorid A ja B välja


RotateMotor("ports", "pwr", “degrees”);

RotateMotor(OUT_A, 75, 360);

Pöörab mootorit A 360 kraadi kiirusega 75.


RotateMotorEx("ports", "pwr", “degrees”, “turn”, “sync”, “stop”);

RotateMotorEx(OUT_AB, 75, 360, 50, TRUE, TRUE);

Keerab mootoreid A ja B 360 kraadi kiirusega 75. Turn määrab ära kummale poole keeratakse (0 – sõidab otse, 50 – üks ratas seisab, 100 – rattad liiguvad vastassuundades), sync määrab et mootorid on sünkroonis ja stop määrab kas lõpus on vaja pidurdada või mitte.


Coast("ports”);

Coast(OUT_AB);

Laseb määratud mootorid vabajooksuga seismiseni.

Juhend: Andurite defineerimine ja lugemine

Roboti juures andurite kasutamine on seotud kahe tegevusega: a) defineerimine b) lugemine.
Selleks, et programmis andureid kasutada (lugeda), tuleb robotile kõigepealt selgeks õpetada millised andurid ja kus pordis asuvad.

Alljärgnevas tabelis on andurite initsialiseerimisega seotud funktsioonid. Erinevaid andureid tuleb erineval moel initsialiseerida.

SetSensorLight("port");

SetSensorLight(S1);
Valgussensori initsialiseerimine pordis 1.

SetSensorTouch("port")

SetSensorTouch(S1);
Puuteanduri initsialiseerimine pordis 1.

SetSensorSound("port");

SetSensorSound(S1);
Helianduri initsialiseerimine pordis 1.

SetSensorHTEOPD("port");

SetSensorHTEOPD(S1);
Hitechnic EOPD sensori initsialiseerimine.

SetSensorHTGyro("port");

SetSensorHTGyro(S1);
Hitechnic Gyro sensori initsialiseerimine.

SetSensorLowspeed("port");

SetSensorLowspeed(S1);
Ultrasonic ja Hitechnic IRSeeker, Color, Compass sensori initsialiseerimine pordis 1.

Anduritelt andmete lugemiseks kasutatavad funktsioonid on alljärgnevas tabelis. Jällegi tuleb tähele panna, et osade andurite jaoks saab kasutada sama funktsiooni lugemiseks, kuid osade jaoks on spetsiifilised funktsioonid.

Sensor("port");

Sensor(S1);
Andmete lugemine Touch, Light, Sound sensorist.

SensorUS("port");

SensorUS(S1);
Andmete lugemine Ultrasonic sensorist.

SensorHTGyro("port", “Offset”);

SensorHTGyro(S1, 2);
Andmete lugemine Gyro sensorist. Ette on antud offset ehk nulli hälve.

SensorHTEOPD("port");

SensorHTEOPD(S1);
Andmete lugemine EOPD sensorist.

SensorHTCompass("port");

SensorHTCompass(S1);
Andmete lugemine Compass sensorist.

SensorHTIRSeekerDir("port");

SensorHTIRSeekerDir(S1);
IR Seeker sensorist suuna lugemine.

SensorHTColorNum("port");

SensorHTColorNum(S1);
Värviandurist vastava värvi numbri lugemine.

Juhend: Lego rattad ja matemaatika

Alljärgnevalt juhis, kuidas arvutada erinevate Lego rataste korral välja otseliikumiseks või pööramiseks vajalik rattanurk kraadides.
 
Need kolm on peamised rattad mida kasutatakse Lego Mindstorm robotite ehitamisel.
Ratas
Diameeter
Ümbermõõt
Väike ratas
clip_image002
43,2 mm
135,7 mm
Keskmine ratas
clip_image004
56 mm
175,9 mm
Suur ratas
clip_image006
82 mm
257,6 mm
Alljärgnevalt on ära kirjeldatud moodused, kuidas arvutada välja mootori pöörded kraadides kui robot peab a) kindla maa edasi liikuma b) pöörama teatud nurga võrra nii, et üks ratas seisab c) pöörama teatud nurga võrra kohapeal.
Kraadides on tarvis tulemust seetõttu, et NXC-s ehk C-keeles programmeerides saab kasutada funktsiooni RotateMotorEx, mille abil saab roboti väga täpselt liikuma panna.
Huvitav nüanss on see, et peale keeramist funktsiooniga RotateMotorEx on vaja panna kasvõi 10ms pikkune wait, kuna muidu lähevad mootorid peast segi.
Kui näiteks alljärgnevas näites Wait lõpust ära võtta on tulemuseks mõõdetamatu segadus. Õigupoolest üritab robot oma mootorid sünkroonis panna liikuma esialgses suunas. Seega peale kolmandat repeati robot ainult keerleb kohapeal.

repeat(4){
RotateMotorEx(OUT_AB, 50, 500, 0, TRUE, TRUE);
RotateMotorEx(OUT_AB, 50, 365, 0, TRUE, TRUE);
Wait(MS_10);
}

 

Otseliikumine

Roboti otseliikumise korral teepikkuse väljendamine mootori pöörded kraadides.
image
Distants - vahemaa, mille robot peab läbima. Näiteks 200 mm (20 cm)
Ümbermõõt – roboti ratta ümbermõõt
x kraadi – mootori pöörded kraadides
 

Pööramine ühe rattaga

clip_image010Roboti keeramisel ühe rattaga (st. teine ratas on paigal) liikumise väljendamine mootori pööretes mis on antud kraadides.
image
Roboti pööramise nurk – see on kraadides väljendatud nurk, mille robot peab pöörama. Näiteks 90*
Rataste vahe – rataste keskkohast mõõdetud nende omavaheline kaugus. Näiteks 135 mm
x kraadi – mootori pöörded kraadides
 

Pööramine kohapeal

clip_image014Roboti keeramisel mõlema rattaga (st. rattad liiguvad vastassuundades) liikumise väljendamine mootori pööretes mis on antud kraadides.
image
Roboti pööramise nurk – see on kraadides väljendatud nurk, mille robot peab pöörama. Näiteks 90*
Rataste vahe – rataste keskkohast mõõdetud omavaheline kaugus. Näiteks 135 mm
x kraadi – mootori pöörded kraadides

Juhend: Loogika kasutamine

Loogikalülitused omavad NXT-maailmas alati ja ainult kahte väärtust: true või false (1 või 0).
1 ja 0 abil saab teha hulgaliselt tehteid ning lahendada erinevaid ülesandeid. Programmis tähistatakse seda tüüpi muutujat sõnaga bool (tuletatud sõnast Boolean ehk kahendmuutuja).
C-keeles muutuja defineerimine ja algväärtustamine:
bool ShootBall = FALSE;
Enimkasutatavad kahendmuutujatega tehtavad tehted on AND (JA), OR (VÕI), XOR (välistav VÕI), NOT (EI).
Kahendmuutujate tabelid
Loogikatehete vasted NXC-s:
AND tehte korral kasuta kahte ja märki &&
Alltoodud avaldistes muutub if lause tõeseks ainult siis kui mõlemad muutujad on tõesed. Kõik toodud kolm kombinatsiooni on õiged, kuid soovituslik on kasutada esimest kui kõige lühemat varianti.
if(ShootBall && ShootButton)
if(ShootBall == TRUE && ShootButton == TRUE)
if(ShootBall == 1 && ShootButton == 1)

OR tehte korral kasuta kahte püstist kriipsu ||
Alltoodud avaldistes muutub if lause tõeseks juba siis kui vaid üks muutuja on tõene ja jääb ka tõeseks kui mõlemad on 1-d. Kõik toodud kolm kombinatsiooni on õiged, kuid soovituslik on kasutada esimest kui kõige lühemat varianti.
if(ShootBall || ShootButton)
if(ShootBall == TRUE || ShootButton == TRUE)
if(ShootBall == 1 || ShootButton == 1)

NOT tehte korral kasuta hüüumärki !
Alltoodud avaldise sees muudab hüüumärk muutuja väärtuse vastupidiseks. If lause muutub tõeseks sel juhul kui muutuja on FALSE ehk 0. Kuigi kõik alljärgnevad avaldised annavad sama tulemuse, on soovituslik kasutada esimest kui lühimat.
if(!ShootBall)
if(ShootBall == FALSE)
if(ShootBall == 0)

Juhend: PID controlleriga NXT joonejälgija robot

Robootikas on joonejälgija roboti ehitamine üks esmaseid asju mida tavaliselt õpitakse ja tehakse. Ja alati on eesmärgiks luua robot, mis suudab joonel püsides kõige kiiremini liikuda.
NXT robotile saab joonejälgimise anduritena kasutada algselt kaasasolevat valgusandurit, NXT 2.0 värviandurit, Hitechnic värviandurit, Hitechnic EOPD andurit (ehk infrapuna kaugusandur). Lisaks on olemas spetsiaalne Mindsensori joonejälgija andur, mis koosneb 8-st infrapuna diood-andurist, tagades erakordselt nobeda liikumise.
Joonejälgija robot töötab alati põhimõttel, et jälgitakse joone serva ehk siis musta-valge vahekohta. See tähendab, et ühe sensoriga joonejälgija on ehitatud alati järgima kas joone parem- või vasakpoolset serva. Kui andur satub valgele, keerab robot tagasi musta peale ja vastupidi – tulemuseks on sik-sak sõitmine. Selle väite juures on paar erisust:
  1. NXT 2.0 värviandurit kasutades saame jälgida joone mõlemat serva, kuna 3 andur-dioodi paiknevad kolmnurkselt ja äärmistel dioodidelt saadud erineva värviinfo alusel saame otsustada kummal pool joont robot asub. Kuid tehniliselt toimub ikkagi joone serva lugemine.
  2. Kasutades 2-te või enamat andurit saame luua kiiremini liikuva roboti, kuna siis saame jälgida joone mõlemat serva, lastes robotile suurema veahälbe ja seeläbi kiiremini liikuva roboti ehitada.
Lego Light sensorHitechnic Color sensorHitechnic EOPD sensorLego Color sensor
Mindsensors Line Follower
Kiirema ja töökindlama joonejälgija loomiseks on mitu võimalust, muuta programm keerukamaks ja või suurendades andurite arvu, millega kaasneb samuti paratamatult programmi keerukus.
Siinkohal on ülevaade selle kohta, kuidas teha kiire joonejälgija ühe anduriga kasutades PID controlleri põhimõtet.
 
PID teooria
PID controller koosneb tegelikkuses kolmest controllerist: proportional-integral-derivative (proportsionaalne – integreeruv - tuletav). Igal kontrolleril on oma funktsioon ning igaühe häälestamiseks vajame konstanti või teisisõnu kontrolleri võimendust: Kproportional, Kintegral, Kderivative.
 
Proportional controller
See iseloomustab roboti keeramise intensiivsust sõltuvalt sellest kui kaugele robot parasjagu on hälbinud oma joonest.
Proportional controlleri juures kõige olulisem omadus on lineaarsus, st. kui robot on natuke joonelt kõrvale hälbinud, keeratakse robotit aeglaselt ja sarnaselt kui robot on palju kõrvale hälbinud keeratakse robotit proportsionaalselt kiiremini tagasi joonele. Antud kontrollerit häälestatakse konstandiga Kp.
Matemaatiliselt väljendades: Turn = Kp(error)
Turn - lineaarne suurus mille võrra tuleb robotit joone suunas keerata, see tuleb teisendada roboti pööramiseks.
Teisenduseks kasutada moodust, kus VOTSE (see on kiirus, mis on mootorile ettenähtu otseliikumise korral) muudetakse vastavalt suuremaks/väiksemaks ja tulemuseks on uued kiirused mootoritele vastavalt VA ja VB.
VA = VOTSE + Turn (mootori A poweri suurendamine)
VB = VOTSE – Turn (mootori B poweri vähendamine)
Kp – konstant mis antakse ette vastavalt roboti ehitusele ja selle alusel määratakse Turn keeramise võimendus. Selle määramiseks on mõningad aluseks võetavad rusikareeglid, kuid katsetamine annab viimase lihvi. See sõltub ka konkreetse raja keerukusest, kas on väikeste raadiustega tihedad keeramised täpsussõiduks või suhteliselt lauge kurvid võidusõiduks.
Error – suurus, kui palju on robot hälbinud oma ideaalselt ehk otsesõitmise trajektoorilt. See number saabub andurist. Mida laiema amplituudiga on võimalik errorit saada, seda efektiivsemalt sõitva roboti saame. Näiteks tõstes anduri kõrgemale, saame lugeda aluselt laiema ala millelt hälbimist lugeda, kuid samas muutub andur ebatäpseks, lastes aga anduri väga lähedale joonele saame täpse asukoha info kuid hälbe ulatus on väike ning seeläbi kannatab roboti kiirus.
 
Integral controller
Integral controlleri tundmaõppimine on vajalik, et tasandada P controlleri tulemusena tekkivat paratamatut sik-sak sõitu. I controller summeerib roboti hälbed ja arvutab selle põhjal uue hälbe, millega juhitakse P controllerit.
Programmaatiliselt lihtne: integral = integral + error
Error – ikka seesama roboti hälbe suurus mis saadakse andurist.
Integral – summeeritud error väärtused. Edaspidi tuleb mängu ka see, kui tihti võetakse andurilt summeeritavaid lugemeid – see on suurusjärgus iga 20ms järel. See on seotud juba NXT protsessori kiirusega.
Edasi võtame saadud integrali ning korrutame selle läbi uue konstandiga KI millega seejärel korrigeeritakse roboti keeramist. Vaatame matemaatiliselt:
Turn = KP*(error) + KI*(integral)*dT
Ülaltoodud valemi alusel on meil tegemist PI kontrolleriga, kusjuures proportional controller on KP*(error) ja integral controller KI*(integral)*dT. Parameetrist dT on kirjas allpool.
Nüüd aga valemite juurest sisulise arusaamise juurde. I controller tasandab P controllerist tulenevat sik-sakitamist tänu oma mäluefektile.
Nimelt kui error annab meile hetkelise hälbe joone serva suhtes siis integral muudkui kasvab ja kasvab (kui robot on jätkuvalt ühele poole viltu). Ja arvestama peab, et integral kasvab tohutu kiirusega, kuna error ehk anduri lugem saadakse iga ca 20ms järel. Edasi sõltub integrali nö. töösserakendumine temaga seotud konstandist KI.
Ja nüüd natuke veel lihtsat matemaatikat, et aru saada, kuidas I controller aitab robotil sujuvamalt joonel püsida ja tasandab sik-sakitamist. Olgu näiteks hälbe suurused 1+2+3+3+4+1=14, seega integral on 14, kuigi anduri viimane lugem oli 1. Kuna selle tulemusena keerab robot teisele poole hakkab tekkiv miinuspool tasandama pööramise intensiivsust. Oletame et saadi andurist lugemid -1-2-3-3=-9, seega keeramise intensiivsus on ikka veel +poolel, mis omakorda tähendab, et robot ei keera veel tagasi seega roboti sik-sakitamisest on saanud sujuvam lainetamine.
Eelnevalt esitatud valemites on kirjeldatud aeg dT, mis kulub kahe andurist saadava info lugemisks.
I = KI*(integral)*dT
dT – see on ajaühik mis jääb kahe lugemi vahele ja tuleb konkreetse programmi juures välja arvutada. Selle arvutamiseks võib kasutada loop-i, mis loeb andurit 1000 korda ning mõõdab selleks kulunud aega. Hiljem siis jagame saadud aja 1000-ga ning vastuseks ongi dT. See võib varieeruda vahemikus 10ms – 30ms, sõltudes programmi keerukusest ning saadaolevast protsessori ressursist.
 
Derivative controller
Derivative controller ehk teisisõnu tuletis-kontroller annab robotile ennustamisvõime ning omab enim tähtsust joone keerukohtade juures.
Antud situatsioonis eeldame roboti liikumisel, et järgmine error on sama mis oli eelmine error ehk hälve ehk robot sõidab otse. Selle eelduse abil tuletatakse roboti tegelik hälve, ehk tegelik hälve on võrdne praegune hälve + uuest lugemist saadud praeguse ja uue hälbe vahe. Lihtsustatult,
derivative = (praegune error) – (eelmine error)
Seda muutust kahe hälbe vahel nimetataksegi derivative ehk tuletiseks. Kuna lugemite vahe võtmiseks kulub taas protsessori aega, peab arvesse võtma ka ajakomponendi. Kõik selle kokku pannes saame järgmise lihtsustatud matemaatilise valemi:
D = KD*(derivative)/dT
KD – konstant mille abil määratakse sarnaselt eelmistele kontrolleritele tuletise võimendus ehk selle mõjuulatus.
On ilmselge, et tuleviku ennustamine ehk teisisõnu joone suuna ette teadmine võimaldab robotil kiiremini liikuda. D controlleri lisamine mõjutab roboti liikumist järgmiselt: kui praegune hälve on suurem kui eelmine hälve – püüab robot oma positsiooni parandada keeramise läbi kui aga praegune hälve on väiksem kui eelmine siis D controller püüab robotit enam mitte sundida tagasi keerama vaid et see liiguks otse.
Teisisõnu, kui derivative on positiivne number siis D controller suurendab roboti tagasikeeramise kiirust, kui aga derivative on negatiivne number (ehk hälve juba väheneb) püüab D controller vähendada jõuliselt tagasikeeramise kiirust. Olulist rolli mängib siinkohal muidugi konstant, mille abil antakse D controllerile korrigeerimise võimendus.
 
Seega, lõplik matemaatiline PID controlleri valem.
Turn = KP*(error) + KI*(integral)*dT + KD*(derivative)/dT
Lõpliku lahendusena on tegemist suhteliselt lihtsa valemiga ja selle matemaatika programmaatiline realiseerimine robotil on suhteliselt lihtne.
Alljärgnevalt PID controlleri pseudokood, millest on kerge kirjutada nii NXT-G visuaalses kui ka NXC C-keeles töötav kood.
previous_error = 0
integral = 0
start:
  error = setpoint - actual_position
  integral = integral + (error*dt)
  derivative = (error - previous_error)/dt
  turn = (Kp*error) + (Ki*integral) + (Kd*derivative)
  previous_error = error
//  wait(dt)
  goto start
 
PID controlleri häälestamine
Mis aga on keeruline, õigete konstantide leidmine PID controlleritele. Peamine oskus robot kiirelt liikuma panna seisnebki õigete konstantide ja nendevahelise suhte määramises.
Mõned võtted siinkohal.
Meil on vaja neli komponenti omavahel õigesti tööle sättida, need on KP, KI, KD, VOTSE.
1. Elimineeri alguses KI, ja KD ehk pane need nulliks ning pane VOTSE kiiruseks 50 (kiirus sõltub siiski robotiehitusest ja kasutatavatest ratastest/ülekannetest)
2. Arvuta välja soovituslik võimendus KP. Olgu mootori otsesõitmise võimsus 50 ja maksimaalse hälbe (olgu hälve näiteks +/-5) korral tahame saavutada mootori võimsuseks 100 siis selle arvutamiseks vastav valem on järgmine: KP = (VAB - VOTSE)/error = (100 – 50)/5 = 10
Nüüd pane robot tööle ja vaata kuidas ta käitub joonel. Kui sik-sakitab liiga palju siis tuleb KP vähendada seni kuni sõidab silma järgi vaadates väikse jonksutamisega. Saadud väärtust nimetatakse KC ehk kriitiline võimendus (critical gain). Järgmiseks on vaja ära mõõta roboti võnke amplituudi kestus PC. See on aeg, mis kulub robotil ühe nö sik-saki tegemiseks. See aeg algab tüüpiliselt 0,5 sekundist kuid võib olla ka kuni 1-2 sekundit.
Teades ülalnimetatud väärtusi on võimalik järgmise tabeli abil arvutada välja ülejäänud sobivad konstandid. Antud tabelis toodud metoodikat nimetatakse Ziegler-Nichols meetodiks.
Ziegler-Nichols meetod PID controlleri konstantide määramiseks.
Controller
KP
KI
KD
P
0,5 * KC
0
0
PI
0,45 * KC
1,2 * KP / PC
0
PID
0,6 * KC
2 * KP / PC
KP*PC / 8
Peale konstantide häälestamist vaata kuidas robot joonel käitub ja proovi hakata suurendama VOTSE kiirust. Kui selle tulemusena muutub robot ebastabiilseks tasub kogu matemaatika uuesti läbi teha, ehk määra alguses P controlleri konstant Kp, roboti amplituudi kestus Pc ning siis arvuta ülejäänud.
 
Konstantide mõju roboti käitumisele
Kui oled PID controlleriga piisavalt kaua mänginud ja katsetanud siis oskad juba tunnetuslikult parameetreid häälestada. Siinkohal ongi esitatud mittematemaatiline ehk tunnetuslik juhend konstantide häälestamiseks.
Konstantide suurendamise tagajärjel tekkiv mõju roboti liikumisele
Parameeter
Joone hoidmise täpsus
Üle keeramise amplituud
Nulli jõudmise kiirus
Hälve tasakaaluasendis
KP
Väheneb
Suureneb
Väike mõju
Väheneb
KI
Väheneb
Suureneb
Suureneb
Kõrvaldab
KD
Väga väike mõju
Väheneb
Väheneb
Puudb
Roboti liikumise eripärade täpsustamine.
Joone hoidmise täpsus – iseloomustab seda, kui kiiresti üritab robot tagasi keerata joonelt kõrvalekaldumise korral.
Üle keeramise amplituud – on roboti liikumise amplituud ehk suurus kui kaugele robot võib joonelt ära „kaduda“.
Nulli jõudmise kiirus – iseloomustab aega mis kulub robotil selleks, et error väärtus nulli jõuaks. See muutub oluliseks järskude keeramiste korral.
Hälve tasakaaluasendis – on suurus, mis iseloomustab roboti hälvet otse liikumise korral, kuid mille korral veel ei võeta roboti juhtimiseks midagi ette.
Järgmistes artiklites annan ülevaate selle kohta kui palju on võimalik erinevate anduritega joonejälgijast välja pigistada kasutades tavalist valgusandurit, NXT 2.0 värviandurit, Hitechnic värviandurit, Hitechnic EOPD andurit ja Mindsensori spetsiaalset joonejälgijat.
Kasutatud materjalid: