Reaali Robootika.COM

NXT robotimaailm ja programmeerimine C-keeles

8. 9 klass 3 õppetundi Koopa kaardistaja robot

Tiigrihype_logoSee materjal on loodud Tiigrihüppe Sihtasutuse programmi ProgeTiiger raames.

Siit projektist omandad teadmise, kuidas kasutada kauguseandureid tundmatu piirkonna kaardistamiseks, kuidas edastada andmeid üle bluetooth ühenduse ja kuidas need inimese jaoks arusaadavaks teha.

 

Kooparobot kaardistab koopa ja selle kõrvalkäikude skeemi ning edastab info Bluetoothi vahendusel teisele, koopast väljas olevale robotile.

Käesoleva projekti raames on eeldatud, et koobas, mille robot läbib, on sirgjooneline ning kaardistada on tarvis kõrvalkäikude asukohad. Robot peab andmed saatma kaardistamise hetkel, et juhul, kui varisemisohtlik koobas peaks sisse langema, oleks osa infot juba edastatud.

Robot jõuab tunneli lõppu, millest annab märku puuteandur roboti ees. Kuna tunnel on sirgjooneline, pole vaja robotil ringi keerata, vaid ta võib sama teed mööda tagurpidi tagasi tulla.

 

 

clip_image002

Roboti ehitus

Robot peab suutma liikuda sirgjoonelises koopas vaid edasi-tagasi, robot ei pea keerama. See tähendab, et robot võib olla lihtne ja ühe mootoriga.

NB! Kui on olemas valmis baasrobot kahe mootoriga, siis otseliikumise tagamiseks võib mootorid omavahel jäiga pulgaga ühendada. See võte on lubatud ning ei riku kuidagi mootoreid, kuid sellisel juhul peab mootorid võrdselt tööle panema näiteks käsuga OnFwd(OUT_BC, 75).

Seega on koopa kaardistamise robot iseenesest lihtne ning põhineb tavalisel baasrobotil, millele on kinnitatud külgedele kauguseandurid, võimaldamaks tuvastada kaugust külgsuunas.

Roboti ette on tarvis kinnitada lüliti, mis rakendub siis, kui robot sõidab vastu seina. See teavitab robotit, et on jõutud koopa lõppu ning tuleb hakata tagasi sõitma.

Seda robotit nimetame edaspidi kooparobotiks või saatjaks. Bluetooth ühenduse osas on see robot masteri rollis.

Teine robot, mis infot vastu võtab, on kasutusel sisuliselt ekraanina, mis kuvab koopast väljasolevale inimesele käikude asukohad. Seda nimetame edaspidi puldirobotiks, juhtpuldiks või vastuvõtjaks. Bluetooth ühenduse osas on see robot slave´i rollis.

Kooparoboti programmeerimine

Programm on vaja kirjutada mõlema roboti jaoks, nii kooparobotile kui ka puldirobotile. Kooparobot sõidab koopas edasi ja tagasi ning edastab mootori ja kauguseandurite signaalid vastuvõtjale. Juhtpult annab kooparobotile käskluse liikumise alustamiseks ning teisendab vastuvõetud signaalid koopa jooniseks.

Alljärgnev diagramm iseloomustab kooparoboti käitumist. Esmalt käivitatakse Bluetooth ühenduse kontroll ja seejärel jääb robot ootama, kuni juhtpult oma valmisolekust märku annab. Kui inimene on juhtpuldil nuppu vajutanud, alustab kooparobot liikumist.

Sõitmise ajal kontrollitakse pidevalt roboti ees oleva lüliti asendit, mille alusel otsustatakse, kas robot on jõudnud koopa lõpuni. Samal ajal edasi liikudes on käivitatud paralleelselt alamprogramm andmete saatmiseks puldirobotile. Roboti koopast välja jõudmise kontroll baseerub mootori pöörete arvu lugemisel. Seega, kui tagasi sõites on mootori pöörded nulli jõudnud, on robot koopast välja jõudnud ja jääb seisma.

clip_image004

Bluetooth ühenduse loomine

Bluetooth ühenduse loomine on tehtud standardsel viisil, nagu seda on kirjeldatud käesoleva raamatu juhendis Bluetooth ühenduse loomise osas.

Stardi ootamine

Alamprogramm StardiOotamine tagab selle, et kooparobot ei hakka enne liikuma, kui puldirobot on talle alustamise kohta käskluse edastanud. Sellega sünkroniseeritakse robotite tegevus ja tagatakse korrektne kaardistamine.

Loodav kahendmuutuja ACK kasutatakse while-tsükli tingimusena. Kui see muutub tõeseks, väljub robot stardi ootamise while-tsüklist ning hakkab edasi sõitma ja koobast kaardistama.

