BricxCC
| www: | |
.... | |
BrixCC – textový programovací nástroj pro vývoj robotických aplikací s LEGO roboty RXC a NXT.
Postup: 1. Stáhněte a nainstalujte program ze stránek http://bricxcc.sourceforge.net/
2. Nainstalujte nebo nakopírujte program do složky: C:\Program Files (x86)\
3. Program spusťte. V okně nastavte vyhledání a připojení NXT. Zvolte port, který chcete využít. Nabízí se USB nebo BlueTooth. Firmware nechte na Standard.
4. Následně v novém okně můžete psát program.
5. Každý program bude na začátku vypadat následovně.
Ukázky programů pro zobrazování údajů:
Výpis textu na LCD
Zadání: Napište program tak, aby došlo k výpisu textu (i čísel) na LCD displeji robota NXT. Text musí být bez diakritiky.
Funkce: Po úvodní inicializaci a skoku do nekonečné smyčky, se vypíše text „Nahodna cisla“ a vygeneruje se náhodné číslo. Těchto 8 náhodných čísel se postupně zobrazí vedle sebe. Mezi jednotlivým generováním je zpoždění 300 ms.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
int RandomValue; //náhodné číslo
int X = 10; //poloha
while(true) //nekonečná smyčka
{
TextOut (10, LCD_LINE3, "Nahodna cisla"); //výpis textu na display
RandomValue = Random(10); //vygenerování náhodného čísla od 0 do 9
NumOut (X, LCD_LINE5, RandomValue); //výpis hodnoty na display
X += 10; //inkrementace X o 10
if(X > 80) X = 10; //kontrola překročení hodnoty
Wait(300); //spoždění o 300 milisekund
}
} //konec programu
Zobrazení grafiky
Zadání: Vyzkoušejte program, který řeší zobrazení grafických obrazců, pomocí kterých se zobrazuje stav baterie robota
Funkce: Načítaná hodnota, která je v mV, se vypíše a zároveň přes vzorec převede na procentuelní úroveň a ta se poté vykreslí. Navíc při poklesu napětí pod 6V se ohlásí stav nízké úrovně baterie.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
int BatLvl; //hodnota baterie
int BatPos; //pozice stavu baterie
int PercentBat; //procentuelní stav baterie
while(true) //nekonečná smyčka
{
BatLvl = BatteryLevel (); //stav baterie v mV
BatPos = ((BatLvl * 78) / 8500); //vypočítání polohy na grafu stavu baterie
PercentBat = (BatLvl / 85);
RectOut (10, 25, 80, 22, true); //vykreslení grafu
RectOut (11, 26, BatPos, 20); //vykreslení stavu baterie do grafu
if(BatLvl < 6000) TextOut(10, LCD_LINE1, "Low Batery"); //oznámení nízkého stavu baterie
NumOut(10, LCD_LINE2, BatLvl); //vypsání hodnoty v mV
TextOut (36, LCD_LINE2, "mV");
NumOut(58, LCD_LINE2, PercentBat); //vypsání hodnoty v %
TextOut (75, LCD_LINE2, "%");
Wait(10); //spoždění 10ms (kvůli problikávajícímu jevu při mazání)
}
} //konec programu
Ukázky programů pro práci se senzory:
Touch to LCD
Zadání: Naprogramujte robota tak, aby při stisknutí senzoru doteku zobrazoval na LCD displeji Stisknuto/Rozepnuto.
Funkce: Na LCD displeji je zobrazována hodnota 0=Rozepnuto. Při sepnutí tlačítka se zobrazí hodnota 1=Stisknuto.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorTouch (IN_1); //zapnutí tlačítkového sensoru na portu 1
//SetSensorMode (IN_1, SENSOR_MODE_PULSE); //zapne mod pro čítač - reaguje jen na sestupnou hranu (vypínaní)
//SetSensorMode (IN_1, SENSOR_MODE_EDGE); //zapne mod pro čítač - reaguje na vzestupnou i sestupnou hranu
int Touch; //proměnná pro uložení stavu tlačítka
while(true) //nekonečná smyčka
{
Touch = Sensor (IN_1); //zjištění stavu
NumOut (10, LCD_LINE2, Touch); //vypsání hodnoty na display
if(Touch == true) //pokud je stisknuto tlačítko
{
TextOut (10, LCD_LINE3, "Stisknuto"); //vypsání stavu
}
else
{
TextOut (10, LCD_LINE3, "Rozepnuto");
}
}
} //konec programu
Sound to LCD
Zadání: Naprogramujte robota tak, aby zobrazoval na LCD displeji hlasitost okolního ruchu.
Funkce: Robot pomocí senzoru hluku snímá hlasitost okolí a tu poté zobrazuje na LCD displeji.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorSound (IN_2); //zapnutí tlačítkového senzoru na portu 1
int Sound; //proměná pro uložení hodnoty světla
int SoundPos; //poloha ekvalizéru
while(true) //nekonečná smyčka
{
Sound = Sensor (IN_2); //stav baterie
SoundPos = (((Sound * 76) / 100)); //vypočítání polohy ekvalizéru
RectOut (12,27, SoundPos, 1, true); //vykreslení ekvalizéru
RectOut (10, 25, 80, 5); //vykreslení grafu
NumOut(10, LCD_LINE2, Sound); //vypsání hodnoty
Wait(1); //spoždění 1ms
}
} //konec programu
Light to LCD
Zadání: Naprogramujte robota tak, aby pomocí senzoru světla zobrazoval na LCD dipleji hodnotu intenzity světla.
Funkce: Na LCD displeji je zobrazována intenzita světla která se nachází pod senzorem světla.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorTouch (IN_1); //zapnutí tlačítka
SetSensorLight (IN_3); //zapnutí světelného senzoru
SetSensorMode (IN_3, IN_MODE_RAW); //přepnutí módu na rolišení 0 až 1023
int Light; //proměná pro uložení hodnoty světla
bool Touch; //proměná pro uložení stavu tlačítka
while(true) //nekonečná smyčka
{
Light = Sensor (IN_3); //načtení intenzity světla
Touch = Sensor (IN_1); //zjistí stav tlačítka
NumOut (10, LCD_LINE2, Light, true); //výpis hodnoty
if(Touch == true)
{
SetSensorType (IN_3, IN_TYPE_LIGHT_INACTIVE); //vypnutí LED u světelného senzoru
}
else
{
SetSensorType (IN_3, IN_TYPE_LIGHT_ACTIVE); //zapnutí LED u světelného senzoru
}
}
} //konec programu
Sonar
Zadání: Sonar
Funkce: Ultrazvukové čidlo, které funguje na bázi sonaru, vysílá řadu pulzů s frekvencí okolo 40kHz a čeká kdy se mu tyto impulsy vrátí (od odraženého předmětu). Na základě rychlosti zvuku poté vypočítá vzdálenost a vrátí její hodnotu v centimetrech. Sonar v tomto programu slouží k nastavení frekvence. Tlačítko zase k nastavení výšky tónu. Tlačítko je nastaveno na čítač a s neustálím mačkáním roste výška výsledného tónu. Hodnota sonaru zase mění tón okolo výsledné hodnoty tlačítka. Tóny jsou v reálném čase přehrávány reproduktorem.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorTouch (IN_1); //zapnutí tlačítka
SetSensorLowspeed (IN_4); //zapnutí sonaru
SetSensorMode (IN_1, IN_MODE_PERIODCOUNTER); //přepnutí módu na čítač
int Tone; //tón
int Frequency; //frekvence
int Distance; //vzdálenost
int Touch; //stav tlačítka
while(true) //nekonečná smyčka
{
//Distance = SensorUS (IN_4); //načte vzdálenost ze sonaru v cm
Touch = Sensor (IN_1); //zjistí stav tlačítka
Distance = 1;
if(Touch > 10) ClearSensor (IN_1); //zajištění přetečení
Tone = Touch * 100; //výpočet tónu
Frequency = Tone + Distance; //výpočet frekvence
PlayTone(Frequency, 0); //zahrání zvuku
TextOut(10, LCD_LINE2, "Ton:", true); //vypsání hodnot
TextOut(10, LCD_LINE4, "Frekvence:");
NumOut (50, LCD_LINE2, Tone);
NumOut (50, LCD_LINE4, Frequency);
}
} //konec programu
Enkodér
Zadání: Enkodér
Funkce: V tomto programu je použit enkodér z motoru B a oba jeho čítače. Jeden z nich můžeme resetovat. Zobrazují se potom obě hodnoty z obou enkodéru. Zároveň je toto důkaz, že enkodéry jsou relativní a nepočítají absolutní natočení kola.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorTouch (IN_1); //zapnutí tlačítkového senzoru na portu 1
int NXTCounter; //proměné pro uložení hodnot z čítačů
int ServoCounter;
bool Touch; //uložení stavu tlačítka
while(true) //nekonečná smyčka
{
Touch = Sensor (IN_1); //zjištění stavu tlačítka
NXTCounter = MotorRotationCount (OUT_B);//zjískání natočení z čítače v NXT
ServoCounter = MotorTachoCount (OUT_B); //zjískání natočení z čítače serva
if(Touch == true) ResetRotationCount (OUT_B); //vynulování čítače v NXT
TextOut (10, LCD_LINE3, "NXT citac:", true); //zobrazení textu
TextOut (10, LCD_LINE4, "Servo citac:");
NumOut (82, LCD_LINE3, NXTCounter); //zobrazení hodnot
NumOut (82, LCD_LINE4, ServoCounter);
Wait (1); //spoždění 1ms
}
} //konec programu
Joystick
Zadání: Joystick
Funkce: Demonstrační program pro vyrobený joystic. Program čte hodnotu joystiku pro všechny směry a zobrazuje kurzor na vypočítané pozici.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
int MiddleY = (Sensor (IN_1) / 3); //určení Y středu
int MiddleX = (Sensor (IN_2) / 3); //určení X středu
int PositionX = 0; //X pozice kurzoru
int PositionY = 0; //Y pozice kurzoru
while(true) //nekonečná smyčka
{
PositionY = (MiddleY - (Sensor (IN_1) / 3)) + 32; //vypočítání pozice
PositionX = (MiddleX - (Sensor (IN_2) / 3)) + 50;
ClearScreen (); //smazání obrazovky
CircleOut (PositionX, PositionY, 2); //vykreslení kurzoru
Wait(1); //čekání 1ms pro odstranění blikání
}
} //konec programu
Ukázky programů pro rozpohybování robota:
Jízda=
Zadání: Jízda
Funkce: Program znázorňuje možnosti řízení, brzdění a synchronizaci motorů. Postupně program projíždí řízení bez regulace s brzdou, dále jízdu s regulací napětí na motoru bez brzdy a poslední krok je regulace otáček a také bez brzdy.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
while(true) //nekonečná smyčka
{
//jizda bet regulace
//s brzdou
TextOut(1, LCD_LINE3, "Bez regulace", true);
TextOut(1, LCD_LINE5, "Brzda");
OnFwd (OUT_BC, 60);
Wait(1000);
Off (OUT_BC); //elektronická brzda
Wait(1000);
//jizda s regulaci napeti
//bez brzdy
TextOut(1, LCD_LINE3, "Regulace napeti", true);
TextOut(1, LCD_LINE5, "Bez brzdy");
OnFwdReg (OUT_BC, 60, OUT_REGMODE_SPEED);
Wait(1000);
Coast (OUT_BC); //pozvolné zastavení
Wait(1000);
//jizda s regulaci otacek
//bez brzdy
TextOut(1, LCD_LINE3, "Regulace otacek", true);
TextOut(1, LCD_LINE5, "Bez brzdy");
OnFwdReg (OUT_BC, 60, OUT_REGMODE_SYNC);
Wait(1000);
Coast (OUT_BC); //pozvolné zastavení
Wait(1000);
}
} //konec programu
Rotace
Zadání: Rotace
Funkce: U tohoto programu jsou ukázány způsoby zatáčení s využitím zpětné vazby i bez ní. První část ukazuje natáčení motoru podle informace z enkodéru. Pozor! Zadané natočení není rotace robota, ale kola! Nutno proto dopočítat podle následujícího vztahu (za předpokladu, že jedno kolo stojí):
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
while(true) //nekonečná smyčka
{
RotateMotor (OUT_C, 50, 360); //rotování s kontrolou podle enkodéru
Wait(1000); //čekání 1s
OnFwd (OUT_C, 50); //rotování jen na základě času
Wait(1000); //-> rotování 1s
Off(OUT_C); //zastavení motoru
Wait(1000); //čekání 1s
}
} //konec programu
RotSEnkod
Zadání: RotSEnkod
Funkce: Tento program spojuje senzor s efektorem. Senzor je enkodér, který čte hodnotu natočení jednoho motoru a následně ji přenáší na druhý motor. Tím dojde k jevu, že robot neustále drží stejný směr. Tak jak pohnu jedním kolem, tak robot pohne druhým.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
int Rotation; //proměná pro uložení natočení
while(true) //nekonečná smyčka
{
Rotation = MotorTachoCount (OUT_B); //zjištění natočení motoru B
NumOut (20, LCD_LINE4, Rotation, true);//vypsání
RotateMotor (OUT_C, 50, Rotation); //rotace motoru C stejně jako byl natočen motor B
ResetTachoCount (OUT_B); //vynulování natočení
}
} //konec programu
Ukázky složitějších programů:
Balanc
Zadání: Balanc
Funkce: Tato skupina je nejtěžší a nejsložitější, a to buď délkou programu nebo postupem, který řeší daný problém. To je hned první případ Balancovníku, který je programově jednoduchý, ale kvůli matematické části se stává velmi složitým. Balancovník využívá plný PID regulátor, aby dosáhl rovnováhy. Snímá se intenzita světla, která s padajícím robotem buď stoupá nebo klesá, podle strany na kterou robot padá. Aby robot nespadl musí zabrat motorem na příslušnou stranu a tím dorovnat pád. To jak moc má motor zabrat právě řeší proporcionálně-integračně-derivační regulátor. Tento regulátor je zde implementován softwarovou formou a proto potřebuje 3 regulační konstanty. Tyto hodnoty jsou váhami pro jednotlivé členy PID regulátoru. Jak PID vlastně pracuje? Proporcionální člen zesiluje odchylku od požadované hodnoty (taková hodnota, při které je robot dokonale vyvážený). Integrační složka nesčítává chybu a tím zajišťuje zrychlování motoru. Derivační složka zase naopak snižuje zrychlení motoru, a to tak, že čím více se robot blíží do rovnovážného stavu, tím více motor zpomaluje. Tím jsou rušeny překmity.
Ukázka programu:
//vložení hlavičkového souboru
#include "NXCDefs.h"
//váhy jsou ve skutečnosti 100x menší (dále jsou děleny 100x)
#define Kp 310 //váha proporcionální složky
#define Ki 4 //váha integrační složky
#define Kd 800 //váha derivační složky
task main() //hlavní funkce
{
SetSensorLight (IN_3); //zapnutí světelného senzoru
SetSensorTouch (IN_1);
SetSensorMode (IN_3, IN_MODE_RAW); //přepnutí módu na rozlišení 0 až 1023
int Light; //proměná pro uložení hodnoty světla
int Touch; //indikace stisku tlačítka
int Turn; //natočení motoru
int Offset = 545; //klidová poloha (střed)
int Error = 0; //odchylka
int LastError = 0; //minulá odchylka
int Integral = 0; //integrace
int Derivate = 0; //derivace
while(!Touch)
{
Light = Sensor (IN_3);
Touch = Sensor (IN_1);
Offset = Light; //nastavení klidové hodnoty
NumOut(10, LCD_LINE2, Light, false); //zobrazení na display
Off(OUT_BC);
}
while(true) //nekonečná smyčka
{
Light = Sensor (IN_3); //načtení intenzity světla
Error = Light - Offset; //vypočítání odchylky
Integral = Integral + Error; //integrování
Derivate = Error - LastError; //derivování
Turn = Kp * Error + Ki * Integral + Kd * Derivate; //výpočet natočení
Turn = Turn / 100; //vydělení 100 pro navrácení vah
LastError = Error;
OnFwd (OUT_C, Turn); //rotování motorem
}
} //konec programu
Dance
Zadání: Tanec
Funkce: Algoritmus načte několik po sobě jdoucích hlasitostí zvuku a snaží se na ně reagovat tancem. Náhodně určí směr natočení a vyvolá tyto uložené hlasitosti, které jsou přímo úměrné rychlosti pohybu rotace.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
#define MAX 300
task main() //hlavní funkce
{
SetSensorTouch (IN_1);
SetSensorSound (IN_2);
SetSensorType (IN_2, SENSOR_TYPE_SOUND_DB);
char SoundVol[MAX];
int RandomRotate;
PlayTone(1000, 100);
Wait(100);
for(int i = 0; i < MAX; i++)
{
SoundVol[i] = Sensor(IN_2);
Wait(50);
}
while(true) //nekonečná smyčka
{
for(int i = 0; i < MAX; i++)
{
RandomRotate = Random(4);
Coast(OUT_BC);
if(RandomRotate == 0)
{
OnFwd(OUT_B, 10 + SoundVol[i]);
}
else if(RandomRotate == 1)
{
OnFwd(OUT_C, 10 + SoundVol[i]);
}
else if(RandomRotate == 2)
{
OnRev(OUT_B, 10 + SoundVol[i]);
}
else
{
OnRev(OUT_C, 10 + SoundVol[i]);
}
Wait(10 * SoundVol[i]);
}
}
} //konec programu
Color
Zadání: Barvy
Funkce: Tento program patří mezi ty delší, protože umožňuje rozpoznat až 15 barev. Úkol není tak snadný jak na první pohled vypadá. Dostupný je pouze světelný senzor a ten „vidí“ jen stupně šedi, které se poté musí podle naměřené tabulky rozpoznat a převést na barvu.
Ukázka programu:
#include "NXCDefs.h" //hlavičkový soubor
int Light; //intenzita světla
int CalibRange; //kalibrační hodnota
int Factor = 1; //faktor rozptylu
const int white = 710; //světelné konstatny pro jednotlivé barvy
const int yellow = 698;
const int orange = 694;
const int red = 690;
const int lgreen = 630;
const int lgrey = 620;
const int lblue = 578;
const int grey = 546;
const int blue = 532;
const int green = 524;
const int dgrey = 482;
const int dgreen = 472;
const int lblack = 415;
const int dblue = 409;
const int black = 380;
task main() //hlavní funkce
{
SetSensorTouch(IN_1); //inicializace
SetSensorLight(IN_3);
SetSensorMode (IN_3, IN_MODE_RAW); //nastavení módu na základní zobrazení
while (true) //nekonecna smycka
{
Off(OUT_BC); //vypnutí motorů
while(true) //cyklus kalibrace
{
while(Sensor(IN_1));
TextOut(4, LCD_LINE1, "Kalibrace");
SetSensorType (IN_3, IN_TYPE_LIGHT_INACTIVE);
Light = Sensor (IN_3);
TextOut(2, LCD_LINE5, "Svetlo:", true);
NumOut(50, LCD_LINE5, Light);
CalibRange = (Light - 70);
TextOut(2, LCD_LINE6, "Calib:" );
NumOut(50, LCD_LINE6, CalibRange);
if(Sensor(IN_1)) break;
}
while (true) //cyklus měření
{
while(Sensor(IN_1));
TextOut(4, LCD_LINE1, "-Mereni--");
SetSensorType (IN_3, IN_TYPE_LIGHT_ACTIVE);
Light = Sensor(IN_3);
TextOut(2, LCD_LINE5, "Svetlo:" );
NumOut(50, LCD_LINE5, Light);
if (Light >= ((white + CalibRange) - Factor)) //bílá
{
TextOut(10, LCD_LINE2, "-Bila-----" );
Factor--;
}
else if (Light <= ((yellow + CalibRange) + Factor) && Light >= ((yellow + CalibRange) - Factor)) //žlutá
{
TextOut(10, LCD_LINE2, "-Zluta----" );
Factor--;
}
else if (Light <= ((orange + CalibRange) + Factor) && Light >= ((orange + CalibRange) - Factor)) //oranžová
{
TextOut(10, LCD_LINE2, "-Oranzova-" );
Factor--;
}
else if (Light <= ((red + CalibRange) + Factor) && Light >= ((red + CalibRange) - Factor)) //červená
{
TextOut(10, LCD_LINE2, "-Cervena--" );
Factor--;
}
else if (Light <= ((lgreen + CalibRange) + Factor) && Light >= ((lgreen + CalibRange) - Factor)) //světle zelená
{
TextOut(10, LCD_LINE2, "-S Zelena-" );
Factor--;
}
else if (Light <= ((green + CalibRange) + Factor) && Light >= ((green + CalibRange) - Factor)) //zelená
{
TextOut(10, LCD_LINE2, "-Zelena---" );
Factor--;
}
else if (Light <= ((dgreen + CalibRange) + Factor) && Light >= ((dgreen + CalibRange) - Factor)) //tmavě zelená
{
TextOut(10, LCD_LINE2, "-T Zelena-" );
Factor--;
}
else if (Light <= ((lblue + CalibRange) + Factor) && Light >= ((lblue + CalibRange) - Factor)) //světle modrá
{
TextOut(10, LCD_LINE2, "-S Modra--" );
Factor--;
}
else if (Light <= ((blue + CalibRange) + Factor) && Light >= ((blue + CalibRange) - Factor)) //modrá
{
TextOut(10, LCD_LINE2, "-Modra----" );
Factor--;
}
else if (Light <= ((dblue + CalibRange) + Factor) && Light >= ((dblue + CalibRange) - Factor)) //tmavě modrá
{
TextOut(10, LCD_LINE2, "-T Modra--" );
Factor--;
}
else if (Light <= ((grey + CalibRange) + Factor) && Light >= ((grey + CalibRange) - Factor)) //šedá
{
TextOut(10, LCD_LINE2, "-Seda-----" );
Factor--;
}
else if (Light <= ((black + CalibRange) + Factor)) //černá
{
TextOut(10, LCD_LINE2, "-Cerna----" );
Factor--;
}
else //CHYBA
{
Factor++;
}
if(Sensor(IN_1)) break;
}
}
}
Hand
Zadání: Ruka
Funkce: Program využívá vyrobený joystic, kterým se dá robot interaktivně řídit a určovat natočení ruky a výšku ramene. Tento joystic je absolutní, takže nastavenou polohu přenese na polohu ramene. K otevírání a zavírání samotné ruky jsou k dispozici dvě tlačítka.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
#define F 3
task main() //hlavní funkce
{
SetSensorTouch (IN_3); //zapnutí tlačítkového sensoru na portu 1
SetSensorTouch (IN_4); //zapnutí tlačítkového sensoru na portu 1
int MiddleX = Sensor(IN_1);
int MiddleY = Sensor(IN_2);
int Touch1; //proměnná pro uložení stavu tlačítka
int Touch2; //proměnná pro uložení stavu tlačítka
int Joystic1; //joystic
int Joystic2; //joystic
int Rotate;
int Altitude;
int ActualRotate;
int ActualAltitude;
while(true) //nekonečná smyčka
{
ActualAltitude = MotorRotationCount (OUT_B);
ActualRotate = MotorRotationCount (OUT_C);
Joystic1 = Sensor (IN_1); //zjištění stavu
Joystic2 = Sensor (IN_2); //zjištění stavu
Touch1 = Sensor (IN_3); //zjištění stavu
Touch2 = Sensor (IN_4); //zjištění stavu
Altitude = (Joystic1 - MiddleX) * 3;
Rotate = (Joystic2 - MiddleY) * 4;
if(Touch1)
{
OnFwd(OUT_A, 20);
}
else if(Touch2)
{
OnRev(OUT_A, 20);
}
else
{
Off(OUT_A);
}
if(Altitude > (ActualAltitude + F))
{
OnFwd(OUT_B, 40);
}
else if(Altitude < (ActualAltitude - F))
{
OnRev(OUT_B, 30);
}
else
{
Off(OUT_B);
}
if(Rotate > (ActualRotate + F))
{
OnFwd(OUT_C, 50);
}
else if(Rotate < (ActualRotate - F))
{
OnRev(OUT_C, 50);
}
else
{
Off(OUT_C);
}
}
} //konec programu
Ukázky nejčastějších programů v soutěžích:
Line
Zadání: Čára
Funkce: Robot se snaží držet šedé barvi. Šedé proto, že senzor vidí jen stupně šedi a na středu hranice mezi černou čárou a bílým povrchem je právě ona šedá. V programu je implementován PD regulátor, který zajišťuje vcelku rychlé rozhodování. Proto je robot schopen jezdit podle čáry rychle, což je v soutěžích na čas znatelné.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
#define SPEED 100 //rychlost motorů
//#define Kp 1 //proporcionální konstanta
//#define Kd 50 //derivační konstanta
#define Kp 12 //proporcionální konstanta
#define Kd 1000 //derivační konstanta
task main() //hlavní funkce
{
SetSensorLight (IN_3); //zapnutí světelného senzoru
SetSensorTouch (IN_1); //zapnutí tlačítko
SetSensorMode (IN_3, IN_MODE_RAW); //přepnutí módu na rozlišení 0 až 1023
int Light; //proměná pro uložení hodnoty světla
int Touch; //proměná pro indikaci stavu tlačítka
int Turn; //natočení
int PowerB; //rychlost motoru B
int PowerC; //rychlost motoru C
int Offset = 0; //požadovaná hodnota
int Error = 0; //chyba
int LastError = 0; //minulá chyba
int Derivate = 0; //derivace
//--ZDE NUTNO NASTAVIT DO POŽADOVANÉ POLOHY--
//--Pro černou čáru je ro napůl na bílou a na půl na černou = šedá
while(!Touch) //dokud není stisknuto tlačítko
{
Light = Sensor (IN_3); //ulož hodnotu intenzity světla
Touch = Sensor (IN_1); //tlačítko
Offset = Light; //určení požadované polohy
NumOut(10, LCD_LINE3, Light, true);
Off(OUT_BC); //vypnutí motorů
}
while(true) //nekonečná smyčka
{
Light = Sensor (IN_3); //načtení intenzity světla
Error = Light - Offset; //zjištění chyby
Derivate = Error - LastError; //spočítání derivace
Turn = Kp * Error + Kd * Derivate; //výpočet natočení
PowerC = SPEED; //určení rychlosti
PowerB = SPEED;
Turn = Turn / 10;
if(Turn > 0) //zatáčení
{
PowerC -= Turn;
}
else
{
PowerB += Turn;
}
LastError = Error; //chyba přesunuta do minulé
//PowerC -= 20;
OnFwd (OUT_B, PowerB); //zapnutí motorů
OnFwd (OUT_C, PowerC);
}
} //konec programu
Maze
Zadání: Bludiště
Funkce: Existují 2 typy bludiště. Za 1. kdy je bludiště označeno pouze černou čárou na zemi a za 2. kdy je vyznačeno zdmi. My se budeme zabývat druhou možností. Tento algoritmus řeší pouze maticové bludiště (takové bludiště, kde jsou stěny nebo mezery pravoúhlé a vždy stejně velké).
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorLowspeed (IN_4);
int Distance;
while(true) //nekonečná smyčka
{
Distance = SensorUS (IN_4);
while(Distance > 8)
{
Distance = SensorUS (IN_4);
OnFwdReg(OUT_BC, 50, OUT_REGMODE_SYNC);
}
Off(OUT_ABC);
while(true)
{
RotateMotor(OUT_A, 25, 90);
Off(OUT_A);
Distance = SensorUS(IN_4);
if(Distance > 30)
{
RotateMotor(OUT_A, 25, -90);
Off(OUT_A);
RotateMotor(OUT_B, 40, -100);
RotateMotor(OUT_C, 40, 300);
Off(OUT_BC);
break;
}
else
{
RotateMotor(OUT_A, 25, -180);
Off(OUT_A);
Distance = SensorUS(IN_4);
if(Distance > 35)
{
RotateMotor(OUT_A, 25, 90);
Off(OUT_A);
RotateMotor(OUT_C, 40, -100);
RotateMotor(OUT_B, 40, 300);
Off(OUT_BC);
break;
}
else
{
while(true)
{
Off(OUT_ABC);
TextOut(10, LCD_LINE4, "CIL", true);
Wait(1);
}
}
}
}
}
} //konec programu
Fotbal
Zadání: Fotbal
Funkce: Asi nejznámějším sportem na světě je fotbal, tak proč by ho nemohli hrát roboti? Robot hledá míček a jakmile jej nalezne jede za ním dokud není míček na odpal. Jest-li že je, odpálí a jede za ním znovu. Pokud ho ztratí, hledá znovu.
Ukázka programu:
#include "NXCDefs.h" //vložení hlavičkového souboru
task main() //hlavní funkce
{
SetSensorLowspeed (IN_4);
SetSensorLight (IN_3);
int Distance;
int Light;
int Lost = 0;
Off(OUT_ABC);
while(true) //nekonečná smyčka
{
Light = Sensor (IN_3);
if(Light < 40)
{
OnRevReg(OUT_BC, 50, OUT_REGMODE_SYNC);
Wait(200);
RotateMotor(OUT_B, 100, -400);
RotateMotor(OUT_C, 100, 400);
}
else
{
Distance = SensorUS (IN_4);
if(Distance < 10)
{
Off (OUT_ABC);
RotateMotor (OUT_A, 100, -70);
Coast (OUT_A);
OnFwd (OUT_BC, 50);
RotateMotor (OUT_A, 100, 70);
Coast (OUT_A);
Lost = 0;
}
else if(Distance >= 10 && Distance < 50)
{
OnFwdReg(OUT_BC, 70, OUT_REGMODE_SYNC);
}
else
{
Lost++;
if(Lost < 50)
{
OnFwdReg(OUT_BC, 100, OUT_REGMODE_SYNC);
}
else
{
OnFwd (OUT_B, 50);
Coast (OUT_C);
}
}
}
}
} //konec programu