leggor.de -> nxt -> red_beast
+-
home
|
|
+--+-
nxt
| |
| |
| +----
red_beast
|
|
+---
ris
|
|
+---
technic
|
|
+---
train
|
|
+---
vignettes
|
|
+---
history
|
|
+---
links
|
|
+---
mail
|
|
+---
impressum
<<
>>
// software: // --------- // compiler: nbc.exe - version 1.0.1.29 // ide: bricx commandcenter - version 3.3 // firmware: lego nxt - version 1.03 // os: windows xp - sp2 // // //__________________________________________________________ // hardware: // --------- // lego nxt // usb kabel // //__________________________________________________________ // anschlussbelegung: // ------------------ // OUT_1 - motor links // OUT_2 - nicht belegt // OUT_3 - motor rechts // SENSOR_1 - optischer sensor(lego-licht-sensor) links // SENSOR_2 - optischer Sensor(lego-licht-sensor) mitte // SENSOR_3 - optischer Sensor(lego-licht-sensor) rechts // SENSOR_4 - nicht belegt // //__________________________________________________________ // programmablauf/doku: // -------------------- // // // 1.) datei "NXCDefs.h" includieren. // // // 2.) variablen deklaration:DunkelL = oberer grenzwert // links // DunkelM = oberer grenzwert // mitte // DunkelR = oberer grenzwert // rechts // HellL = unterer grenzwert // links // HellR = unterer grenzwert // mitte // HellM = unterer grenzwert // rechts // iL = sensorwert von // SENSOR_1 // iM = sensorwert von // SENSOR_2 // iR = sensorwert von // SENSOR_3 // Tolleranz = 1/5 der diff von // hell und dunkel // Kraft = motorkraft // fM = menuesteuerung // i = hilfsvariable // BtnL links = button 2 // BtnR rechts= button 1 // BtnE Enter = button 3 // // 3.) makro-definitionen. // // // 4.) variablen werden initialisiert. // // // 5.) optische sensoren an eingang 1, 2 und 3 mit raw modus // bekannt machen. // // // 6.) hauptmenue anzeigen(gehe zu inputmenue um grenzwerte // einzulesen). // // // 7.) das unterprogramm hauptmenue() erlaubt dem benutzer // zwischen den unterprogrammen Liniefolgen(), // InputMenue(), WerteTabelle(), EnergieAnzeige(), // SetTolleranz() und SetKraft() zu verzweigen. // fM1 >> Liniefolgen() // fM2 >> InputMenue() // fM3 >> WerteTabelle() // fM4 >> EnergieAnzeige() // fM5 >> SetTolleranz() // fM6 >> SetKraft() // // // 8.) im task Liniefolgen() befindet sich der // eigentliche algorithmus zum linie entlang fahren. // dieser teilt sich in folgende sieben aufgaben auf: // // // + fahrauftrag bestimmen // ----------------------- // um eine hohe durchschnittsgeschwindigkeit beibehalten // zu koennen sollten sich moeglichst immer beide raeder // drehen. als erstes werden durch den aufruf des // unterprogramms SW() die sensorwerte aktualisiert. // nun kann durch den vergleich der werte der neue // fahrauftrag bestimmt werden. befindet sich der // mittlere sensor allein ueber der linie so wird das // unterprogramm GeradeF() ausgefuehrt. Kommt das // fahrzeug nun zum linken rand der linie(dass heisst // der mittlere und der rechte sensor "sehen" die Linie), // so wird mit einem einfachen gegensteuern durch den // aufruf des unterprogramms RechtsF() gegengelenkt. // wenn aber der mittlere sensor die linie verlassen // hat(zum beispiel nach rechts(nur der linke sensor // "sieht" die Linie noch)) dann wird mit dem // Unterprogramm RechtsD() stark gegengelenkt. // // // +-------------+-----------+-----------+-----------+ // | fahrauftrag | SENSOR_1 | SENSOR_2 | SENSOR_3 | // | | | | | // +-------------+-----------+-----------+-----------+ // | Stop() | 0 | 0 | 0 | // | | | | | // +-------------+-----------+-----------+-----------+ // | RechtsD() | 0 | 0 | 1 | // | | | | | // +-------------+-----------+-----------+-----------+ // | GeradeF() | 0 | 1 | 0 | // | | | | | // +-------------+-----------+-----------+-----------+ // | RechtsF() | 0 | 1 | 1 | // | | | | | // +-------------+-----------+-----------+-----------+ // | LinksD() | 1 | 0 | 0 | // | | | | | // +-------------+-----------+-----------+-----------+ // | Stop() | 1 | 0 | 1 | // | | | | | // +-------------+-----------+-----------+-----------+ // | LinksF() | 1 | 1 | 0 | // | | | | | // +-------------+-----------+-----------+-----------+ // | Anhalten() | 1 | 1 | 1 | // | | | | | // +-------------+-----------+-----------+-----------+ // // 0 = hell // 1 = dunkel // // // + makro GeradeF() -ahren // ------------------------ // linker und rechter motor drehen mit "Kraft1" // vorwaerts. // // // + makro LinksF() -ahren // ----------------------- // linker motor dreht mit "Kraft2", // rechter mit "Kraft1" vorwaerts. // // // + makro RechtsF() -ahren // ------------------------ // rechter motor dreht mit "Kraft2", // linker mit "Kraft1" vorwaerts. // // // + makro LinksD() -rehen // ----------------------- // linker motor dreht mit "Kraft2" rueckwaerts. // rechter motor mit "Kraft1" vorwaerts. // // // + makro RechtsD() -rehen // ------------------------ // rechter motor dreht mit "Kraft2" rueckwaerts. // linker motor mit "Kraft1" vorwaerts. // // + makro Anhalten() // ------------------ // linker und rechter motor stoppen. // // // 9.) der task InputMenue() beinhaltet // die grenzwerterfassung der optischen sensoren. // beschreibung der grenzwerterfassung: // variablen "DunkelL", "DunkelM", "DunkelR", // "HellL", "HellM" und "HellR" mit werten // belegen. // dazu wird das fahrzeug
die schwarze // linie bewegt. zum speichern der werte genuegt // ein druck des orangenen tasters am nxt und // man befindet sich wieder im hauptmenue. // // // 10.) der task WerteTabelle(), ruft zur // aktuallisierung der sensorwerte, in einem // 8000 millisekunden interval das unterprogramm // SW() auf. dann zeigt es alle wichtigen werte // des programms in einer tabelle auf dem // display. dabei bedeutet: // // Sensor = aktueller wert // Hell = wert von variable "Hell"(L, M, R) // Dark = wert von variable "Dunkel"(L, M, R) // L = linker sensor(1) // M = mittlerer sensor(2) // R = rechter sensor(3) // // Sensor Hell Dark // // L=0000 0000 0000 // // M=0000 0000 0000 // // R=0000 0000 0000 // // // merke: je heller, desto groesser der wert. // // um zum hauptmenue zurueck zu kehren drueckt // man einfach den orangenen Button. // // // 11.) den aktuellen ladezustand des akku erfaehrt // man im task EnergieAnzeige(). // um zum hauptmenue wieder zurueck zu kehren drueckt // man einfach den orangenen Button. // // // 12.) den wert der variblen tolleranz aendert man im // task SetTolleranz(). dies geschieht durch // einfaches betaetigen der pfeieltasten. // zurueck zum hauptmenue gehts mit dem orangenen Button. // // // 13.) die drehkraft der motoren wird im task SetKraft() // mit den pfeieltasten eingestellt. // der orangene Button bringt dich wie immer zurueck ins // hauptmenue. // // // 14.) im unterprogramm SW() werden die aktuellen // sensorwerte in die dazugehoerigen variablen // geschrieben. // beispiel: "iL" = sensorwert des linken sensor //__________________________________________________________ #include "NXCDefs.h" int DunkelL; int DunkelM; int DunkelR; int HellL; int HellM; int HellR; int iL; int iM; int iR; bool BtnL; bool BtnR; bool BtnE; byte Kraft1; byte Kraft2; byte Tolleranz; byte fM; bool fM1; bool fM2; bool fM3; bool fM4; bool fM5; bool fM6; byte i; byte k; // aktueller Fahrbefehl byte r; // zuletzt gefahrene Richtung(entspricht dem k-Wert(ausser alle sind Hell oder Dunkel)) string LF = "Liniefolgen"; string IM = "InputMenue"; string WT = "WerteTabelle"; string EA = "EnergieAnzeige"; string ST = "Set Tolleranz"; string SK = "Set Kraft"; string tmp; bool ea; // wenn Energieabfrage schon statt gefunden hat bool ia; // wenn Grenzwerterfassung schon im Untermenue ausgegeben wurde #define ButtonL ButtonState(2); #define ButtonR ButtonState(1); #define ButtonE ButtonState(3); #define ButtonPause Wait(1200); #define MotorLv100 OnFwd (OUT_A,Kraft1); #define MotorLvHalb OnFwd (OUT_A,Kraft2); #define MotorLz100 OnRev (OUT_A,Kraft1); #define MotorLzHalb OnRev (OUT_A,Kraft2); #define MotorLstop Off (OUT_A); #define MotorRv100 OnFwd (OUT_C,Kraft1); #define MotorRvHalb OnFwd (OUT_C,Kraft2); #define MotorRz100 OnRev (OUT_C,Kraft1); #define MotorRzHalb OnRev (OUT_C,Kraft2); #define MotorRstop Off (OUT_C); #define GeradeF OnFwd (OUT_A,Kraft1 + 10); \ OnFwd (OUT_C,Kraft1 + 10); #define GeradeZ MotorLzHalb \ MotorRzHalb; #define LinksF MotorLvHalb; \ MotorRv100; #define RechtsF MotorLv100; \ MotorRvHalb; #define LinksD MotorLz100; \ MotorRv100; #define RechtsD MotorLv100; \ MotorRz100; #define Anhalten MotorLstop; \ MotorRstop; sub SW() { iL = SENSOR_1; iM = SENSOR_2; iR = SENSOR_3; } void ScrollBarH(byte v) { byte xL1; byte xL2; byte xR1; byte xR2; byte l; byte m = 30; byte sw = 10; v = v - 1; if(v > 0) { l = 10; } else { l = 9; } xL1 = l + (v * sw) xL2 = xL1 + 1 xR1 = xL1 + m xR2 = xR1 - 1 LineOut(1,0,98,0); LineOut(0,1,0,5); LineOut(3,3,5,1); LineOut(4,3,6,1); LineOut(3,3,5,5); LineOut(4,3,6,5); LineOut(8,0,8,6); LineOut(xL2,1,xR2,1); LineOut(xL1,2,xL1,4); LineOut(xR1,2,xR1,4); LineOut(xL2,5,xR2,5); LineOut(91,0,91,6); LineOut(94,1,96,3); LineOut(93,1,95,3); LineOut(94,5,96,3); LineOut(93,5,95,3); LineOut(99,1,99,5); LineOut(1,6,98,6); } sub MenueOut() { LineOut(0,55,99,55); switch(fM) { case 1: tmp = LF; break; case 2: tmp = IM; break; case 3: tmp = WT; break; case 4: tmp = EA; break; case 5: tmp = ST; break; case 6: tmp = SK; break; } } sub HauptMenue() { ClearScreen(); ScrollBarH(fM); MenueOut(); TextOut(10, LCD_LINE6, tmp); TextOut(5, LCD_LINE4, "gehst Du zu"); if(fM == 1 || fM == 5 ||fM == 6) { tmp = "..."; } if(fM == 2) { tmp = "m..."; } if(fM == 3 || fM == 4) { tmp = "r..."; } TextOut(71, LCD_LINE4,tmp); TextOut(5, LCD_LINE3, "Mit Enter"); TextOut(21, LCD_LINE1, "Hauptmenue"); } sub UnterMenue() { ClearScreen(); TextOut(5, LCD_LINE8, "der Entertaste!"); TextOut(5, LCD_LINE7, "Beenden mit"); MenueOut(); TextOut(7, LCD_LINE1, tmp); } sub LinieSuchen() { PlayTone(444,100); switch(r) { case 10: // vorher gerade gefahren GeradeZ; break; case 11: // vorher links gefahren LinksD; break; case 1: // vorher links gedreht LinksD; break; case 110: // vorher rechts gefahren RechtsD; break; case 100: // vorher rechts gedreht RechtsD; break; } Wait(100); } task LinieFolgen() { while(true) { if(fM1) { k = 0; SW(); ClearScreen(); if(iL <= DunkelL + Tolleranz) { k = k + 1; } if(iM <= DunkelM + Tolleranz) { k = k + 10; } if(iR <= DunkelR + Tolleranz) { k = k + 100; } if(! k == 0 && ! k == 111) { r = k; } switch(k) { case 0: tmp = "Anhalten Hell"; LinieSuchen(); break; case 1: tmp = "links drehen"; LinksD; break; case 10: tmp = "gerade fahren"; GeradeF; break; case 11: tmp = "links fahren"; LinksF; break; case 100: tmp = "rechts drehen"; RechtsD; break; case 110: tmp = "rechts fahren"; RechtsF; break; case 111: tmp = "Anhalten Dunkel"; LinieSuchen(); break; } TextOut(0, LCD_LINE8, tmp); } } } task InputMenue() { while(true) { if(fM2) { SW(); if(HellL < iL) { HellL = iL; } if(DunkelL > iL) { DunkelL = iL; } if(HellM < iM) { HellM = iM; } if(DunkelM > iM) { DunkelM = iM; } if(HellR < iR) { HellR = iR; } if(DunkelR > iR) { DunkelR = iR; } if(!ia) { TextOut(5, LCD_LINE4, "werden gelesen"); TextOut(5, LCD_LINE3, "Grenzwerte"); ia = true; } } } } task WerteTabelle() { while(true) { if(fM3) { SW(); ClearScreen(); NumOut(72, LCD_LINE8, DunkelR); NumOut(42, LCD_LINE8, HellR); NumOut(12, LCD_LINE8, iR); TextOut(0, LCD_LINE8, "R="); NumOut(72, LCD_LINE6, DunkelM); NumOut(42, LCD_LINE6, HellM); NumOut(12, LCD_LINE6, iM); TextOut(0, LCD_LINE6, "M="); NumOut(72, LCD_LINE4, DunkelL); NumOut(42, LCD_LINE4, HellL); NumOut(12, LCD_LINE4, iL); TextOut(0, LCD_LINE4, "L="); TextOut(0, LCD_LINE2, "Sensor Hell Dark"); Wait(6000); UnterMenue(); TextOut(5, LCD_LINE4, "aktuelle Werte"); Wait(2000); } } } task EnergieAnzeige() { int e; while(true) { if(fM4) { if(!ea) { UnterMenue(); e = BatteryLevel(); TextOut(5, LCD_LINE4, "Energie="); NumOut(55,LCD_LINE4,e); TextOut(81, LCD_LINE4, "mV"); ea = true; } } } } task SetTolleranz() { while(true) { if(fM5) { UnterMenue(); TextOut(5, LCD_LINE4, "Tolleranz="); NumOut(66,LCD_LINE4,Tolleranz); Wait(1000); } } } task SetKraft() { while(true) { if(fM6) { UnterMenue(); TextOut(5, LCD_LINE4, "Kraft="); NumOut(42,LCD_LINE4,Kraft1); Wait(300); } } } task main() { SetSensorLight(S1); SetSensorLight(S2); SetSensorLight(S3); SetSensorMode(S1, SENSOR_MODE_RAW); SetSensorMode(S2, SENSOR_MODE_RAW); SetSensorMode(S3, SENSOR_MODE_RAW); SetSoundVolume(5); iL = 0; iM = 0; iR = 0; Kraft1 = 40; Kraft2 = 10; fM1 = false; fM2 = false; fM3 = false; fM4 = false; fM5 = false; fM6 = false; Tolleranz = 100; start LinieFolgen; start InputMenue; start WerteTabelle; start EnergieAnzeige; start SetTolleranz; start SetKraft; fM = 2; HauptMenue(); while(true) { BtnL = ButtonL; BtnR = ButtonR; BtnE = ButtonE; if(BtnL) { if(! fM5 && ! fM6) { if(fM > 1) { fM = fM - 1; HauptMenue(); } } else { if(fM5) { if(Tolleranz > 4) { Tolleranz = Tolleranz - 10 } } if(fM6) { if(Kraft1 > 4) { Kraft1 = Kraft1 - 4 Kraft2 = Kraft1 / 4 } } } ButtonPause; } if(BtnR) { if(! fM5 && ! fM6) { if(fM < 6) { fM = fM + 1; HauptMenue(); } } else { if(fM5) { if(Tolleranz < 251) { Tolleranz = Tolleranz + 10 } } if(fM6) { if(Kraft1 < 85) { Kraft1 = Kraft1 + 4 Kraft2 = Kraft1 / 4 } } } ButtonPause; } if(BtnE) { switch(fM) { case 1: if(fM1) { fM1 = false; Anhalten; HauptMenue(); } else { fM1 = true; } break; case 2: if(fM2) { fM2 = false; HauptMenue(); } else { DunkelL = 1000; DunkelM = 1000; DunkelR = 1000; HellL = 0; HellM = 0; HellR = 0; UnterMenue(); ia = false; fM2 = true; } break; case 3: if(fM3) { fM3 = false; HauptMenue(); } else { fM3 = true; } break; case 4: if(fM4) { fM4 = false; HauptMenue(); } else { ea = false; fM4 = true; } break; case 5: if(fM5) { fM5 = false; HauptMenue(); } else { fM5 = true; } break; case 6: if(fM6) { fM6 = false; HauptMenue(); } else { fM6 = true; } break; } ButtonPause; } } }