Funktsioon SendRemoteBool(BTconn, MAILBOX6, TRUE) saadab puldirobotile tõeväärtusena TRUE (kolmas parameeter). Seejärel ootab robot 10 ms enne vastuvõtmise kontrollimist ReceiveRemoteBool(MAILBOX5, TRUE, ACK) ja kuvab ekraanile teksti „Ootan starti …“. Kui puldirobot on kätte saanud MAILBOX6 pealt teate TRUE, ootab puldirobot seni, kuni kasutaja vajutab nuppu ehk siis annab stardi. Pärast nupuvajutust saadab puldirobot tagasi MAILBOX5 kaudu väärtuse TRUE, mis omistatakse kooparoboti poolel muutujale ACK. Kui ACK on saanud väärtuse TRUE, väljutakse tsüklist ja alustatakse liikumist ning andurites info edastamist.

Näide. Kaardistaja roboti ootamise protseduur

//Stardi ootamine on vajalik selleks,

//et sünkroniseerida NXT-de tegevus,

//kuna nende programmid käivitatakse erinevatel aegadel.

void StardiOotamine()

{

  bool ACK=FALSE;

  while(!ACK)

  {

    //saadetakse SLAVE NXT-le info,

    //et "ma olen valmis" - TRUE

    SendRemoteBool(BTconn, MAILBOX6, TRUE);

    Wait(MS_10);

    //oodatakse SLAVE NXT pealt stardikäsklust,

    //siis ACK=TRUE

    ReceiveRemoteBool(MAILBOX5, TRUE, ACK);

    TextOut(0, LCD_LINE1, "Ootan starti ...");

    Wait(MS_10);

  }

}

Roboti liikumine koopas edasi ja tagasi

Kooparobot alustab liikumist, kui on saanud selleks vastava käsu puldiroboti käest. Robot sõidab otse edasi OnFwd(OUT_BC, 30) kuni koopa lõpuni, ehk siis hetkeni, kui puuteandur on vajutatud while(!KoopaFinish).

Pärast puuteanduri vajutust käivitub järgmine while-tsükkel, mille tingimuseks on seatud mootori pöörete lugemise nulli jõudmine while(MotorRotationCount(OUT_B)>0).

Seda moodust kasutame selleks, et teada saada, millal robot on tagasi koopast välja jõudnud.

Sõidu alguses on mootori pöörete arv null ja edasi sõites see suureneb. Tagasi sõites aga mootori pöörete arv väheneb.

Näide. Koopas liikumise alamprogramm

//robot liigub koopa lõpuni, kuni puuteandur on vajutatud

//seejärel liigub tagasi, kuni mootori pöörded saavad taas nulli

task move()

{

  //antud tsükkel käib seni, kuni pole vajutatud nuppu

  while(!KoopaFinish)

  {

    //robot sõidab edasi

    OnFwd(OUT_BC, 30);

    //muutujale KoopaFinish omistatakse puuteanduri väärtus

    KoopaFinish = Sensor(S1);    

  }

  //pärast puuteanduri vajutust käivitub järgmine tsükkel,

  //mis käib, kuni mootori pöörete arv on jõudnud 0 või alla selle

  while(MotorRotationCount(OUT_B)>0)

    OnRev(OUT_BC, 100);

  //robot jääb programmi lõppedes seisma

  Off(OUT_BC);

}

Info edastamine Bluetoothi vahendusel

Paralleelselt roboti liikuma hakkamisega käivitatakse alamprogramm saadaInfo, mis edastab infot mootori pöörete, kauguseandurite ja koopa lõppu jõudmise kohta.

Alamprogrammi alguses käivitub lõpmatu while-tsükkel, mille sees toimub info saatmine.  Esmalt loetakse anduritest info ja omistatakse nende väärtused muutujatele. Pärast seda kuvatakse antud info kooparoboti ekraanil. Kuigi seda infot koopas keegi ei näe, on seda hea kasutada roboti testimisel.

Seejärel saadetakse iga info omaette postkasti kaudu teisele NXT-le, kusjuures iga saatmise vahel on 1 ms pikkune paus, mis on piisav, et Bluetooth suudaks neid käskusid iseseisvalt täita.

Näide. Andmete saatmise programm

//alamprogramm saadaInfo saadab ligikaudu

//100 korda/sekundis

//anduritest saabuvat infot vastuvõtjale

task saadaInfo()

