Reaali Robootika.COM

NXT robotimaailm ja programmeerimine C-keeles

9. 9 klass 3 õppetundi Bluetooth puldiauto

Tiigrihype_logoSee 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

clip_image002Rooli 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.

 

clip_image004

 

Järgmiste valemite abil saab lahendada roolipulkade kuvamise ülesande.

clip_image006

clip_image008

 

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.  

clip_image010

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

clip_image012See 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);

  }

}

Add comment

Loading