KURZ: Kompasový senzor

NXT-G

Blok pro ovládání kompasového senzoru

Rozcestník

• Umístění bloku
• Popis bloku
• Parametry bloku

---

Umístění bloku

Kompasový senzor je v programovacím prostředí NXT-G řízen blokem  Compass Sensor. Tento blok není součástí základní instalace. Pro jeho použití je nutné jej do prostředí importovat. Návod k importu rozšiřujících modulů naleznete v kapitole Instalace rozšiřujících modulů.

 

Popis bloku

Blok senzoru obsahuje osm konektorů, jejichž pomocí je možné jej propojit s dalšímy moduly.

 Port - vyjadřuje, ke kterému z portů (1-4) je kompasový senzor připojen.

 Action - udává, zda je zvoleno absolutní čtení, relativní čtení nebo kalibrace (Absolute Reading, Relative Reading, Calibration).

 Target - cílové natočení ve směru, kterého chceme dosáhnout (vyjádřeno ve stupních).

 A - číslo, které vyjadřuje dolní mez rozsahu hodnot určeného pro porovnání (nastavena v sekci Compare).

 B - číslo, které vyjadřuje horní mez rozsahu hodnot určeného pro porovnání (nastavena v sekci Compare).

 Yes / No - hodnota logického datového typu, která vyjadřuje, zda je hodnota v rozmezí mezi zadanými hodnotami  (Inside Range - výstup je roven Yes) nebo vně rozsahu zadaných hodnot (Outside Range - výstup je roven No).

 RelativeHeading - výstup vyjadřující relativní hodnotu natočení senzoru (rozdíl mezi cílovým směrem (Target) a absolutní hodnotou (Absolute Heading).

 Absolute Heading - výstup vyjadřující aktuální natočení senzoru (kterým směrem je momentálně natočen).

 

Parametry bloku

Port: určuje, ke kterému vstupnímu portu je kompasový senzor připojen.

Action: volba, která udává, jakým způsobem se bude s hodnotami pracovat. Absolute Reading pracuje s přesnými hodnotami od 0 do 360 stupňů. Aktuální natočení senzoru je určováno podle magnetického pólu Země.  Relative reading používá k určování pozice hodnoty od -180 do 180. Mód Relative Reading je častěji používán k řízení robota, protože jsou zde porovnávány relativní hodnoty s hodnotami snímanými senzorem, což je pro řízení jednodušší způsob než využití hodnot absolutních. Nejprve volíme aktuální pozici senzoru a následně zjišťujeme relativní přírůstky úhlu. Volba Calibrate označuje kalibraci senzoru.

Compare: volba sloužící k porovnání zadaných hodnot (rozsah hodnot při jejichž docílení je senzor aktivován). Volba Inside Range značí, zda je naměřená hodnota v rozmezí mezi zadanými hodnotami A a B. Outside Range má opačnou funkci. Vyjadřuje, zda je hodnota mimo zadané rozmezí. Target označuje cílovou pozici.

RobotC

PŘÍKAZY PRO OVLÁDÁNÍ kompasového senzoru

Rozcestník

• Nastavení senzoru
• Nastavení typu senzoru
• Práce s hodnotou snímanou senzorem
• Využití předdefinovaných metod

---

NASTAVENÍ SENZORU

Před započetím práce s kompasovým senzorem, je nutné jej v programovacím prostředí RobotC nadeklarovat. Zápis konfigurace se skládá z klíčového slova #pragma a příkazu config(), který obsahuje čtyři parametry. Prvním parametrem je rozlišeno, zda se jedná o deklaraci motoru či senzoru (v našem případě se jedná o senzor). Druhý parametr udává, ke kterému vstupnímu portu (S1-S4) je senzor připojen (v příkladu port číslo 1). Třetí parametr je volitelný. Jedná se o název senzoru v deklaraci a je zadáván uživatelem. Poslední částí deklarace je typ senzoru (sensorI2CHiTechnicCompass = označení kompasového senzoru).

 
#pragma config(Sensor, S1, kompas, sensorI2CHiTechnicCompass)
// Kod je automaticky generovan pruvodcem deklaraci RobotC.
 

---

NASTAVENÍ TYPU SENZORU

Výše popsaná možnost deklarace senzoru není v programovacím prostředí RobotC jediná. Existuje ještě pokročilejší způsob zápisu. Kompasový senzor patří do skupiny rozšiřujících senzorů definovaných ve skupině s názvem tSensors. První co tedy musíme učinit, je říci programu, že se jedná o senzor tohoto druhu (tSensors kompas). Slovo kompas je volitelný název senzoru pro potřeby našeho programu, který budeme dále používat. K zápisu můžeme přidat ještě označení portu, ke kterému je senzor připojen (S1-S4). Následně musíme zapsat, o který konkrétní rozšiřující senzor se jedná. Použijeme příkaz SensorType[]. Jeho parametrem v hranaté závorce bude námi zvolený název senzoru, tedy kompas. Do tohoto příkazu poté přiřadíme standardizované označení kompasového senzoru (sensorI2CHiTechnicCompass).

Tento typ deklarace je doporučeno provádět zkušenějším uživatelům. Začátečníkům se doporučuje deklarovat moduly pomocí Deklaraci motorů a senzorů v horní liště menu.

 
tSensors kompas = S1;
SensorType[kompas] = sensorI2CHiTechnicCompass;
 

 

---

PRÁCE S HODNOTOU SNÍMANOU SENZOREM

Při práci s každým senzorem budeme zpracovávat hodnotu, kterou nám senzor vrací. Tuto hodnotu můžeme dále porovnávat, ukládat do proměnné nebo s její pomocí ovlivňovat další chování robota. K vyjádření aktuálně zjišťované hodnoty ze senzoru se používá příkaz SensorValue[]. Jeho parametrem v závorce je uživatelem deklarovaný název senzoru. V příkladech o něco níže vidíte možné použití příkazu. Můžeme hodnotu jím vyjádřenou porovnávat s fixně zadanou hodnotou. Další možností je průběžné ukládání hodnoty do proměnné, s kterou budeme dále v programu pracovat a bude nám reprezentovat hodnotu vrácenou senzorem.

 
SensorValue[kompas] > 60; // Zjistovani, zda je hodnota zjistena senzorem vyssi nez 60.
 
a = SensorValue[kompas]; // Prirazeni hodnoty ze senzoru do promenne.
 

 

---

Využití předdefinovaných metod

Kompasový senzor nepatří mezi senzory obsažené v základní sadě stavebnice LEGO MINDSTORMS NXT. Při práci v programovacím prostředí RobotC máme k dispozici vývojové knihovny, které obsahují ovladače, ve kterých jsou předdefinovány metody pro práci s rozšiřujícími moduly a senzory. Knihovna pro práci s kompasovým senzorem se jmenuje hitechnic-compass a zavoláme ji v úvodu programu následovně:

 
#include "drivers/hitechnic-compass.h"
 

 

Před započetím práce s kompasovým senzorem je možné, nikoliv ovšem nezbytné senzor zkalibrovat. Postupem času může dojít k určitému ovlivnění z vnějšku, díky působení magnetického pole, nebo určitých točivých zařízení (podrobný popis kalibrace najdete ZDE). Předdefinovaná funkce, která spustí kalibraci se nazývá HTMCstartCal(). Po spuštění kalibrace by se měl senzor otočit o více než 360° podél horizontální roviny po dobu alespoň 20 vteřin. Kalibraci následně zastavíme zavoláním funkce HTMCstopCal(), díky čemuž se senzor znovu přepne do režimu snímání.

Po správném zkalibrování senzoru můžeme přejít ke zpracovávání hodnot, které s jeho pomocí zjišťujeme. První údaj, který dokážeme kompasovým senzorem zjistit, je aktuální natočení podle magnetického pólu Země (Absolute Heading). Funkce, která nám tuto hodnotu vrací se nazývá HTMCreadHeading(). Zjištěná hodnota je datového typu integer.

Druhou možností prezentace dat z kompasového senzoru je pomocí vyjádření relativní hodnoty natočení senzoru. Tato hodnota se vypočte jako rozdíl mezi požadovaným směrem, kterého chceme dosáhnout, a absolutní hodnotou natočení senzoru (Absolute Heading). Vypočtenou hodnotu nám vrací předdefinovaná funkce HTMCreadRelativeHeading(), jejíž výsledná hodnota, která může být v rozsahu od -179 do 180, je datového typu integer. Při využití této funkce budeme potřebovat ještě stanovit, kde se na zmíněném rozmezí nachází nulový bod. K tomu slouží funkce HTMCsetTarget().

U všech funkcí zadáváme pouze jediný parametr umístěný v závorce. Jedná se o volitelný název senzoru, který uživatel zadává při deklaraci.

NXT-G

VYKRESLENÍ KOMPASU

Realizace programu v programovacím prostředí NXT-G může být poněkud zdlouhavější. Při vytváření programu musíme vytvořit programový konstrukt, který dynamicky vypočítává novou pozici ručičky kompasu. K tomu musíme použít několik bloků, díky čemuž se program stane rozsáhlejší. Začínajícím programátorům tak může orientace v něm činit potíže.

Při vytváření programu musíme zrealizovat několik kroků. Nejprve si rozmyslet, jaké proměnné budeme potřebovat pro ukládání hodnot. Následně rozhodnout, jak pomocí hodnoty zjištěné senzorem vypočítat novou pozici ručičky kompasu. Poslední částí bude správné vykreslení celého kompasu a také ručičky.

Krok 1 - Deklarace proměnných

V programu budeme zpracovávat několik hodnot, které si musíme uložit do příslušných proměnných. Před započetím programování si je tedy nadeklarujeme. Budeme potřebovat dvě proměnné, které budou reprezentovat výchozí pozici pro vykreslování ručičky kompasu. Jedna bude vyjadřovat souřadnici X a druhá souřadnici Y bodu displeje. Pojmenujeme si je poslX a poslY. Obě budou datového typu Number. Poslední dvojicí proměnných budou ty, které budou symbolizovat dynamicky se měnící pozici ručičky kompasu. Tento bod bude při vykreslení propojen přímkou spojující osu ručičky kompasu a právě tuto pozici. Proměnné si pojmenujeme X a Y a budou datového typu Number.

 

Krok 2 - Úvodní ošetření programu

Než přejdeme ke zpracování hodnoty snímané kompasovým senzorem, musíme určit, na jakém bodě displeje bude osa ručičky kompasu. Tento bod určují proměnné poslX a poslY. Do programu tedy jako první vložíme dva bloky Variable, jimž nastavíme příslušnou hodnotu. Pro potřeby našeho programu jsme souřadnice osy ručičky kompasu zvolili na pozici (30,30). Na úvod programu je také dobré smazat plochu displeje, než na něj začneme kompas vykreslovat. Použijeme proto blok  Display a v sekci Action zvolíme Reset.

Ošetřili jsme úvod programu. Všechny další kroky, které budeme realizovat, se budou opakovat, dokud uživatel program neukončí. Pro opakování použijeme blok  Loop, u nějž zvolíme nekonečné opakování (Forever). Všechny další bloky budou nyní umístěny v něm.

 

Krok 3 - Výpočet pozice ručičky

Nyní již přejdeme ke zpracování hodnot ze senzoru. Použijeme blok  Compass Senzor a nastavíme snímání v režimu Absolute Reading. Z hodnoty zjištěné senzorem nyní vypočítáme dynamicky se měnící pozici ručičky kompasu. Ta se vypočte následujícím způsobem. Souřadnici X získáme tak, že vypočteme sinus hodnoty ze senzoru, výsledek vynásobíme 30 a následně k němu přičteme 30, což je posun po ose X. Souřadnici Y získáme tak, že vypočteme cosinus hodnoty ze senzoru, výsledek vynásobíme 30 a následně k němu přičteme 30 (posun po ose Y).

V programovacím prostředí výpočet zrealizujeme pomocí následujících bloků. Výstupní hodnotu ze senzoru ( Absolute Heading) přivedeme na vstup bloku  HiTechnic Sin/Cos. Výstup Sin (pro souřadnici X) nebo Cos (pro souřanici Y) poté přivedeme na vstup bloku  Math, kde provedeme násobení (Multiplication) 30. Výsledek přivedeme na vstup dalšího bloku  Math, kde k hodnotě přičteme (Addition) 30. Vypočtenou hodnotu poté uložíme do proměnné X (pro souřadnici X) a Y (pro souřadnici Y).

 

Krok 4 - Vykreslení ručičky a samotného kompasu

Než přejdeme k samotnému vykreslování kompasu, musíme si uvědomit, kdy vlastně chceme, aby se na displeji zobrazila ručička měnící pozici. Bude to v případě, kdy pozice natočení kompasu bude jiná než výchozí pozice ručičky. Musíme proto tuto situaci programově ošetřit. Porovnáme, zda hodnota uložená v proměnné X není náhodou rovna hodnotě v proměnné poslX nebo zda hodnota v proměnné Y není rovna poslY. Pro porovnání dvou proměnných použijeme blok  Compare. Logický součet dvou výroků provedeme pomocí bloku   Logic, v jehož nastavení zvolíme operaci OR (logický součet).

Pokud bude splněna popsaná podmínka, může začít vykreslování kompasu. V opačném případě se nestane nic. Pro rozlišení těchto dvou stavů použijeme blok podmíněného vykonání Switch. Ten bude řízen právě splněním podmínky nerovnosti proměnných. V sekci True se proto bude provádět vykreslování a v sekci False nebude umístěn žádný blok.

Pro vykreslování jednotlivých součástí kompasu budeme používat blok  Display. Nejprve si vykreslíme kružnici reprezentující kompas. Zvolíme akci vykreslování (Drawing) a typ objektu kruh (Circle). Jak jsme zmínili v úvodu, osu ručičky jsme si umístili na pozici (30,30). Střed kružnice proto umístíme na totožnou pozici a jako poloměr nastavíme 30 bodů. Přejdeme k vykreslení písmen popisujících světové strany. Musíme dbát na to, abychom je umístili na displeji symetricky. V sekci Action zvolíme pro vykreslování písmen volbu Text. V programu jsme popisky kompasu umístili na následující pozice:

• S - (29,55)
• J - (29,8)
• Z - (3,30)
• V - (54,30)

Zbývá provést dynamické vykreslování ručičky kompasu. Použijeme dva bloky  Variable, které budou reprezentovat proměnné X a Y, ve kterých máme uloženu vypočtenou pozici ukazatele ručičky. Výstupy těchto proměnných následně přivedeme na vstupní porty End point X a End point Y bloku  Display. U bloku musí být nastaveno vykreslování křivky (Line) a souřadnice počátečního bodu křivky (X a Y) musí být nastaveny na pozici (30,30).

Pro zajištění dynamického vykreslování umístíme ještě za bloky pro vykreslení blok  Wait, kterému nastavíme dobu pro oddálení vykonávání dalšího příkazu na 0,2 sekundy. Nesmíme také zapomenout zaškrtnout u prvního bloku  Display, který realizuje vykreslování možnost Clear. Jinak by při každém dalším vykreslení ručička kompasu nezmizela a zůstávala by na displeji zobrazena každá její předchozí pozice. U všech ostatních bloků bude tato možnost neaktivní.

 

Výsledný program

Co do počtu bloků, je program poněkud rozsáhlejší. Po pečlivém rozvrhnutí a realizaci jednotlivých kroků byste ovšem měli dojít k úspěšnému řešení.

 

Program ke stažení

Stažení ve formátu .rbt

RobotC

Vykreslení kompasu

Realizace příkladu v programovacím prostředí RobotC se skládá z úspěšného provedení několika kroků. Základem je správná deklarace senzoru a promyšlení, jaká vlastně bude výsledná podoba kompasu a z kterých kroků se jeho vykreslení skládá. Pro dobrý vizuální efekt musíme symetricky na displej vykreslit kružnici reprezentující displej a jednotlivé světové strany. Nejobtížnější částí programu je beze sporu dynamický výpočet pozice ručičky kompasu.

Krok 1 - Deklarace senzoru

Než začneme s realizací programu, musíme si deklarovat kompasový senzor, jinak bychom jej nemohli používat. Zápis deklarace vidíte níže. Skládá se z klíčového slova #pragma a příkazu config(). Ten obsahuje čtyři parametry. Označení, že se jedná o senzor (Sensor), port jeho připojení (S1), volitelný název senzoru a standardizované označení senzoru (sensorI2CHiTechnicCompass).

 
#pragma config(Sensor, S1, kompas, sensorI2CHiTechnicCompass)
 

 

Krok 2 - Deklarace proměnných

Během chodu programu se hodnota udávající natočení kompasového senzoru musí někam ukládat. Nadeklarovali jsme si proto proměnnou natoceni, která bude sloužit právě k tomuto účelu. Dále musíme také určit, na jaké pozici displeje bude osa ručičky kompasu. Zvolili jsme pozici (30,30). Tato pozice bude také výchozím bodem pro první vykreslování ručičky. Vytvořili jsme si dvě proměnné (poslX a poslY), které definují tuto pozici a jejichž hodnota je 30.

 
 int natoceni;
 
 int poslX = 30;
 int poslY = 30;
 

 

Krok 3 - Vykreslení kompasu a světových stran

V dalším kroku si na plochu displeje vykreslíme kružnici, která bude reprezentovat kompas a následně také označení světových stran. Střed kružnice bude v bodě (30,30) a poloměr kružnice bude také 30 bodů displeje. Pomocí příkazu nxtDrawEllipse() tedy vykreslíme kružnici, jejíž levý horní roh bude na pozici (0,60) a pravý dolní roh na pozici (60,0).

Nyní přejdeme k vykreslení popisků světový stran. Použijeme příkaz nxtDisplayStringAt(), který umožňuje vykreslit znak nebo řetězec znaků na požadovanou pozici. Umístění popisků může být pro někoho náročné. Pro výsledný dobrý vizuální efekt je důležité popisky umístit na pozice tak, aby byly symetrické. Pozice jednotlivých písmen označujících světové strany, které jsme zvolily, vidíte níže. Ještě před vykreslením kružnice a světových stran je dobré displej smazat příkazem eraseDisplay().

 
 eraseDisplay();
 
 nxtDrawEllipse(0, 60, 60, 0);
 
 nxtDisplayStringAt(29, 58, "S");
 nxtDisplayStringAt(29, 9, "J");
 nxtDisplayStringAt( 3, 34, "Z");
 nxtDisplayStringAt(54, 34, "V");
 

 

Krok 4 - Výpočet pozice ručičky

Dostáváme se k části, kde musíme určit pozici, na jakou se bude ručička natáčet. Jelikož od tohoto kroku se budou všechny příkazy provádět neustále dokola a pozice ručičky se bude dynamicky měnit, musíme všechny příkazy začít zapisovat do nekonečného cyklu while(). Nejprve si nadeklarujeme dvě proměnné, které budou reprezentovat pozici X a Y bodu, který se propojí se středem ciferníku kompasu a vykreslí se tak ručička. Obě budou datového typu integer. Následně si do proměnné natoceni přiřadíme hodnotu z kompasového senzoru pomocí příkazu SensorValue().

Nyní přicházíme k tomu, jak vypočítat pozici proměnlivého bodu. Pro výpočet pozice na ose X vezmeme hodnotu 30, která značí posun po ose X, přičteme k ní sinus hodnoty uložené v proměnné natoceni vynásobený 30. Pozici na ose Y získáme tak, že k posunu na ose Y (30) přičteme cosinus hodnoty uložené v proměnné natoceni vynásobený 30.

 
 int X;
 int Y;
 
 natoceni = SensorValue[kompas];
 
 // vypocet pozice rucicky kompasu
 X = 30 + (sinDegrees(natoceni) * 30);
 Y = 30 + (cosDegrees(natoceni) * 30);
 

 

Krok 5 - Vykreslování pozice ručičky

Posledním krokem při vytváření programu je vykreslování ručičky kompasu. Budeme chtít, aby se ručička vykreslila pouze v případě, že senzor bude snímat, což je případ, kdy hodnota proměnných X a Y bude jiná, než výchozí pozice (poslX a poslY). Použijeme podmínku if(), jejíž podmínka pro vykonávání dalších příkazů bude, že poslední pozice X (poslX) se nebude rovnat X a poslední pozice Y (poslY) se nebude rovnat Y - (poslX != X) || (poslY != Y). Pokud bude podmínka splněna, vykreslí se ručička kompasu. Vykreslení dosáhneme pomocí příkazu nxtInvertLine(), který pomocí vykreslení přímky propojí dva zadané body na displeji. V našem případě se bude jednat o střed ciferníku, tedy bod (30,30) a vypočtenou pozici ukazatele ručičky (X,Y).

Neustálým vykreslováním nové pozice ručičky, v závislosti na natáčení kompasového senzoru, by se na displeji zobrazovaly nové a nové přímky. Předchozí pozice ručičky by se ovšem nemazaly. Smazání předchozí pozice dosáhneme opětovným použitím příkazu nxtInvertLine(). Jelikož před prvním vykreslováním byla ručička v pozici středu ciferníku, smažeme přímku propojující body (30,30) a (poslX,poslY). Hodnota proměnných reprezentujícíh poslední umístění je ovšem v současné době 30. Musíme proto ještě do těchto proměnných přiřadit souřadnice posledního bodu, který byl při vykreslení propojen s osou ciferníku. Do proměnné poslX tedy přiřadíme hodnotu proměné X a do poslY hodnotu proměnné Y. Pro zajištění dynamického vykreslování je dobré jestě před přiřazení do proměnných umístit příkaz wait1Msec(), který oddálí vykonávání dalšího příkazu. Jako jeho hodnotu jsme zvolili 0,2 vteřiny, což se ukázalo jako dostačující.

 
 if ((poslX != X) || (poslY != Y))
 {
  // vykresluje novou pozici ukazatele od (31,31) do (X,Y)
  nxtInvertLine(30, 30, X, Y);
 
  // prekresli predchozi pozici
  nxtInvertLine(30, 30, poslX, poslY);
  wait1Msec(200);
  poslX = X;
  poslY = Y;
 }
 

 

 

Výsledný program

Vytvoření tohoto programu může začátečníkům činit obtíže. Jedná se spíše o pokročilejší úlohu. Pokud ovšem správně zrealizujete jednotlivé kroky a budete si počínat pečlivě, dojdete ke zdárnému výsledku. Hotový program by poté měl vypadat následovně:

 
// deklarace kompasoveho senzoru
#pragma config(Sensor, S1, kompas, sensorI2CHiTechnicCompass)
 
task main()
{
  int natoceni;
 
  // pozice osy rucicky
  int poslX = 30;
  int poslY = 30;
 
  eraseDisplay();
 
  // vykresleni kompasu a svetovych stran
  nxtDrawEllipse(0, 60, 60, 0);
  nxtDisplayStringAt(29, 58, "S");
  nxtDisplayStringAt(29, 9, "J");
  nxtDisplayStringAt( 3, 34, "Z");
  nxtDisplayStringAt(54, 34, "V");
 
  while (true)
  {
    int X;
    int Y;
 
    natoceni = SensorValue[kompas];
 
    // vypocet pozice rucicky kompasu
    X = 30 + (sinDegrees(natoceni) * 30);
    Y = 30 + (cosDegrees(natoceni) * 30);
 
    if ((poslX != X) || (poslY != Y))
    {
      // vykresluje novou pozici ukazatele od (31,31) do (X,Y)
      nxtInvertLine(30, 30, X, Y);
 
     // prekresli predchozi pozici
     nxtInvertLine(30, 30, poslX, poslY);
     wait1Msec(200);
     poslX = X;
     poslY = Y;
   }
  }
}
 

 

 

Program ke stažení

Stažení ve formátu .c