{

  while(TRUE)

  {

    //loeme andurist andmed, mootori pöörded

    long MootoriPoorded = MotorRotationCount(OUT_B);

    //loeme anduritest andmed, parem-vasak kauguseandur

    int sensorP = SensorUS(S3);

    int sensorV = SensorUS(S4);

   

    //kuvame ekraanil info andurite olukorra kohta

    TextOut(0, LCD_LINE1,

          FormatNum("Mootor: %5d", MootoriPoorded));

    TextOut(0, LCD_LINE2,

          FormatNum("Parem: %5d", sensorP));

    TextOut(0, LCD_LINE3,

          FormatNum("Vasak: %5d", sensorV));

   

    //saadame mootori pöörete väärtuse

    SendRemoteNumber(BTconn, MAILBOX1, MootoriPoorded);

    Wait(1);

    //saadame parempoolse kauguseanduri info

    SendRemoteNumber(BTconn, MAILBOX2, sensorP);

    Wait(1);

    //saadame vasakpoolse kauguseanduri info

    SendRemoteNumber(BTconn, MAILBOX3, sensorV);

    Wait(1);

    //saadame koopa lõpuinfo,

    //pärast seda enam ei kaardistata

    SendRemoteBool(BTconn, MAILBOX4, KoopaFinish);

    Wait(1);

  }

}

Puldiroboti programmeerimine

Alljärgnev diagramm iseloomustab roboti vastuvõtja-poolset tegevust.

Puldirobot ootab kooparoboti käest teavitust selle kohta, et viimane on valmis liikuma. Kui teavitus käes, kuvatakse kasutajale kiri „Anna start“ ning pärast nupuvajutust saadetakse koopa­robotile liikumise alustamise korraldus. Pärast seda alustab puldirobot info vastuvõtmist ja kaardistamast. Kui kooparobot on lõppu jõudnud, lõpetab puldirobot kaardistamise.

clip_image006

Start käskluse andmine

Alamprogramm StardiAndmine ootab kooparobotilt infot, et see on valmis, ning seejärel saadab omakorda vastuse liikumise alustamiseks.

Tsükkel koos tingimusega while(!ACK) käib seni, kuni puldirobot on saanud kooparoboti käest signaali ReceiveRemoteBool(MAILBOX6, TRUE, ACK), et kooparobot on valmis. Kui muutuja ACK väärtus muutub tõeseks, väljutakse tsüklist.

Seejärel kuvatakse ekraanile kiri „Anna start! Robot on valmis.“ Siis oodatakse kasutajapoolset nupuvajutust ning pärast seda saadetakse kooparobotile stardikäsklus SendResponseBool(MAILBOX5, TRUE), mille tulemusena hakkab kooparobot liikuma ja puldirobot kaardistama.

Näide. Puldirobotilt stardi saatmine

//stardiandmine on vajalik kahe

//NXT tegevuse sünkroniseerimiseks

void StardiAndmine()

{

  bool ACK=FALSE;

  //käivitub tsükkel, millest väljutakse pärast seda, kui

  //slave robot on saanud master-i käest info valmis

  while(!ACK)

  {

    //SLAVE ootab masteri käest infot

    ReceiveRemoteBool(MAILBOX6, TRUE, ACK);

    TextOut(0, LCD_LINE1, "Ootan roboti");

    TextOut(0, LCD_LINE2, "valmisolekut");

    Wait(MS_10);

  }

 

  TextOut(0, LCD_LINE1, "Anna start !");

  TextOut(0, LCD_LINE2, "Robot on valmis.");

 

  //oodatakse kuni kasutaja vajutab nuppu

  while(!ButtonPressed(BTNCENTER, FALSE));

  while(ButtonPressed(BTNCENTER, FALSE));

 

  //SLAVE saadab MASTER NXT-le stardikäskluse

  SendResponseBool(MAILBOX5, TRUE);

}

Bluetooth andmete vastuvõtmine

Pärast stardikäskluse saatmist käivituvad paralleelselt kaks alamprogrammi: Bluetooth ühenduse kaudu andmete vastuvõtmine ja nende andmete põhjal kaardistamine.

Alamprogrammi receiveBTsignal alguses käivitub lõpmatu tsükkel, mille sees olevad neli funktsiooni võtavad vastu Bluetoothi erinevatest postkastidest infot ja salvestavad need muutujatesse.

Näide. Puldirobotil Bluetooth andmete vastuvõtt

//võetakse vastu Bluetooth ühenduse kaudu infot

task receiveBTsignal()

{

  while(TRUE)

  {

    ReceiveRemoteNumber (MAILBOX1, FALSE, MootoriPoorded);

    ReceiveRemoteNumber (MAILBOX2, FALSE, sensorUSParem);

    ReceiveRemoteNumber (MAILBOX3, FALSE, sensorUSVasak);

    ReceiveRemoteBool (MAILBOX4, FALSE, KoopaFinish);

  }

}

Teepikkusega arvestamine

