See materjal on loodud Tiigrihüppe Sihtasutuse programmi ProgeTiiger raames.
Õpime, kuidas programmeerida bluetoothi vahendusel juhitavat puldiautot ning seda nii keeravate rataste ja diferentsiaalveermikuga auto kui ka tavalise roboti korral.
Paljude poiste unistuseks on olnud omada puldiautot, just sellist, mida saaks eemalt juhtida.
Robotimaailmas pole midagi lihtsamat, kui ehitada inimese juhitav robot ja korrektsuse huvides olgu välja toodud, et selliseid roboteid nimetatakse manipulaatoriteks.
Hetkel nimetame seda siiski puldiautoks või robotiks.
Roboti ehitus
Selle roboti ehitamine koosneb kahest iseseisvast komponendist, autost ja juhtpuldist. Auto võib teha kas lihtsa, kasutades baasrobotit, või keerulisema ja nutikama.
Lihtne auto tähendab seda, et auto kummagi vedava ratta küljes on oma mootor. Seega roboti otsesõitmiseks töötavad mootorid samal kiirusel, kuid keeramise korral peab üks mootor liikuma kiiremini ning teine aeglasemalt. Seda robotit nimetame siin peatükis baasrobot puldiautoks.
Nutikas auto eeldab vedava silla juures diferentsiaali kasutamist ja vedavate rataste juhtimist ühe mootoriga. Teine mootor on ühendatud esimeste ratastega, mis on võimelised keerama. See meenutab juba päris autot, millel on samuti neli ratast ning mis keeramiseks pöörab oma esimesi rattaid paremale-vasakule.
Nutikama auto ehitamiseks tasub kulutada aega ja proovide erinevaid variante. Juuresolev pilt on auto altvaates, millel on näha vasakul pool diferentsiaal ja paremal pool keeravad rattad.
Nutikama auto juures tuleb tagumised ehk vedavad rattad ühendada omavahel diferentsiaaliga. See võimaldab autol keerata sellisel viisil, et vedavad rattad ei hakka mööda maad libisedes teineteist takistama. Diferentsiaal võimaldab jagada ratastevahelist kiirust sellisel viisil, et üks ratas sõidab kiiremini kui teine. Sellise auto võib ise välja mõelda või leida internetist juba valmis tehtud näidise.
Juhtpult koosneb NXT-st ja kahest mootorist. Üks mootor tuleb panna toimima roolina ning teine mootor on kiiruse jaoks. Kõige sobilikum on pult, kui sellel on peal rool just samasuguses positsioonis nagu autol, ehk „näoga“ juhi poole. Teine mootor võiks olla vertikaalselt ning selle küljes peaks olema nn. käigukang. Kui kang on otse üleval, siis auto seisab, kui lükata ettepoole, hakkab auto edasi liikuma, muutes sujuvalt oma kiirust vastavalt kangi asendile ning kangi tahapoole tõmmates hakkab auto vastavalt tagurpidi liikuma. See kang toimib üheaegselt nii käigukangi kui gaasipedaalina.
Juhtpuldi programmeerimine
Kummagi NXT, nii juhtpuldi kui auto jaoks, on vaja kirjutada oma programm. Siin lahenduses on tarvis ainult ühesuunalist kommunikatsiooni – puldist autole. Auto ei pea juhtpuldile midagi teatama. Seega juhtpuldi NXT on Bluetooth ühenduse korral masteri rollis ning auto oma on rollis slave. See peab olema just niipidi, kuna Bluetoothi korral on master see, kes alustab ühenduse loomist ja signaalide edastamist.
Juhtpult täidab kolme funktsiooni. Esiteks on juhtpult see, mis algatab Bluetooth ühenduse loomise. Teise funktsioonina saadab juhtpult autole käsklused edasisõitmise ning pööramise kohta. Kolmandaks kuvab juhtpult oma ekraanil auto rooli ja gaasipedaali asendid.
Bluetooth ühenduse loomine on tehtud siin standardsel viisil, nagu seda on kirjeldatud käesoleva raamatu juhendis Bluetooth ühenduse loomise osas. Seega on Bluetooth ühenduse loomiseks vajalik lisafail BluetoothCheck.nxc, mida kasutavad nii master kui slave programmid ja seda pole siinkohal rohkem käsitletud.
Info edastamine
Info edastamise alamprogramm saadab autole infot rooli ja pedaali asendite kohta.
Selle alamprogrammi sees on lõpmatuseni käiv while-tsükkel, kuni ta tsükliväliselt lõpetatakse. Tsükli käivitumisel omistatakse muutujale rool väärtus A mootorist, mida kasutatakse roolina. Seejärel kontrollitakse, kas saadud väärtus jääb vahemikku -180 kuni 180. Kui muutuja väärtus on suurem kui 180, siis säilitatakse selles jätkuvalt väärtus 180. See kontroll tagab, et auto rooli juhtimise programm saab kindla numbrivahemiku rooli keeramiseks. Selge on see, et auto rattad ei hakka keerama 360 kraadi, vaid keeravad kokku ca 90 kraadi – 45 kraadi ühele ja 45 teisele poole.
Seejärel omistatakse muutuja rooli väärtus globaalsele muutujale RooliNurk, mida kasutatakse rooli asukoha joonistamiseks. Seejärel saadetakse rooli väärtus Bluetoothi kaudu auto NXT-le käsuga SendRemoteNumber(BTconn, MAILBOX1, rool).
Sama tegevus toimub gaasipedaaliga, kuid selle vahega, et numbrid hoitakse vahemikus -90…90 kraadi ja muutujale PedaaliNurk, millega joonistatakse gaasipedaali asukoht ekraanil, omistatakse väärtus muutujast kiirus.
Näide. Juhtpuldi programm, info edastamine autole
task InfoEdastamine()
{
//loome muutujad rooli ning gaasipedaali tarvis
long rool, kiirus;
//käivitame lõpmatu tsükli, mille käigus saadetakse
//SLAVE-le rooli ning käigukangi asendi info
while (TRUE)
{
//omistame muutujale rool väärtused mootorist A: +/-180
rool = MotorRotationCount(OUT_A);
if (rool>180) rool = 180;
if (rool<-180) rool = -180;
RooliNurk = rool;
//saadame rooli väärtuse SLAVE NXT-le
SendRemoteNumber(BTconn, MAILBOX1, rool);
//omistame muutujale kiirus väärtused mootorist B: +/-90
kiirus = MotorRotationCount(OUT_B);
if (kiirus>90) kiirus = 90;
if (kiirus<-90) kiirus = -90;
PedaaliNurk = kiirus;
//saadame gaasipedaali väärtuse SLAVE NXT-le
SendRemoteNumber(BTconn, MAILBOX2, kiirus);
}
}
Rooli kuvamine ekraanil
Rooli kuvamisel ekraanil on meelelahutuslik ja õpetlik eesmärk. Ekraanil antakse edasi rooli kujutis, mis keerab kaasa, kui rooli keeratakse.
Alamprogrammi RooliAsukoht käivitamisel luuakse konstant RooliRaadius, mis määrab rooli suuruse ekraanil, ning muutujad xAlg ja yAlg, mis määravad rooli keskkoha ekraanil.
Pärast seda käivitatakse lõpmatu tsükkel, milles kasutusel olev globaalne muutuja RooliNurk väärtustatakse rooliinfot edastava mootori positsiooniga 1000 korda sekundis.
Rool on kolme roolipulgaga, nende jaoks on vajalikud roolipulkade lõppkoordinaadid x1, x2, x3, ja y1, y2, y3. Roolipulgad joonistatakse ekraanile funktsiooniga LineOut, mille alg-koordinaat on ringjoone keskpunkt ning lõppkoordinaat roolipulga lõpp, mis asub rooli välimisel ringil.
Lõppkoordinaadid leitakse täisnurkse kolmnurga lahendamisega. Kuna rooli läbimõõt on muutumatu, siis roolipulga pikkust on alati ringi raadiuseks. Muutub aga rooli asend, seega ka nurk rooli algse ehk keskasendi suhtes. Teades rooli raadiust (kolmnurga hüpotenuus) ja rooli pöördenurka (kolmnurga nurk), saab välja arvutada roolipulga välimised koordinaadid siinus ja koosinus teoreeme kasutades.
Teised roolipulgad asuvad keskmise roolipulga suhtes 120 kraadi võrra paremal ja 120 kraadi võrra vasakul. Seega koordinaatide x2, y2 ja x3, y3 leidmiseks vastavalt liidetakse ja lahutatakse rooli pöördenurgast 120 enne siinuse ja koosinuse võtmist.
Järgmiste valemite abil saab lahendada roolipulkade kuvamise ülesande.
Vastaskaatet: x-telje koordinaat
Lähiskaatet: y-telje koordinaat
Teravnurk: rooli pöördenurk kraadides
Hüpotenuus: rooli raadius
Järgnev joonis ilmestab roolipulkade arvutamist, vastaskaatet on võrdne x-telje koordinaadiga ja lähiskaatet on võrdne y-telje koordinaadiga.
Programmi koodis joonistavad järgmised kolm funktsiooni CircleOut valmis rooli välimise ketta. Iga järgmine ring on eelmisest ühe piksli võrra suurem. Seega rooliratta välimise ringi paksus on 3 pikslit. Neljas CircleOut joonistab roolile rooliratta sisemise ringi ja värvib selle seest piksleid täis.
Kolm funktsiooni LineOut joonistavad valmis kolm erinevat roolipulka, mille koordinaadid on eelnevalt välja arvutatud.
Näide. Rooli keeramise kuvamine ekraanil
task RooliAsukoht()
{
const int RooliRaadius = 25;
int xAlg=50;
int yAlg=30;
int x1, x2, x3;
int y1, y2, y3;
while(TRUE)
{
ClearScreen();
//arvutatakse välja ülemise roolisamba koordinaat
x1 = sind(RooliNurk)*RooliRaadius;
y1 = cosd(RooliNurk)*RooliRaadius;
//arvutatakse välja parempoolse roolisamba koordinaat
x2 = sind(RooliNurk+120)*RooliRaadius;
y2 = cosd(RooliNurk+120)*RooliRaadius;
//arvutatakse välja vasakpoolse roolisamba koordinaat
x3 = sind(RooliNurk-120)*RooliRaadius;
y3 = cosd(RooliNurk-120)*RooliRaadius;
//joonistatakse välimine rooliratta ring
CircleOut(xAlg, yAlg, RooliRaadius);
CircleOut(xAlg, yAlg, RooliRaadius+1);
CircleOut(xAlg, yAlg, RooliRaadius+2);
//joonistatakse rooliratta keskmine ring
//ja värvitakse seest täis
CircleOut(xAlg, yAlg, 5, DRAW_OPT_FILL_SHAPE);
//kuvame ekraanil rooli ülemise samba
LineOut(xAlg, yAlg, xAlg+x1, yAlg+y1);
//kuvame ekraanil rooli parempoolse samba
LineOut(xAlg, yAlg, xAlg+x2, yAlg+y2);
//kuvame ekraanil rooli vasakpoolse samba
LineOut(xAlg, yAlg, xAlg+x3, yAlg+y3);
Wait(80);
}
}
Gaasipedaali kuvamine ekraanil
Ekraanil kuvatakse väikeste ristkülikutega gaasipedaali asukoht. Kui pedaal on keskel, siis auto seisab, ning vastavalt ülespoole lükates liigub auto edasi ja allapoole tõmmates tagasi.
Muutujaid xAlg ja yAlg kasutatakse esimese ehk siis keskmise ruudu joonistamiseks ekraanile. Muutuja laius määrab joonistatava ruudu laiuse. Muutuja korgus määrab kõrguse pikslites, mis on ühe ruudu alumisest küljest kuni järgmise ruudu alumise küljeni. Muutuja RuutKorgus on kahe ühiku võrra väiksem muutujast korgus, arvestades ruudu külje joonistamise paksuse ja ruutude vahele jääva ühe piksli suuruse vahega.
Globaalsest muutujast PedaaliNurk saabub gaasipedaali asendi info vahemikus -90 .. 90. Programmis jagatakse see kolmega, et saada tulemuseks -30…30, mis on sobilik ekraanile joonistamiseks, kuna ekraani kõrgus on 64 pikslit.
Järgnevalt on kaks for-tsüklit, esimene neist on kasutusel ülemiste ja teine alumiste ristkülikute joonistamisel. Kogu programmikood on lõpmatu tsükli sees, seega igal ringil käivitatakse mõlemad for-tsüklid ja arvutatakse uuesti ristkülikute asukohad.
For-tsükkel alustab iga kord nullist ja käib kuni numbrini, mis saadakse muutujast GaasiPedaal. Tsükli sees on if-tingimuslause, mis kontrollib, kas for-tsükli numbri jagamisel muutujaga kõrgus on jääk null. Kuna käesolevas näites on kõrgus=5, siis tulemus on null iga viie ühiku järel. Seega kui GaasiPedaal on maksimaalses asendis, leiab antud if-lause kuus korda nulli (30/5=6). Kui if-lause tingimus on tõene, joonistatakse valmis ristkülik, mille x-koordinaat on xAlg ja y-koordinaat on yAlg, millele on liidetud üks kõrgus (see on vajalik esimese ruudust möödumiseks) ning liidetakse juurde for-tsükli vastav number. Tulemus on see, et iga 5 piksli järel joonistatakse uus ristkülik ja iga ristküliku vahele jääb 1 piksel. Järgmisel joonisel on välja toodud kuidas vastavalt pedaali asendile arvutatakse for-tsükli sees välja ruudu joonistamise asukoht.
Sarnase loogika kohaselt saab tekitada allapoole joonistatavad ristkülikud, kuid miinusmärgiga.
Näide. Gaasipedaali asendi kuvamine ekraanil
task PedaaliAsukoht()
{
int xAlg=90;
int yAlg=28;
int laius=9;
int korgus=5;
int RuutKorgus=korgus - 2;
while(TRUE)
{
//kuvame keskmise ristküliku
RectOut(xAlg, yAlg, laius, RuutKorgus);
//jagame gaasipedaali 3-ga,
//tulemuseks on poole ekraani kõrgus
int GaasiPedaal=PedaaliNurk/3;
//kuvame ekraani ülemise poole ristkülikud
for(int i=0; i<GaasiPedaal; i++)
{
//iga uus ristkülik joonistatakse
//ühiku "kõrgus" järel
if(i%korgus==0)
//joonistatakse ristkülik
RectOut(xAlg, yAlg+korgus+i, laius, RuutKorgus);
}
//kuvame ekraani alumisele poolele ristkülikud
for(int i=0; i>GaasiPedaal; i--)
{
if(i%korgus==0)
//joonistatakse ristkülik
RectOut(xAlg, yAlg+i, laius, RuutKorgus);
}
}
}
Nutika auto programmeerimine
Auto tegeleb Bluetoothi vahendusel info vastuvõtmisega, esimeste rataste pööramisega ning vedavate rataste juhtimisega. Lisaks nimetatud funktsioonidele on autol üks ühekordne ülesanne, mis seisneb pööravate rataste keskasendisse panemises.
Bluetooth ühendus luuakse standardsel viisil, seda on käsitletud juhendis Bluetooth ühenduse loomise juures ja siin rohkem üle ei korrata.
Info vastuvõtmine
Info vastuvõtmine on lõpmatu tsükkel, mis saab suure sagedusega (ca 100 korda sekundis) puldi käest infot ja salvestab selle globaalsetesse muutujatesse Rool ning Kiirus.
Näide. Bluetooth info vastuvõtmine
task InfoVastuvott()
{
while(TRUE)
{
//Rool on vahemikus -180..180
//Kiirus on vahemikus -90..90
ReceiveRemoteNumber (MAILBOX1, FALSE, Rool);
ReceiveRemoteNumber (MAILBOX2, FALSE, Kiirus);
}
}
Rataste keeramine keskasendisse
Autot käivitades pole teada, millisesse asendisse jäid pööravad rattad, seega enne sõitma hakkamist on tarvis saada need keskasendisse. Keskasendisse keeramine töötab põhimõttel, et rattad keeratakse paremale ja siis vasakule lõpuni välja ning arvutatakse seejärel saadud kraadide vahe.
Programmi alguses keeratakse rattad ühe sekundi jooksul paremale. Sõltuvalt rooli ülekandest, kui kaua läheb aega ühest servast teise jõudmisega, võib seda ootamist kas pikendada või lühendada. Seejärel omistatakse muutujale parem mootori pöörde kraadid. Roolimootor keeratakse vasakule ning muutujale vasak omistatakse teises servas mootori pöörde kraadid.
Seejärel võetakse vasaku ja parema poole vahe, jagatakse see kahega ning lisatakse miinusmärk. Selle tehte tulemusena on programmil teada, kui palju on vaja hetkeasendist keerata tagasi, et rattad asuksid keskel.
Funktsioon PosRegEnable võimaldab kasutada mootorit sellisel moel, et seda saab kraadi täpsusega juhtida ja hoida ettenähtud kraadi peal algse suuna suhtes. Selle omaduse abil on võimalik väga täpselt juhtida keeravaid rattaid. See funktsioon kasutab mootori hoidmiseks PID kontrollerit ning kolm viimast numbrilist parameetrit ongi vastavalt P, I ja D parameetrid. PID kontrollerist on juttu joonejärgija juures, seega siinkohal sellel pikemalt ei peatuta.
Funktsioon PosRegSetAngle võimaldab mootorit keerata täpselt mingi kraadi võrra ja hoida mootori positsiooni selles kindlas asendis. Käesoleval juhul on arvestatud, et mootor on pärast vasakule keeramist igal juhul ühes servas, seega mootorit peab tagasi keerama kraadi võrra, mis arvutati välja ja omistati muutujale RattadKeskel.
Nüüd on rattad keskel, seega võib käivitada rataste keeramise ja edasi-tagasi sõitmise alamprogrammid.
Näide. Rataste keeramine keskele
task RattadKeskele()
{
long parem, vasak;
//rattad keeratakse paremale lõpuni välja 1 sekundi jooksul
OnFwd(OUT_C, 40);
Wait(SEC_1);
parem = MotorRotationCount(OUT_C);
//rattad keeratakse vasakule lõpuni välja 1 sekundi jooksul
OnFwd(OUT_C, -40);
Wait(SEC_1);
vasak = MotorRotationCount(OUT_C);
//mootorid lülitatakse välja
Off(OUT_C);
//arvutatakse saadud tulemuste vahe ning jagatakse see kahega
RattadKeskel = -((vasak-parem)/2);
//võimaldatakse kasutada mootorit viisil,
//et seda saab kraadi kaupa juhtida ja hoida kindlas asendis
PosRegEnable(OUT_C, 40, 10, 10);
//rattad pööratakse keskasendisse
PosRegSetAngle(OUT_C, RattadKeskel);
Wait(SEC_1);
//käivitatakse rataste keeramise ja
//edasisõitmise alamprogrammid
StartTask(KeeraRattaid);
StartTask(RobotSoida);
}
Sõitmine ja keeramine
Roboti edasisõitmise ja keeramise jaoks on omaette alamprogrammid. See on lõpmatult käiv tsükkel, mis annab Bluetooth vastuvõtust saadud kiiruse info edasi vedavatele mootoritele.
Näide. Sõitmise alamprogramm
task RobotSoida()
{
while(TRUE)
{
//roboti edasisõitmine
OnFwdReg(OUT_B, Kiirus, OUT_REGMODE_SPEED);
}
}
Rataste keeramise alamprogramm saab info Bluetooth vastuvõtjast globaalse muutujaga Rool. Sellele liidetakse juurde suurus, mis arvutati rataste keskele pööramise funktsioonist. See tagab rataste õige käitumise. Käsuga PosRegSetAngle(OUT_C, Asend) toimub roolirataste pööramine. PosRegSetAngle funktsiooni kasutamine võimaldati RattadKeskele alamprogrammis funktsiooniga PosRegEnable, seetõttu seda siin alamprogrammis rohkem kordama ei pea.
Näide. Keeramise alamprogramm
task KeeraRattaid()
{
long Asend;
while(TRUE)
{
//roolile liidetakse juurde eelnevalt saadud
//rataste keskasukoht
Asend=Rool+RattadKeskel;
//pööravate rataste mootori juhtimine
PosRegSetAngle(OUT_C, Asend);
}
}
Baasroboti auto programmeerimine
Baasrobotist puldiautol on kaks vedavat ratast omaette mootoritega. Kui robot peab otse sõitma, siis mõlemad mootori liiguvad võrdse kiirusega. Kui aga robot peab keerama, siis üks mootoritest liigub aeglasemalt ja teine kiiremini.
Järgnevalt on välja toodud rataste juhtimise alamprogramm. Rataste juhtimise sisendiks saab kasutada Kiirus ja Rool nimelisi globaalseid muutujaid, mis saadakse Bluetoothi kaudu juhtpuldi käest.
Rataste juhtimine on lõpmatu tsükli sees. Esimese arvutustehtega tagatakse, et rooli suund mõjutaks mootoreid vastassuunaliselt. Nimelt mootor B-le liidetakse rooli info ja mootor C-st lahutatakse rooliinfo. Rooli info leitakse tehtega Kiirus*Rool/200. Kiirus korrutatakse rooliga seetõttu, et kui auto sõidab kiiremini, siis rooli mõju peab olema suurem ning aeglase sõidu korral peab rooli mõju olema väiksem. Rooli info jagatakse omakorda 200-ga, kuna Bluetoothi kaudu on rooli muutuja väärtus vahemikus -180..180 ja see on liiga suur vahemik, seega antud jagamistehtega teisendame rooli info auto juhtimiseks sobilikuks vahemikuks.
Järgmine rida MotorB=abs(MotorB)<abs(Kiirus)?MotorB:Kiirus on ternary-tingimuslause, kus võrreldakse eelnevalt välja arvutatud mootori kiiruse juhtpuldist tuleva kiiruse absoluutväärtuseid. Mootorile antakse edasi neist väiksem suurus.
Selle tingimuslause tulemusena auto keerab ratta aeglustamisega ning keeramine tundub loomulik. Kui see tingimuslause ära jätta, siis keeramise korral auto kiirendaks ühe rattaga ja teisega aeglustaks, mis teeks roboti keeramise veidi ebaloomulikuks.
Näide. Roboti sõitmise ja keeramise alamprogramm
task RobotSoida()
{
int MotorB, MotorC;
while(TRUE)
{
//puldist saadud kiiruse ja rooli info teisendamine
MotorB = Kiirus + Kiirus*Rool/200;
MotorC = Kiirus - Kiirus*Rool/200;
//kiiruse muutmine viisil, et keeramine toimuks ainult
// ratta aeglustamisega, st. võetakse minimaalne suurus
MotorB = abs(MotorB) < abs(Kiirus) ? MotorB : Kiirus;
MotorC = abs(MotorC) < abs(Kiirus) ? MotorC : Kiirus;
//roboti edasisõitmine
OnFwdReg(OUT_B, MotorB, OUT_REGMODE_SPEED);
OnFwdReg(OUT_C, MotorC, OUT_REGMODE_SPEED);
}
}
Lõplikud programmid
Näide. Juhtpuldi programm
//MASTER. pult
#include "BluetoothCheck.nxc"
const byte BTconn=1;
const string SlaveNXTName = "NXT6";
int RooliNurk;
int PedaaliNurk;
task RooliAsukoht()
{
int RooliRaadius = 25;
int xAlg=50;
int yAlg=30;
int x1, x2, x3;
int y1, y2, y3;
while(TRUE)
{
ClearScreen();
//arvutatakse välja ülemise roolisamba koordinaat
x1 = sind(RooliNurk)*RooliRaadius;
y1 = cosd(RooliNurk)*RooliRaadius;
//arvutatakse välja parempoolse roolisamba koordinaat
x2 = sind(RooliNurk+120)*RooliRaadius;
y2 = cosd(RooliNurk+120)*RooliRaadius;
//arvutatakse välja vasakpoolse roolisamba koordinaat
x3 = sind(RooliNurk-120)*RooliRaadius;
y3 = cosd(RooliNurk-120)*RooliRaadius;
//joonistatakse välimine rooliratta ring
CircleOut(xAlg, yAlg, RooliRaadius);
CircleOut(xAlg, yAlg, RooliRaadius+1);
CircleOut(xAlg, yAlg, RooliRaadius+2);
CircleOut(xAlg, yAlg, RooliRaadius+3);
//joonistatakse keskmine rooliratta ring
//ja värvitakse seest täis
CircleOut(xAlg, yAlg, 5, DRAW_OPT_FILL_SHAPE);
//kuvame ekraanil rooli ülemise samba
LineOut(xAlg, yAlg, xAlg+x1, yAlg+y1);
//kuvame ekraanil rooli parempoolse samba
LineOut(xAlg, yAlg, xAlg+x2, yAlg+y2);
//kuvame ekraanil rooli vasakpoolse samba
LineOut(xAlg, yAlg, xAlg+x3, yAlg+y3);
Wait(80);
}
}
task PedaaliAsukoht()
{
int xAlg=90;
int yAlg=28;
int laius=9;
int korgus=5;
int RuutKorgus=korgus - 2;
while(TRUE)
{
//kuvame keskmise ristküliku
RectOut(xAlg, yAlg, laius, RuutKorgus);
//jagame gaasipedaali 3-ga, tulemuseks on
//ligikaudne poole ekraani kõrgus
int GaasiPedaal=PedaaliNurk/3;
//kuvame ekraani ülemise poole ristkülikud
for(int i=0; i<GaasiPedaal; i++)
{
//iga ristkülik joonistatakse ühiku "kõrgus" järel
if(i%korgus==0)
//joonistatakse ristkülik
RectOut(xAlg, yAlg+korgus+i, laius, RuutKorgus);
}
//kuvame ekraani alumise poole ristkülikud
for(int i=0; i>GaasiPedaal; i--)
{
if(i%korgus==0)
//joonistatakse ristkülik
RectOut(xAlg, yAlg+i, laius, RuutKorgus);
}
}
}
task InfoEdastamine()
{
//muutujad rooli ja gaasipedaali/käigukangi tarvis
long rool, kiirus;
//käivitame info edastasmise tsükli, mis saadab
//100 korda sekundis rooli ning käigukangi asendit
while (TRUE)
{
//omistame muutujale rool
//väärtused mootorist A: +/-180
rool = MotorRotationCount(OUT_A);
if (rool>180) rool = 180;
if (rool<-180) rool = -180;
RooliNurk = rool;
//rooli saadetav info muudetakse märgiliselt vastupidiseks,
//see sõltub mootori asendist, kui mootor ringi keerata
//tuleb alljärgnev rida eemaldada
rool = -rool;
//saadame rooli väärtuse SLAVE NXT-le
SendRemoteNumber(BTconn, MAILBOX1, rool);
//omistame muutujale kiirus
//väärtused mootorist B: +/-90
kiirus = MotorRotationCount(OUT_B);
if (kiirus>90) kiirus = 90;
if (kiirus<-90) kiirus = -90;
PedaaliNurk = kiirus;
//kiiruse saadetav info muudetakse märgiliselt vastupidiseks,
//see sõltub mootori asendist, kui mootor ringi keerata
//tuleb alljärgnev rida eemaldada
kiirus = -kiirus;
//saadame gaasipedaali väärtuse SLAVE NXT-le
SendRemoteNumber(BTconn, MAILBOX2, kiirus);
}
}
task main()
{
//käivitatakse BT ühenduse kontroll ja/või loomine
//kui ühendamine ei õnnestunud, väljutakse programmist
if(!BluetoothConnect(BTconn, SlaveNXTName)==NO_ERR)
Stop(TRUE);
//kuvame kasutajale ühe sekundi jooksul teate
Wait(SEC_1);
ClearScreen();
//palume rooli ja käigukangi keskasendisse keerata
TextOut(0, LCD_LINE1, "Palun keera");
TextOut(0, LCD_LINE2, "rool ja");
TextOut(0, LCD_LINE3, "gaasipedaal");
TextOut(0, LCD_LINE4, "keskasendisse.");
while(!ButtonPressed(BTNCENTER, FALSE));
while(ButtonPressed(BTNCENTER, FALSE));
//nullime mootori counterid
ResetRotationCount(OUT_AB);
StartTask(RooliAsukoht);
StartTask(PedaaliAsukoht);
StartTask(InfoEdastamine);
}
Nutika auto programm
See programm on mõeldud autole, millel esimesi rattaid keeratakse ühe mootoriga ja tagumised on ühendatud omavahel diferentsiaaliga ning neid juhitakse samuti ühe mootoriga.
Siin on kõik eelnevalt kirjeldatud programmi osad kokku pandud. Auto peal olev NXT on slave-rollis.
Näide. Nutika auto programm
//SLAVE. Auto
#include "BluetoothCheck.nxc"
long Rool;
long Kiirus;
long RattadKeskel;
//Bluetooth info vastuvõtu alamprogramm
task InfoVastuvott()
{
while(TRUE)
{
//Rool on vahemikus -180..180
//Kiirus on vahemikus -90..90
ReceiveRemoteNumber (MAILBOX1, FALSE, Rool);
ReceiveRemoteNumber (MAILBOX2, FALSE, Kiirus);
}
}
//roboti edasisõitmise alamprogramm
task RobotSoida()
{
while(TRUE)
{
//roboti edasisõitmine
OnFwdReg(OUT_B, Kiirus, OUT_REGMODE_SPEED);
}
}
//rataste keeramise alamprogramm
task KeeraRattaid()
{
long Asend;
while(TRUE)
{
//roolile liidetakse juurde eelnevalt saadud
//rataste keskasukoht
Asend=Rool+RattadKeskel;
//pööravate rataste mootori juhtimine
PosRegSetAngle(OUT_C, Asend);
}
}
//alamprogramm RattadKeskele sätib
//rattad enne sõitma hakkamist otseks
task RattadKeskele()
{
long parem, vasak;
//rattad keeratakse paremale
//lõpuni välja 1 sekundi jooksul
OnFwd(OUT_C, 40);
Wait(SEC_1);
parem = MotorRotationCount(OUT_C);
//rattad keeratakse vasakule
//lõpuni välja 1 sekundi jooksul
OnFwd(OUT_C, -40);
Wait(SEC_1);
vasak = MotorRotationCount(OUT_C);
//mootorid lülitatakse välja
Off(OUT_C);
//arvutatakse saadud tulemuse vahe
//ning jagatakse see kahega
RattadKeskel = -((vasak-parem)/2);
//võimaldatakse kasutada mootorit viisil,
//et seda saab kraadi kaupa juhtida ja hoida
PosRegEnable(OUT_C, 40, 10, 10);
//rattad pööratakse keskasendisse
PosRegSetAngle(OUT_C, RattadKeskel);
Wait(SEC_1);
//käivitatakse keeramise ja edasisõitmise alamprogrammid
StartTask(KeeraRattaid);
StartTask(RobotSoida);
}
task main ()
{
//käivitatakse BT ühenduse kontroll
//kui ühendamine ei õnnestunud, väljutakse programmist
if(!BluetoothCheck()==NO_ERR)
Stop(TRUE);
//käivitatakse rataste keskele keeramise alamprogramm
StartTask(RattadKeskele);
//käivitatakse Blutooth info vastuvõtu programm
StartTask(InfoVastuvott);
while (TRUE)
{
ClearScreen();
TextOut(0, LCD_LINE4, FormatNum("Rool: %d", Rool));
TextOut(0, LCD_LINE3,
FormatNum("Kiirus: %d", Kiirus));
Wait(MS_200);
}
}
Baasrobotist puldiauto programm
Baasrobotist puldiauto juhtimiseks saab kasutada sama pulti koos programmiga. Ainuke erinevus seisneb vastuvõtu programmis.
Näide. Baasrobotiga auto programm
//SLAVE
#include "BluetoothCheck.nxc"
long Rool, Kiirus;
long RattadKeskel;
//Bluetooth info vastuvõtu alamprogramm
task InfoVastuvott()
{
while(TRUE)
{
//Rool on vahemikus -180..180
//Kiirus on vahemikus -90..90
ReceiveRemoteNumber (MAILBOX1, FALSE, Rool);
ReceiveRemoteNumber (MAILBOX2, FALSE, Kiirus);
}
}
//roboti edasisõitmise alamprogramm
task RobotSoida()
{
int MotorB, MotorC;
while(TRUE)
{
//puldist saadud kiiruse ja rooli info teisendamine
MotorB = Kiirus + Kiirus*Rool/200;
MotorC = Kiirus - Kiirus*Rool/200;
//kiiruse muutmine viisil, et keeramine toimuks ainult
//ratta aeglustamisega, st. võetakse minimaalne suurus
MotorB = abs(MotorB) < abs(Kiirus) ? MotorB : Kiirus;
MotorC = abs(MotorC) < abs(Kiirus) ? MotorC : Kiirus;
//roboti edasisõitmine
OnFwdReg(OUT_B, MotorB, OUT_REGMODE_SPEED);
OnFwdReg(OUT_C, MotorC, OUT_REGMODE_SPEED);
}
}
task main ()
{
//käivitatakse BT ühenduse kontroll
//kui ühendamine ei õnnestunud, väljutakse programmist
if(!BluetoothCheck()==NO_ERR)
Stop(TRUE);
//käivitatakse Blutooth info vastuvõtu programm
StartTask(InfoVastuvott);
//käivitatakse roboti sõitmise alamprogramm
StartTask(RobotSoida);
while (TRUE)
{
ClearScreen();
TextOut(0, LCD_LINE4,
FormatNum("Rool: %d", Rool));
TextOut(0, LCD_LINE3,
FormatNum("Kiirus: %d", Kiirus));
Wait(MS_200);
}
}