Koopa kaardistamisel peab arvestama asjaoluga, et NXT ekraani kõrgus on 64 punkti. Lisaks on hea ette teada kaardistatava koopa ligikaudset pikkust, sest siis saab selle taandada ekraani pikkuseks.

Kui näiteks koopa pikkus on 64 cm, saaks iga koopas läbitud cm peale kaardistada ühe punkti ekraanil. Koopa pikkus tegelikkuses võib aga olla suvaline number. Ülesande lahendamiseks on vaja teada, mitu sentimeetrit või meetrit koobast vastab ühele ekraanipunktile. See on vaja taandada ühikuteks, mitu kraadi kooparoboti mootori pöördest vastab puldiroboti ühele ekraanipunktile.

Käesoleva ülesande juures on teada kooparoboti ratta diameeter ja see tuleb teisendada ümbermõõduks.

clip_image008

Kuna roboti programm omab infot ainult selle kohta, mitu pööret mootor on teinud, siis tuleb teisendada teadaolev teepikkus ratta pööreteks.

clip_image010

Rattakraadid on suurus, mis väljendab läbitavat teepikkust kraadides valitud rataste korral. Seega, kui tegemist on näiteks ratastega, mille ümbermõõt on 10 cm, siis 1 m pikkuse koopa läbimiseks teevad rattad 10 tiiru ehk kraadides 3600 kraadi.

Kaardistamise sagedus

Eelnevast on teada number, mitu kraadi rattad pööravad kogu teepikkuse jooksul, ja ekraani kõrgus pikslites, seega saab välja arvutada kaardistamise sageduse.  Teisisõnu tähendab kaardistamise sagedus seda, mitme rattapöörde kraadi järel on tarvis kauguseanduritelt lugem võtta ja kaardile kanda.

clip_image012

Alljärgnevalt on näide selle kohta, kuidas lahendada kaardistamise sageduse ülesanne. Selles näites on eeldatud, et mootoripöörded saabuvad Bluetooth vastuvõtu kaudu muutujasse MootoriPoorded.

Muutujale perimeter omistatakse väärtus ratta ümbermõõt. Muutujale turns omistatakse väärtus koopa pikkus mootori pöörded kraadides. Muutujale MAPFrequent omistatakse kaardistamise sageduse väärtus.

Käivitub while-tsükkel tingimusega lõpetada siis, kui jõutakse koopa lõppu while(!KoopaFinish). Pärast seda omistatakse muutujale MotorB väärtus mootori pöörded kraadides MotorB = MootoriPoorded. Tingimusega MotorB > LastMotorB + MAPfrequent kontrollitakse kaardistamise vajadus. Kaardistamise hetkel salvestatakse MotorB väärtus muutujasse LastMotorB. Pärast seda jääb programm taas ootama seda, millal on MotorB väärtus kasvanud suuremaks kui eelmine salvestatud väärtus LastMotorB pluss kaardistamise sagedus.

NB! Järgnev programmikood on fragment suuremast programmist ning iseseisvalt see ei tööta.

Näide. Kaardistamise sageduse seadmine

const int Xekraan = 100;    //ekraani x-telje suurus

const int Yekraan = 64;     //ekraani y-telje suurus

const int teepikkus = 2000; //eeldatav teepikkus mm

const int wheel = 56;       //roboti ratta diameeter mm

 

task kaardista()

{

  //ratta ümbermõõt

  float perimeter = wheel * PI;

  //teepikkuse teisendamine ratta pööramise kraadideks

  float turns = teepikkus / perimeter * 360;

  //ratta pööramise kraadide teisendamine

  //kaardistuse sageduseks

  int MAPfrequent = turns / Yekraan;

 

  long LastMotorB;

  long MotorB;

 

  while (!KoopaFinish)

  {

    MotorB = MootoriPoorded;           

   

    //kaardistamine toimub iga ühiku MAPfrequent järel

    if(MotorB > LastMotorB + MAPfrequent)

    {

      LastMotorB = MotorB;

      PlayTone(TONE_A7, 1);

      //siit edasi on kaardistamise programmikood

    }

  }

}

Kauguseanduri teisendamine ekraanile arusaadavaks

Koopa kaardistamisel võtame eelduseks, et ekraani keskkohale vastab koopa telgjoon. Seega ekraani keskkohast paremale poole peab ilmuma parempoolse kauguseanduri näit (ja parempoolsed käigud) ning vasakule vasakpoolse anduri näit (ja vasakpoolsed käigud). Koopa kaardistamisel eristame ainult kahte pikkust – kas on tegemist koopa seinaga (nö. nullpikkus) või on tegemist käiguga (maksimaalne pikkus).

Kauguseandurist saabub signaal vahemikus 10…250, mis väljendab objekti kaugust andurist sentimeetrites.

Ekraani kogulaius on 100 pikslit, pool laiust on 50. Seega andurist saabuv signaal vahemikus 10..250 tuleb teisendada poole ekraani laiuseks, ehk siis 0..50.

Kõige lihtsam koopa kaardistamise algoritm oleks selline, kui kauguseandurist saadud signaal jagatakse viiega ning tulemus kuvatakse LineOut funktsiooniga ekraanile. Viiega jagamise mõttekus seisneb selles, et siis on teisendatud kauguseanduri maksimaalne näit ekraani maksimaalseks näiduks. Kirjeldatud moodus arvestab sujuva kauguse muutumisega ja siinkohal on ära toodud sellise kaardistaja programmi osa.

Lõplikus programmis kasutame kaardistamiseks täiustatud varianti, kus on elimineeritud kauguseandurist saabuvad vigased signaalid ehk niinimetatud signaali müra.

NB! Järgnev programmikood on fragment suuremast programmist ning iseseisvalt see ei tööta.

Näide. Kauguseanduri teisendamine ekraanile

while (TRUE)

{

  MotorB = MotorRotationCount(OUT_B);         

 

  //kaardistamine toimub iga ühiku MAPfrequent järel

  if(MotorB > LastMotorB + MAPfrequent)

  {

    LastMotorB = MotorB;

    PlayTone(TONE_A7, 1);

   

    //loetakse mõlemast kauguseandurist parameetrid

    sensorP = SensorUS(S3);

    sensorV = SensorUS(S4);

   

    //kauguseanduri näit jagatakse viiega, tulemuseks on näit

    //vahemikus 2...50, mis sobib suurepäraselt ekraani jaoks

    sensorP /= 5;

    sensorV /= 5;

   

    //kauguseanduritest saadud numbrid paigutatakse kas

    //ühele või teisele poole ekraani keskkoha suhtes

    xV=ekraanCenter - sensorV;

    xP=ekraanCenter + sensorP;

   

    //y on kasutusel y-telje arvestuse juures, ehk käigu kaugus

    y++;

   

    //ekraanil kuvatakse käikude asukohad

    LineOut(50, y, xP, y);

    LineOut(50, y, xV, y);

  }

}

Käesoleva projekti juures tasub eeldada, et kõik kõrvalkäigud on igal juhul pikad ning meil pole tarvis arvestada kauguseanduri vahepealsete näitudega. Kaardistamise tulemus peab olema seega piltlikult öeldes 1 või 0 (kas on kõrvalkäik või ei ole). Ülesande lahendamisel tuleb arvestada, et kauguseandur tekitab vigu ning iga väärtus ei väljenda veel kõrvalkäigu asukohta. Seega on vaja leida moodus vigade taandamiseks.

Vigade taandamist saab teha keskmise arvutamisega. Selle tulemusena peab arvestama, et kaardistamisel tekib teatav hilistus. Viide tuleneb sellest, et kaardile kuvatav näit muutub alles pärast näitude keskmistamist. Kui viimase kolme võetud näidu keskmine väljendab kõrvalkäigu asukohta, siis alles seejärel kuvatakse ekraanile joon kõrvalkäigu alguse kohta.

Alljärgnev lahendus arvestab sellega, et koopa kõrvalkäigud on lõpmatu pikkusega ja andurid on ebatäpsed ning tekitavad müra.

Müra elimineerimiseks luuakse kaks massiivi koopaErrorP[] ja koopaErrorV[], kuhu salvestatakse järjest anduritest saabuvad näidud. Need massiivid luuakse pikkusega kolm mälupesa ArrayInit(koopaErrorP, 0, idx) kusjuures konstant idx määrab massiivi pikkuse.

Andurist saadud info korrigeeritakse sellisel viisil, et kõik näidud, mis on üle 50 cm, nende asemel arvestatakse 50 ja kõik alla selle arvestatakse väärtusega null xV = sensorV > 50 ? 50 : 0. Toodud programmifragment on näide ternary tingimuse kasutamisest.

Järgmisena on programmikoodis muutuja i, mille väärtust suurendatakse iga tsükli käigus ühe ühiku võrra. Seejärel kontrollitakse, kas i on sama suur kui massiivi maksimaalne mälupesade arv i = i == idx ? 0 : i. Kui i on võrdne mälupesade arvuga, siis omistatakse i-le väärtus 0. Selle võtte tulemusena muutub i nullist kuni massiivi mälupesade arvuni idx, käesolevas näites 0,1,2,0,1,2 jne.

Vastavalt i väärtusele kirjutatakse massiivi väärtused järjest üle koopaErrorV[i]=xV. Seejärel arvutatakse antud massiivi keskmine xV=ArrayMean(koopaErrorV, NA, NA). Saadud tulemus on niisiis viimase kolme anduri näidu keskmine, kusjuures andurist tulevad näidud on 0 või 50. Seega keskmised saavad olla 0 {0,0,0}, 16 {0,0,50}, 33 {0,50,50}, 50 {50,50,50}.

Järgmise tegevusena kontrollitakse saadud keskmist näitu, kui see on väiksem kui 35, siis on x-telje väärtuseks 3, kui üle 35, siis on x-telje väärtuseks 50 xV=xV < 35 ? 3 : 50.

Saadud tulemus paigutatakse seejärel siis vastavalt andurile kas paremale või vasakule poole ekraanil.

Muutuja y tähistab y-telje koordinaati ning see võib olla maksimaalselt 64. Iga tsükli käigus liidetakse muutujale y üks juurde y++ ning seejärel võrreldakse selle suurust ekraani kõrgusega. Kui y on suurem kui ekraani kõrgus siis omistatakse y suuruseks taas 1 ning alustatakse kaardistamist uuesti ekraani altservast.

Näide. Kauguseandurist tuleva müra tasandamine

task kaardista()

{

  //ratta ümbermõõt

  float perimeter = wheel * PI;

  //teepikkuse teisendamine ratta pööramise kraadideks

  float turns = teepikkus / perimeeter * 360;

  //ratta pööramise kraadide teisendamine kaardistuse sageduseks

  int MAPfrequent = turns / Yekraan;

  //ekraani keskkoht

  int ekraanCenter = Xekraan /2;

 

  const char idx=3; //massiivi algväärtustamise konstant

  long LastMotorB;

  long MotorB;

  int xV, xP, y;

  int sensorP, sensorV;

  int i;

 

  //massiivide loomine, kuhu salvestatakse anduri näidud

  //millest võetakse keskmine, et välistada vigu

  int koopaErrorP[];

  int koopaErrorV[];

 

  //massiivide initsialiseerimine ja massiivi suuruse määramine,

  //mõistlik massiivi suurus võiks olla vahemikus 2..5

  ArrayInit(koopaErrorP, 0, idx);

  ArrayInit(koopaErrorV, 0, idx);

 

  while (!KoopaFinish)

  {

    MotorB = MootoriPoorded;           

   

    //kaardistamine toimub iga ühiku MAPfrequent järel

    if(MotorB > LastMotorB + MAPfrequent)

    {

      LastMotorB = MotorB;

      PlayTone(TONE_A7, 1);

     

      //y on kasutusel y-telje punkti arvestuse juures

      y++;

      if (y>Yekraan)

      {

        y=1;

        ClearScreen();

      }

     

      sensorP = sensorUSParem;

      sensorV = sensorUSVasak;

     

      //salvestame massiivi väärtused, mis jäävad

      //ekraani mõõtude piiridesse, ehk siis 0 või 50

      //kõik üle 50 väärtused arvestatakse kaardistamise suhtes

      //lõpmatuks kauguseks ehk võrdsustatakse 50-ga

      xV = sensorV > 50 ? 50 : 0;

      xP = sensorP > 50 ? 50 : 0;

     

      //i kasutatakse massiivi mälukohtade indeksina

      //idx on massiivi suurus,

      //seega massiiv täidetakse ringiratast

      i++;

      i = i == idx ? 0 : i;

     

      //salvestame x-telje numbrid massiivi

      koopaErrorV[i] = xV;

      koopaErrorP[i] = xP;

     

      //arvutame massiivi x-telje numbrite keskmise

      xV = ArrayMean(koopaErrorV, NA, NA);

      xP = ArrayMean(koopaErrorP, NA, NA);

     

      //korrigeerime ekraanil kuvatavat käikude süsteemi

      //viisil, et kuvatakse ainult väärtused koopa sein

      //(ekraani keskkoht, väärtus 3) või käik (ekraani serv)

      //ekraani keskel on lõpuks 6 piksli laiune joon, st. koobas

      xV = xV < 35 ? 3 : 50;

      xP = xP < 35 ? 3 : 50;

     

      //US anduritest saadud numbrid paigutatakse kas

      //ühele või teisele poole ekraani keskkoha suhtes

      xV=ekraanCenter - xV;

      xP=ekraanCenter + xP;

     

      //ekraanil kuvatakse käikude asukohad

      LineOut(50, y, xP, y);     

      LineOut(50, y, xV, y);     

    }

  }

}

Lõplik programm

Kooparobot

NB! Selle programmi jaoks on vajalik Bluetooth ühenduse loomise fail „BluetoothCheck.NXC“, mis on kirjeldatud käesoleva raamatu juhendis Bluetoothi alapeatükis.

Näide. Kooparoboti programm

//MASTER. Koopakaardistaja programm

#include "BluetoothCheck.nxc"

 

bool KoopaFinish = FALSE; //koopa lõpu määramiseks

 

const byte BTconn=1;

const string SlaveNXTName = "NXT2";

 

//alamprogramm saadaInfo saadab anduritest saabuva info vastuvõtjale

task saadaInfo()

{

  while(TRUE)

  {

    //loeme andurist andmed, mootori pöörded

    long MootoriPoorded = MotorRotationCount(OUT_B);

    //loeme anduritest andmed, parem-vasak kauguseandur

    int sensorP = SensorUS(S3);

    int sensorV = SensorUS(S4);

   

    //kuvame ekraanil info andurite olukorra kohta

    TextOut(0, LCD_LINE1,

        FormatNum("Mootor: %5d", MootoriPoorded));

    TextOut(0, LCD_LINE2,

        FormatNum("Parem: %5d", sensorP));

    TextOut(0, LCD_LINE3,

        FormatNum("Vasak: %5d", sensorV));

   

    //saadame mootori pöörete väärtuse

    SendRemoteNumber(BTconn, MAILBOX1, MootoriPoorded);

    Wait(10);

    //saadame parempoolse kauguseanduri info

    SendRemoteNumber(BTconn, MAILBOX2, sensorP);

    Wait(10);

    //saadame vasakpoolse kauguseanduri info

    SendRemoteNumber(BTconn, MAILBOX3, sensorV);

    Wait(10);

    //saadame koopa lõpuinfo,

    //pärast seda enam ei kaardistata

    SendRemoteBool(BTconn, MAILBOX4, KoopaFinish);

    Wait(10);

  }

}

//robot liigub koopa lõpuni, kuni puuteandur on vajutatud

//seejärel liigub tagasi,

//kuni mootori pöörded saavad taas nulli

task move()

{

  //antud tsükkel käib seni, kuni pole vajutatud nuppu

  while(!KoopaFinish)

  {

    //robot sõidab edasi

    OnFwd(OUT_BC, 30);

    //muutujale KoopaFinish omistatakse

    //puuteanduri väärtus

    KoopaFinish = Sensor(S1);    

  }

  //pärast puuteanduri vajutust käivitub järgmine tsükkel

  //mis käib seni, kuni mootori

  //pöörete arv on jõudnud 0 või alla selle

  while(MotorRotationCount(OUT_B)>0)

    OnRev(OUT_BC, 100);

 

  //robot jääb programmi lõppedes seisma

  Off(OUT_BC);

}

 

//Stardi ootamine on vajalik selleks,

//et sünkroniseerida NXT-de tegevus, kuna nende

//programmid võidakse käivitada erinevatel aegadel.

void StardiOotamine()

{

  bool ACK=FALSE;

  while(!ACK)

  {

    //saadetakse slave-le info, et "olen valmis" - TRUE

    SendRemoteBool(BTconn, MAILBOX6, TRUE);

    Wait(MS_10);

    //oodatakse slave NXT pealt stardikäsklust ACK=TRUE

    ReceiveRemoteBool(MAILBOX5, TRUE, ACK);

    TextOut(0, LCD_LINE1, "Ootan starti ...");

    Wait(MS_10);

  }

}

 

 

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

 

  SetSensorTouch(S1);

  SetSensorLowspeed(S3);

  SetSensorLowspeed(S4);

  ResetRotationCount(OUT_BC);

 

  StardiOotamine();

  ClearScreen();

  //alamprogramm move paneb roboti edasi liikuma

  StartTask(move);

  StartTask(saadaInfo);

}

Puldirobot

NB! Selle programmi jaoks on vajalik Bluetooth ühenduse loomise fail „BluetoothCheck.NXC“, mis on kirjeldatud käesoleva raamatu juhendi osas Bluetoothi alapeatükis.

Näide. Puldiroboti programm

//Puldiroboti programm

 

#include "BluetoothCheck.nxc"

 

const int Xekraan = 100;    //ekraani x-telje suurus

const int Yekraan = 64;     //ekraani y-telje suurus

const int teepikkus = 2000; //eeldatav teepikkus mm

const int wheel = 56;       //roboti ratta diameeter

 

long MootoriPoorded;

int sensorUSParem;

int sensorUSVasak;

bool KoopaFinish;

 

task kaardista()

{

  //ratta ümbermõõt

  float perimeter = wheel * PI;

  //teepikkuse teisendamine ratta pööramise kraadideks

  float turns = teepikkus / perimeter * 360;

  //ratta pööramise kraadide teisendamine

  //kaardistuse sageduseks

  int MAPfrequent = turns / Yekraan;

  //ekraani keskkoht

  int ekraanCenter = Xekraan /2;

  

  const char idx=3; //massiivi algväärtustamise konstant

  long LastMotorB;

  long MotorB;

  int xV, xP, y;

  int sensorP, sensorV;

  int i;

  //massiivide loomine, kuhu salvestatakse anduri näidud

  //millest võetakse keskmine, et välistada vigu

  int koopaErrorP[];

  int koopaErrorV[];

 

  //massiivide init ja massiivi suuruse määramine

  ArrayInit(koopaErrorP, 0, idx);

  ArrayInit(koopaErrorV, 0, idx);

 

  while (!KoopaFinish)

  {

    MotorB = MootoriPoorded;           

   

    //kaardistamine toimub iga ühiku MAPfrequent järel

    if(MotorB > LastMotorB + MAPfrequent)

    {

      LastMotorB = MotorB;

      PlayTone(TONE_A7, 1);

     

      //y on kasutusel y-telje punkti arvestuse juures

      y++;

      if (y>Yekraan)

      {

        y=1;

        ClearScreen();

      }

     

      sensorP = sensorUSParem;

      sensorV = sensorUSVasak;

     

      //salvestame massiivi väärtused, mis jäävad

      //ekraani mõõtude piiridesse, ehk siis 0 või 50

      //kõik üle 50 väärtused arvestatakse lõpmatuseks

      xV = sensorV > 50 ? 50 : 0;

      xP = sensorP > 50 ? 50 : 0;

     

      //i kasutatakse massiivi mälukohtade indeksina

      //idx on massiivi pikkus,

      //massiiv täidetakse ringiratast

      i++;

      i = i == idx ? 0 : i;

     

      //salvestame x-telje numbrid massiivi

      koopaErrorV[i] = xV;

      koopaErrorP[i] = xP;

     

      //arvutame massiivi x-telje numbrite keskmise

      xV = ArrayMean(koopaErrorV, NA, NA);

      xP = ArrayMean(koopaErrorP, NA, NA);

     

      //korrigeerime ekraanil kuvatavat käikude

      //süsteemi viisil, et kuvatakse ainult väärtused

      //koopa sein (ekraani keskkoht) või

      //käik (ekraani serv) ekraani keskkoha väljendab

      //arv 3, sellega tekitatakse ekraani keskele

      //6 punkti laiune joon, väljendades koobast

      xV = xV < 35 ? 3 : 50;

      xP = xP < 35 ? 3 : 50;

     

      //US anduritest saadud numbrid paigutatakse kas

      //ühele või teisele poole ekraani keskkoha suhtes

      xV=ekraanCenter - xV;

      xP=ekraanCenter + xP;

     

      //ekraanil kuvatakse käikude asukohad

      LineOut(50, y, xP, y);     

      LineOut(50, y, xV, y);     

    }

  }

}

 

//võetakse vastu Bluetooth ühenduse kaudu infot

task receiveBTsignal()

{

  while(TRUE)

  {

    ReceiveRemoteNumber (MAILBOX1, FALSE, MootoriPoorded);

    ReceiveRemoteNumber (MAILBOX2, FALSE, sensorUSParem);

    ReceiveRemoteNumber (MAILBOX3, FALSE, sensorUSVasak);

    ReceiveRemoteBool (MAILBOX4, FALSE, KoopaFinish);

  }

}

 

//stardiandmine on vajalik kahe NXT

//omavahelise tegevuse sünkroniseerimiseks

void StardiAndmine()

{

  bool ACK=FALSE;

  //käivitub tsükkel, millest väljutakse pärast seda,

  //kui slave robot on saanud master-i käest info,

  //et see on valmis

  while(!ACK)

  {

    //SLAVE ootab masteri käest infot

    ReceiveRemoteBool(MAILBOX6, TRUE, ACK);

    TextOut(0, LCD_LINE1, "Ootan roboti");

    TextOut(0, LCD_LINE2, "valmisolekut");

    Wait(MS_10);

  }

 

  TextOut(0, LCD_LINE1, "Anna start !");

  TextOut(0, LCD_LINE2, "Robot on valmis.");

 

  //oodatakse, kuni kasutaja vajutab nuppu

  while(!ButtonPressed(BTNCENTER, FALSE));

  while(ButtonPressed(BTNCENTER, FALSE));

  //SLAVE saadab MASTER NXT-le stardikäskluse

  SendResponseBool(MAILBOX5, TRUE);

}

 

task main()

{

  //käivitatakse BT ühenduse kontroll

  //kui ühendamine ei õnnestunud, väljutakse programmist

  if(!BluetoothCheck()==NO_ERR)

    Stop(TRUE);    

 

  StardiAndmine();

  ClearScreen();

 

  StartTask(receiveBTsignal);

  StartTask(kaardista);

}

 

Add comment

Loading