Labyrinth-Roboter mit LEGO Mindstorms

Transkript

1 Labyrinth-Roboter mit LEGO Mindstorms Philipp Bodewig, Christoph Weiler 8. August

2 Inhaltsverzeichnis Inhaltsverzeichnis 1 Praktikumsziele Labyrinth Realisierung der Praktikumsziele Bausteine Lichtsensor Beschaffenheit des Untergrundes Test DataLog Linie Motor RCX Roboter Radius und Mittelpunkt der Standardkurve Versuchsbeschreibung Auswertung Ergebnis Design Installation von NQC Voraussetzungen Erfüllen der Voraussetzungen Kernelmodule Deviceknoten Installation der benötigten Pakete FLEX GNU M GNU Bison Installation von NQC Installation der Firmware Bewegungsalgorithmen Zwei Sensoren Weg Kreuzung Drei Sensoren Weg Kreuzung Erkennungsalgorithmen zwei Sensoren Kreuzung Sackgasse drei Sensoren Kreuzung

3 Inhaltsverzeichnis Sackgasse Ziel Suchalgorithmen Tiefensuche (Depth-First-Search) Breitensuche (Breadth-First-Search) Fazit Speicherung des Labyrinths Speichern einer Kreuzung Speichern mehrer Kreuzungen auf dem Stack Implementierung des Stack Rückfahrt Addons Start ab beliebigem Startpunkt Suchalgorithmus Speicherung des Labyrinths Maximum der Kreuzungsanzahl finden Implementation des größeren Stack Test des größeren Stack Probleme

4 1 Praktikumsziele 1 Praktikumsziele Das Ziel unseres Praktikums ist, mit LEGO Mindstorms einen Roboter zu entwickeln, der ein, nach von uns festgelegten Spezifikationen erstelltes, Labyrinth erforschen kann. Dabei soll er sich den bisher erkundeten Teil des Labyrinths merken und einen Weg vom Ausgangspunkt zum Ziel finden. Anschließend soll er auf schnellstem Weg zum Start zurückkehren. 1.1 Labyrinth Für unser Labyrinth haben wir uns folgende Spezifikationen überlegt. Das Labyrinth ist mit Kreide auf schwarzem Grund aufgemalt. Es existiert ein eindeutiges Ziel im Labyrinth, welches durch einen großen weißen Bereich gekennzeichnet ist. Im Labyrinth sind höchstens 20 T- und X-Kreuzungen vorhanden. Die Kreuzungen sind im 90 Winkel gehalten, eckige Kurven haben einen Winkel von mindestens 90, differenzierbare Kurven einen Kurvenradius von mindestens 10 cm. Sackgassenpunkte haben einen Abstand von mindestens 15 cm zu anderen Wegen. Es existieren keine Schleifen im Labyrinth. Abbildung 1.1: Ein typischer Labyrinthaufbau 1.2 Realisierung der Praktikumsziele Das Praktikum haben wir in folgende Sektionen unterteilt, die wir einzeln bearbeiten und dokumentieren. Bausteine In diesem Abschnitt wird insbesondere auf die Fähigkeiten der Sensoren eingegangen sowie technische Informationen zu den Motoren und der RCX-Einheit geliefert. Roboter Hier geht es um das Design und die Eigenschaften unseres Roboters. 4

5 1 Praktikumsziele Installation von NQC. Dieser Teil des Praktikums beschäftigt sich mit der Einrichtung einer Entwicklungsumgebung im Robotiklabor, in der wir die Steuerprogramme für unseren Roboter schreiben. Bewegungsalgorithmen. In diesem Teil unseres Praktikums schreiben wir Methoden, die den Roboter einer geraden Linie, einer Kurve oder einer Kreuzung (in eine bestimmte Richtung) folgen lassen. Er ist eng mit den Erkennungsalgorithmen verbunden. Erkennungsalgorithmen. Hier beschäftigen wir uns mit der Erkennung von Wegen, Kreuzungen und Sackgassen. Suchalgorithmen. Die Algorithmen mit denen wir das Labyrinth auf der Suche nach dem Ziel erkunden. Speicherung des Labyrinths. Um, nachdem das Ziel erreicht wurde, den schnellsten Weg zum Ausgangspunkt zurückzufinden, müssen wir den erkundeten Teil des Labyrinths im Speicher des Roboters festhalten. In diesem Teil des Praktikums beschäftigen wir uns mit verschiedenen Möglichkeiten des Speichermanagements. Im Folgenden gehen wir auf alle diese Abschnitte im Detail ein und beschreiben auftretende Probleme und Lösungsansätze. 5

6 2 Bausteine 2 Bausteine Für den Bau unseres Roboters verwenden wir das LEGO Mindstorms Robotics Invention System 2.0. Die wichtigsten Bauteile stellen wir in den nächsten Abschnitten vor. 2.1 Lichtsensor Der Lichtsensor besteht aus einem Sensor und einer LED eingebaut in einen 2 x 4 Baustein. Das Licht der LED wird von verschiedenfarbigem Grund (speziell weißer Kreide und schwarzer Tafeloberfläche) unterschiedlich stark reflektiert und auf den Sensor zurückgeworfen. Der Sensor wandelt das Licht in Spannung, welche die RCX-Einheit in sogenannte RAW-Werte skaliert. Diese RAW-Werte liegen zwischen 0 und Alternativ kann der RCX auch Werte in Prozent (SENSOR_MODE_PERCENT)verwenden. Sie können durch die Formel (vergleiche [2]) P ERCENT = 25 (1023 RAW ) 156 ineinander umgewandelt werden. Mit einem größeren Wertebereich lassen sich genauere Ergebnisse erzielen, weshalb in den Tests mit RAW-Werten gearbeitet wird Beschaffenheit des Untergrundes Als Untergrund wird die im Robotiklabor vorhandene Tafelfläche verwendet und mit Kreide ein Labyrinth aufgemalt. Diese Methode ist sehr einfach, unter anderem auch, weil ein Labyrinth leicht zu reproduzieren ist. Allerdings tauchen Probleme auf, die vor allem die Erkennung der Kreidemarkierung betreffen. Kreide deckt nicht vollständig den Grund mit Farbe ab, es verbleiben einige schwarze Stellen. Daher führen wir mehrere Tests durch, um festzustellen, wie wir eine Linie zeichnen müssen, damit sie optimal erkannt wird Test Als Test für die Güte der Erkennung malen wir mehrere Kreidestriche auf die Tafelfläche. Diese Linien tragen wir in verschiedener Dicke und Intensität auf. Abbildung 2.1: Muster mit 17 Kreide-Linien 6

7 2 Bausteine DataLog Der RCX beinhaltet eine Methode um Sensorwerte zu speichern, genannt Datalog. In diesen kann man mittels des NQC-Befehls CreateDataLog(X) Daten schreiben, wobei X die Kardinalität der Punktmenge ist, die gespeichert wird. NQC-Code 1: datalog/datalog.nqc #include "defs.nqc" task main() { SetSensor(SENS_MID, SENSOR_LIGHT); SetSensorMode(SENS_MID, SENSOR_MODE_RAW); SetPower(ENG_LEFT,4); SetPower(ENG_RIGHT,5); start fwd; start datalogging; Wait(650); StopAllTasks(); task fwd() { fwd_until(0); task datalogging() { CreateDatalog(600); int i; for(i=0;i<=600;i++) { AddToDatalog(SENS_MID); PlaySound(SOUND_DOWN); Mit dem Befehl nqc -datalog > datalog.txt werden die Daten vom RCX runtergeladen und in einer Text-Datei eingelesen. Die Werte sind, in Abbildung 1, in Abhängigkeit von der Zeit grafisch dargestellt. 7

8 2 Bausteine Abbildung 2.2: RCX-Data / Zeit Diagramm Durch die dunkelgrauen Querbalken in der Abbildung wird ein Intervall markiert. Innerhalb dieses Intervalls kann jeder Wert als Grenze für die Entscheidung zwischen Linie und Tafel benutzt werden. Im Vergleich mit den Eigenschaften der Linien und der Art wie sie auf die Tafel aufgetragen wurden, ist folgendes zu bemerken (Die Linien sind von links von 1 bis 17 durchnummeriert): 1. Zu den Linien: a) Bestenfalls sollen Linien von ähnlicher Farbstärke und Dicke wie die der Linien 9 und 16 gemalt werden. b) Auch die Linien 3 bis 7 sind voll ausreichend. c) Die Linien 1, 2, 13 und 15 haben nur an wenigen Zeitpunkten Werte unterhalb der unteren Intervallgrenze. Es ist nicht auszuschließen, dass sie übersehen werden. d) Linie 17 wird wohl nicht übersehen, doch sie ist ebenso ungeeignet. Wie an dem Peak nach oben zwischen den beiden Peaks nach unten zu sehen, ist es nicht ausgeschlossen, dass diese eine Linie als zwei Linien erkannt wird. e) Alle anderen Linien sind entweder zu schwach oder zu dünn gezeichnet. Sie dienen als Beispiel, wie es nicht gemacht werden sollte. 2. Zu dem grau markierten Bereich: Das Intervall liegt zwischen den RAW-Werten 772 und 796. Dies entspricht Prozent- Werten von 37 und 40. Mit Werten dazwischen sollten einerseits alle Linien erkannt, 8

9 2 Bausteine jedoch nicht durch Verunreinigungen irritiert werden. Die Linien 8 und 11 sind als ein Beispiel für solche Verunreinigungen anzusehen. Der gleiche Test mit Speicherung der Daten in Prozent bestätigt die Ergebnisse. Abbildung 2.3: RCX-Data / Zeit Diagramm Linie Ein weiterer Test liefert Meßwerte eines Lichtsensors, während der Roboter eine Linie entlang fährt. Aus Gründen, die später näher erläutert werden, fährt der Roboter nicht gerade, sondern hat einen Drift nach links. Um eine korrekte Fahrweise entlang der Linie sicherzustellen wird das Differential, mit dem unser Roboter ausgestattet ist, durch einen einfachen Trick gesperrt. Es werden flache Bausteine so an dem Roboter befestigt, dass sie in das Zahnrad des Differentials hineingreifen und dieses an der Bewegung hindern. Dadurch drehen sich die beiden Räder gleichschnell. 9

10 2 Bausteine Abbildung 2.4: Differentialsperre Abbildung 2.5: RCX-Data / Zeit Diagramm Es ist zu erkennen, dass der oben definierte Bereich verlassen wird, obwohl der Roboter eigentlich auf der geraden Linie durchgehend Weiß sehen sollte. Die Linie wurde ohne besonderes Hilfsmittel (Lineal oder ähnliches) gezeichnet und ist deshalb nicht ganz so gerade wie gefordert. Die Abweichung erfolgt über ein etwas vergrößertes Zeitintervall hinweg und ist kein plötzlicher Peak. Damit liegt eine Krümmung der Linie vor. Die Abweichung auf die schwarze Fläche der Tafel würde daher durch den Erkennungsalgorithmus als Verlassen der Linie 10

11 2 Bausteine identifiziert werden und eine Korrektur würde erfolgen. Der Test liefert trotzdem ein befriedigendes Ergebnis. Außerhalb der beschriebenen Abweichung sind die Meßwerte weit unterhalb der oberen Toleranzintervallgrenze, was zum einen die Qualität der Linie, zum anderen die Sensorerkennung für gut befinden lässt. 2.2 Motor Wir verwenden den im Mindstorm-Baukasten enthaltenen Electric Technic Mini-Motor 9v, auch Power-Motor genannt. Dieser Motor besitzt eine interne Zahnradübersetzungen und ein Schwungrad, wodurch er eine mittlere Geschwindigkeit und ein hohes Drehmoment hat. Im Normalbetrieb benötigt er 10 ma und wenn er festgefahren ist 250 ma. Die Leistungssteuerung der Motorausgänge erfolgt über Pulsweitenmodulation (vergleiche [4]). Die Motoren, die wir verwenden, unterscheiden sich in ihrer Leistung. Dieser Umstand ist wohl auf unterschiedliche Abnutzung zurückzuführen. Wir gleichen diesen Defekt mit Hilfe von NQC aus. Die jeweilige Stärke der Motoren läßt sich mittels SetPower(ENG_LEFT,4); SetPower(ENG_RIGHT,5); definieren. Wir erhalten eine zufriedenstellende Bewegung. Auch durch das ständige Korrigieren (siehe Abschnitt 5.2.1) ist eine Sperrung des Differentials, wie oben beschrieben, nicht nötig. 2.3 RCX Die RCX-Einheit bildet das Herzstück unseres Roboters. Sie ist als Lego-Baustein konstruiert und kann daher am Rumpf des Roboters angebracht werden. In ihr befindet sich ein Renesas H8/3292 Mikrocontroller[6], ein Ein-Chip-Computersystem. In dieses System sind die Komponenten CPU (H8/300), Speicher und Input/Output integriert und durch einen Bus[7] miteinander verbunden. Der Instruktionssatz der H8/300 CPU beinhaltet Register-Register Arithmetik und logische Operationen. An Speicher steht ein 16 kb ROM und ein 32 kb RAM zur Verfügung[8]. In Verbindung mit NQC lassen sich darin 32 globale und 16 lokale (Integer-) Variablen speichern[12]. Mittels eines Infrarot Towers wird eine Infrarot-Verbindung mit der RCX-Einheit hergestellt. Dazu muß der RCX direkt vor dem Tower postiert werden. So ist es möglich, Programme auf den RCX hochzuladen (und gleichzeitig zu kompilieren) oder Daten vom RCX runterzuladen. Vergleiche Abschnitt

12 3 Roboter Abbildung 2.6: RCX-Data / Zeit Diagramm 3 Roboter 3.1 Radius und Mittelpunkt der Standardkurve In einem einfachen Versuch bestimmen wir den Radius unseres Roboters Versuchsbeschreibung An die Front des Roboters wird mit Tesafilm ein Bleistift senkrecht auf den Boden zeigend festgeklebt. Die Spitze des Bleistifts befindet sich direkt auf dem Boden und zeichnet, zirkelähnlich, eine Kurve, sobald sich der Roboter in Bewegung setzt. Es entstehen Kreise, denen jedoch noch ein Mittelpunkt und damit auch der Radius fehlt. Abbildung 3.1: Roboter mit Bleistift 12

13 3 Roboter Auswertung Mit leichten Hilfsmitteln aus der Schul- Geometrie wird der Mittelpunkt gefunden und die Länge des Radius kann abgelesen werden: Es werden zwei beliebige Geraden durch den Kreis gezeichnet, sodass Strecken durch die zwei jeweiligen Schnittpunkte von den Geraden mit dem Kreis entstehen. Diese werden mit Mittelsenkrechten versehen, die sich im Mittelpunkt des Kreises schneiden Ergebnis Links- und Rechtskurve werden annähernd mit gleichem Radius gefahren werden. Der Radius hat mit einem Fehler von 0,5% eine Länge von 9cm, weshalb wir für die Abstände zwischen Kreuzungen und Sackgassen ein Minimum von 10 beziehungsweise 15 cm gewählt haben. 3.2 Design Das Design unseres Roboters entwickelten wir unter Zuhilfenahme einer Anleitung aus dem Internet (Link [9]). Diese modifizierten wir für unsere Zwecke. Die wichtigsten Eigenschaften sind: Der Roboter ist sehr breit gebaut, sodass auf ihm Motoren, die RCX-Einheit, Sensoren und weiteres genug Platz finden. Die schmalen Räder sind weit außen am Roboter angebracht und sind damit in sicherer Distanz zu den Kreidelinien, die leicht von dem Gummi der Reifen abgetragen werden (leider kommt es trotzdem oft genug vor, dass die Räder schwarze Stellen auf den Linien hinterlassen, und dass von ihnen auf der sonst schwarzen Oberfläche Kreiderückstände entstehen). Es existieren nur zwei Räder. Zwei Stützen, die aus kleinen Tellern bestehen, gleiten auf dem Boden und stabilisieren den Roboter. Durch das verschieben der Räder nach außen bleibt in der Mitte genug Platz für bis zu drei Sensoren. Mit dem Programm LeoCAD[10] haben wir eine Anleitung zum Nachbauen erstellt, die auf unserer Homepage[11] runtergeladen werden kann. 13

14 4 Installation von NQC 4 Installation von NQC Zur Ansteuerung des RCX existiert eine Fülle von verschiedenen Programmen. Die gebräuchlichsten der unter Linux lauffähigen Programme sind LegOS/BrickOS[13], NQC [12] und das Java-basierte lejos[14]. Wir haben uns dazu entschlossen NQC zu benutzen. Im Vergleich mit den anderen Programmen ist es am einfachsten zu installieren und die Sprache NQC wirkt sehr einfach zu erlernen. Desweiteren haben wir beide mehr Erfahrung mit C-ähnlichen Sprachen als mit Java. 4.1 Voraussetzungen Voraussetzungen für die Installation von NQC sind: Die Kernelmodule für die USB-Schnittstelle (usbcore) und darauf aufbauend das Modul für den Mindstorms USB-Tower (legousbtower) Der Deviceknoten /dev/usb/legousbtower0 (standardmäßig nicht von SuSE angelegt) Zum kompilieren von NQC: FLEX[15] GNU Bison[16] GNU M4[17] (Voraussetzung zur Kompilation von Bison) 4.2 Erfüllen der Voraussetzungen Kernelmodule In diesem Abschnitt muss der USB-Tower mit dem Rechner verbunden sein! Wenn der Befehl lsmod grep legousbtower ohne Ausgabe terminiert ist das Modul nicht geladen und es sind weitere Maßnahmen am Kernel nötig. In unserem Falle ist das Modul im Kernel vorhanden und wurde mit dem Einstecken den USB-Towers auch korrekt eingebunden Deviceknoten Der Befehl ls /dev/usb/ listet alle Knoten im Verzeichnis /dev/usb auf. Wenn der Knoten legousbtower0 nicht vorhanden ist muss er vom Systemadministrator mit dem Befehl mknod -m 666 /dev/usb/legousbtower0 c erstellt werden. 14

15 4 Installation von NQC Installation der benötigten Pakete Da wir keinen Administratorzugang haben legen wir das Verzeichnis local im Heimatverzeichnis /home/cweiler mit dem Befehl mkdir ~/local an, um dort die benötigten Pakete zu installieren. Um einen komfortableren Zugang zu den ausführbaren Dateien zu haben, erweitern wir die PATH -Variable um /home/cweiler/local/bin. export PATH=/home/cweiler/local/bin:$PATH Da nach einem erneuten Einloggen PATH zurückgesetzt wird, tragen wir diesen Befehl auch in die Datei /home/cweiler/.bashrc ein, die bei jedem Login ausgeführt wird. echo "export PATH=/home/cweiler/local/bin:\$PATH" >> ~/.bashrc Des weiteren legen wir das Verzeichnis /home/cweiler/src an, um dort die Programmquellen zu kompilieren FLEX Zur Installation von Flex wechseln wir zunächst in unser gerade erstelltes src-verzeichnis cd ~/src um dort die Quellen für Flex herunterzuladen und zu entpacken, wget tar xvfz flex-2.5.4a.tar.gz und anschließend zu installieren cd flex /configure --prefix=/home/cweiler/local && make && make install GNU M4 Die Installation von M4 läuft ähnlich ab, wie die gerade durchgeführte. cd ~/src wget tar xvfz m4-1.4.tar.gz cd m4-1.4./configure --prefix=/home/cweiler/local && make && make install GNU Bison Für diese Installation muss M4 korrekt installiert sein und die PATH -Variable richtig gesetzt sein. cd ~/src wget tar xvfz bison-2.1.tar.gz cd bison-2.1./configure --prefix=/home/cweiler/local && make && make install 15

16 4 Installation von NQC 4.3 Installation von NQC Zunächst wechseln wir wieder ins src-verzeichnis und laden die Quellen für NQC herunter. wget tar xvfz nqc-3.1.r4.tgz Anschließend öffnen wir das Makefile mit einem Editor, zum Beispiel mit dem Befehl kate nqc-3.1.r4/makefile und ändern die Zeile in PREFIX?=/usr/local PREFIX?=/home/cweiler/local damit NQC ins richtige Verzeichnis installiert wird und entfernen das Kommentarzeichen (#) am Anfang der Zeile # USBOBJ = rcxlib/rcx_usbtowerpipe_linux.o um die Unterstützung für den USB-Tower zu aktivieren. Jetzt öffnen wir die Datei nqc-3.1-r4/rcxlib/rcx USBTowerPipe linux.cpp und ändern Zeile 57 von in #define DEFAULT_TOWER_NAME "/dev/usb/lego0" #define DEFAULT_TOWER_NAME "/dev/usb/legousbtower0" Dieser Schritt ist wichtig damit NQC das richtige Device anspricht. Anschließend laden wir noch eine alte Version des USB-Treibers für den Mindstorms-Tower herunter. Zwar ist der Treiber mittlerweile im Kernel integriert, jedoch lässt sich diese Version von NQC nicht ohne den Treiber kompilieren. Den Treiber fanden wir unter [18] und [19]. Wir haben ihn jedoch noch einmal gespiegelt um ihn mit wget herunterladen zu können und damit diese Anleitung so einfach wie möglich zu halten. mkdir LegoUSB cd LegoUSB wget wget cd.. mv LegoUSB/ nqc-3.1-r4/rcxlib/ Jetzt können wir ins NQC-Verzeichnis wechseln und kompilieren cd nqc-3.1-r4/ make && make install 16

17 4 Installation von NQC Damit ist NQC installiert. Wir setzen noch zwei Umgebungsvariablen um NQC nicht jedes mal das Device mitteilen zu müssen export RCX_PORT="usb" export NQC_OPTIONS="-Trcx2" und speichern diese auch in der.bashrc echo "export RCX_PORT=usb" >> ~/.bashrc echo "export NQC_OPTIONS="-Trcx2"" >> ~/.bashrc Jetzt sollte die Kommunikation mit dem RCX funktionieren. nqc -near No firmware installed on RCX Im nächsten Abschnitt werden wir die Firmware auf dem RCX installieren. 4.4 Installation der Firmware Die Firmware für den RCX ist im LEGO Mindstorms SDK2 [20] enthalten. Wir haben die Firmware-Datei noch einmal gespiegelt. cd ~/src mkdir firmware cd firmware wget nqc -firmware firm0328.lgo Wenn beim letzten Befehl keine Fehlermeldung erscheint, ist die Firmware korrekt installiert und NQC-Programme können mit nqc -d programm.nqc auf den RCX geladen werden. 17

18 5 Bewegungsalgorithmen 5 Bewegungsalgorithmen In diesem Abschnitt beschäftigen wir uns mit den Methoden, die unseren Roboter durch das Labyrinth steuern. Wie schon in Kapitel 2.2 beschrieben besitzt unser Roboter zwei Motoren, die wir ansteuern können, um die beiden Räder des Roboters zu bewegen. Diese Steuerung geschieht in Abhängigkeit davon, welche der Lichtsensoren sich gerade über einer Kreidelinie befinden und welche nicht. 1 Abhängig davon ob wir zwei oder drei Sensoren benutzen unterscheiden sich die Bewegungsalgorithmen erheblich, weshalb wir diese beiden Fälle getrennt betrachten. In den folgenden Abschnitten unterscheiden wir zwischen differenzierbaren und nicht differenzierbaren Kurven. Mit differenzierbaren Kurven sind solche gemeint, die in einem konstanten Radius zu einem Punkt neben dem Weg verlaufen. Unter einer nicht differenzierbaren Kurve verstehen wir einen Knick im Weg, der durch seinen Innenwinkel spezifiziert wird. a) b) Abbildung 5.1: a) Eine differenzierbare Kurve. b) Eine nicht differenzierbare Kurve. 5.1 Zwei Sensoren In diesem Abschnitt beschäftigen wir uns damit, wie unser Roboter, mit zwei Sensoren ausgestattet, einem Weg ohne Abzweigungen folgt und an einer erkannten Kreuzung die von uns gewählte Richtung einschlägt Weg Um einem Weg ohne Abzweigungen zu folgen reicht uns folgendes simples Programm: NQC-Code 2: 2sensor/line.nqc /* 2sensor/line.nqc * 1 vgl. Abschnitt

19 5 Bewegungsalgorithmen * Der Roboter faehrt eine Linie entlang, indem er eine Linkskurve faehrt * so lange der rechte Sensor auf Schwarz ist und anschliessend eine Rechtskurve * bis der linke Sensor Weiss zeigt. * */ #include "kurven.nqc" task main() { SetSensor(SENSOR_1, SENSOR_LIGHT); SetSensor(SENSOR_3, SENSOR_LIGHT); while(1) { linkskurve(); rechtskurve(); NQC-Code 3: 2sensor/kurven.nqc /* 2sensor/kurven.nqc * * Algorithmen um eine Kurve zu fahren bis sich der aeussere Sensor * ueber einer weissen Linie befindet. * */ #define WHITE 37 // Schwellwert definieren. #define SENS_LEFT SENSOR_1 // Sensoren definieren #define SENS_RIGHT SENSOR_3 //... #define ENG_LEFT OUT_A // Motoren definieren #define ENG_RIGHT OUT_C //... void rechtskurve() { On(ENG_LEFT + ENG_RIGHT); SetPower(ENG_RIGHT,2); SetPower(ENG_LEFT,7); Fwd(ENG_LEFT); Fwd(ENG_RIGHT); until(sens_left>= 37); Off(ENG_LEFT + ENG_RIGHT); //beide Motoren einschalten //den rechten Motor schwaecher machen //als den linken //eine Kurve fahren bis der linke Sensor Weiss sieht void linkskurve() 19

20 5 Bewegungsalgorithmen { On(ENG_LEFT + ENG_RIGHT); SetPower(ENG_RIGHT,7); SetPower(ENG_LEFT,2); Rev(ENG_LEFT); Rev(ENG_RIGHT); until(sens_right>=37); Off(ENG_LEFT + ENG_RIGHT); //und umgekehrt... Der benutzte Algorithmus ist simpel: Wir gehen davon aus, dass sich die Linie am Anfang zwischen den beiden Lichtsensoren befindet. Wir wollen eine Linkskurve fahren bis sich der rechte Sensor über der Linie befindet. Dazu stellen wir den rechten Motor so ein dass sich das rechte Rad vorwärts dreht und den linken Motor so dass sich das linke Rad rückwärts dreht. Damit sich unser Roboter nicht im Kreis dreht, steuern wir das rechte, äußere Rad stärker an. Sobald sich der rechte Lichtsensor über der weißen Linie befindet fahren wir eine Rechtskurve bis sich der linke Sensor über der Linie befindet, indem wir die Motoren genau umgekehrt ansteuern. Dieses Verfahren funktioniert erstaunlich gut. Der Roboter kann sowohl enge differenzierbare Kurven fahren als auch nicht differenzierbare Kurven mit sehr kleinem Winkel, vorausgesetzt im Kurvenradius des Roboters befindet sich keine andere weiße Linie Kreuzung Falls eine Kreuzung mit zwei Sensoren erkannt wurde können wir davon ausgehen dass es eine T-Kreuzung ist, und dass sich unsere zwei Sensoren auf den beiden Wegen befinden, zwischen denen wir uns entscheiden müssen. 2 Wollen wir den rechten Weg nehmen, drehen wir den Roboter nach rechts, bis sich der linke Sensor nicht mehr über dem linken Weg befindet, sondern über dem rechten. Umgekehrt verfahren wir genauso. NQC-Code 4: rechten Weg nehmen (zwei Sensoren) On(ENG_LEFT + ENG_RIGHT); SetPower(ENG_LEFT,5); SetPower(ENG_RIGHT,5); Fwd(ENG_LEFT); Fwd(ENG_RIGHT); until(sens_left<= WHITE); until(sens_left>= WHITE); Off(ENG_LEFT + ENG_RIGHT); 5.2 Drei Sensoren Im Folgenden beschreiben wir die Methoden, mit denen der mit drei Sensoren bestückte Roboter einem Weg folgt und wie er an einer Kreuzung eine bestimmt Richtung einschlägt. 2 vgl. Abschnitt

21 5 Bewegungsalgorithmen Weg Im Gegensatz zum Algorithmus für zwei Sensoren, der den Roboter eine Schlangenlinie um den zu verfolgenden Weg fahren lässt, versucht die hier beschriebene Methode den mittleren Sensor exakt auf der Linie zu halten, um so auf beiden Seiten des Weges einen Sensor ausschließlich zur Kreuzungserkennung zur Verfügung zu haben. NQC-Code 5: 3sensor/line.nqc /* 3sensor/line.nqc * * Der Roboter mit 3 Sensoren soll möglichst genau eine Linie abfahren * */ #define WHITE 37 // Schwellwert definieren. #define SENS_LEFT SENSOR_1 // Sensoren definieren #define SENS_MID SENSOR_2 //... #define SENS_RIGHT SENSOR_3 //... #define ENG_LEFT OUT_A // Motoren definieren #define ENG_RIGHT OUT_C //... task main() { SetSensor(SENS_LEFT, SENSOR_LIGHT); // Sensortypen setzen SetSensor(SENS_MID, SENSOR_LIGHT); //... SetSensor(SENS_RIGHT, SENSOR_LIGHT); //... while (1) { // Die Hauptschleife. // Geradeaus fahren SetPower(ENG_LEFT,4); // Die unterschiedliche Staerke SetPower(ENG_RIGHT,5); // der Motoren wird korrigiert. Rev(ENG_LEFT); // Vorwaerts fahren Fwd(ENG_RIGHT); //... if (SENS_MID>=37) On(ENG_LEFT + ENG_RIGHT); until(sens_mid<37); // bis der mittlere Sensor von der Off(ENG_LEFT + ENG_RIGHT); // Linie abgekommen ist // Korrektur Fwd(ENG_LEFT + ENG_RIGHT); // Nach links drehen bis der mittlere On (ENG_LEFT + ENG_RIGHT); // oder der rechte Sensor Weiss zeigt until (SENS_MID>=37 SENS_RIGHT>=37); if (SENS_RIGHT>=37) { // Wenn wir nach links Rev(ENG_LEFT + ENG_RIGHT); // abgekommen sind drehen wir On(ENG_LEFT + ENG_RIGHT); // den Roboter nach rechts. until (SENS_MID>=37); 21

22 5 Bewegungsalgorithmen // Ende Hauptschleife. Der Roboter fährt geradeaus solange der mittlere Sensor die weiße Linie erkennt. Sobald dieser auf Schwarz umschaltet, versuchen wir den Weg wiederzufinden: der Roboter dreht sich nach links bis entweder der mittlere oder der rechte Sensor die weiße Linie erkennen. Zeigt der mittlere Sensor Weiß an so sind wir vorher nach rechts abgewichen und können nun weiter geradeaus fahren. Zeigt der rechte Sensor Weiß sind wir vorher nach links abgewichen und drehen den Roboter nun nach rechts bis der mittlere Sensor wieder Weiß signalisiert, um anschließend weiter geradeaus zu fahren. Der Roboter kann mit diesem Programm eine ausreichend dick gezogene Linie nachfahren. Auch geschwungene Kurven sowie Kurven mit stumpfem Winkel erkennt der Roboter. Probleme gibt es bei Rechtskurven mit spitzem Winkel, da der Roboter zuerst nach links korrigiert. Der Fehler lässt sich beheben, indem wir, nachdem sich der Roboter eine gewisse Zeit nach links gedreht hat und keine Linie erkannt hat, annehmen, dass sich die Linie rechts der Sensoren befindet. Kurven mit spitzem Winkel schließen wir in unserer endgültigen Labyrinthspezifikation aus, da sie sehr schwer von Kreuzungen zu unterscheiden sind Kreuzung Um den Quellcode der Bewegungs- und Erkennungsalgorithmen für drei Sensoren besser lesbar zu machen, definieren wir in folgender Datei einige Bewegungsmakros für den Roboter. NQC-Code 6: 3sensor/defs.nqc // Sensordefinitionen #define SENS_LEFT SENSOR_1 #define SENS_MID SENSOR_2 #define SENS_RIGHT SENSOR_3 // Motordefinitionen #define ENG_LEFT OUT_A #define ENG_RIGHT OUT_C // Bewegungsmakros #define fwd_until(a) Rev(ENG_LEFT); Fwd(ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); until (a); Off(ENG_LEFT + ENG_RIGHT) #define rev_until(a) Fwd(ENG_LEFT); Rev(ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); until (a); Off(ENG_LEFT + ENG_RIGHT) #define left_until(a) Fwd(ENG_LEFT + ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); until (a); Off(ENG_LEFT + ENG_RIGHT) #define right_until(a) Rev(ENG_LEFT + ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); until (a); Off(ENG_LEFT + ENG_RIGHT) #define fwd_wait(a) Rev(ENG_LEFT); Fwd(ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); Wait (a); Off(ENG_LEFT + ENG_RIGHT) #define rev_wait(a) Fwd(ENG_LEFT); Rev(ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); Wait (a); Off(ENG_LEFT + ENG_RIGHT) #define left_wait(a) Fwd(ENG_LEFT + ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); 22

23 5 Bewegungsalgorithmen Wait (a); Off(ENG_LEFT + ENG_RIGHT) #define right_wait(a) Rev(ENG_LEFT + ENG_RIGHT); On(ENG_LEFT + ENG_RIGHT); Wait (a); Off(ENG_LEFT + ENG_RIGHT) // Der Schwellwert fuer die Sensoren #define WHITE 37 // Ein "No-Operation" Befehl #define NOP Rev(OUT_B) Makro fwd_until(a) rev_until(a) left_until(a) right_until(a) fwd_wait(a) rev_wait(a) left_wait(a) right_wait(a) Aktion Der Roboter fährt vorwärts bis Bedingung a eintritt. Der Roboter fährt rückwärts bis Bedingung a eintritt. Der Roboter dreht sich nach links bis Bedingung a eintritt. Der Roboter dreht sich nach rechts bis Bedingung a eintritt. Der Roboter fährt für a Hundertstel Sekunden forwärts. Der Roboter fährt für a Hundertstel Sekunden rückwärts. Der Roboter dreht sich a Hundertstel Sekunden nach links. Der Roboter dreht sich a Hundertstel Sekunden nach rechts. Abbildung 5.2: Makros zur Robotersteuerung. Abbildung 5.2 erläutert die Funktionen der Makros. Ist unser Roboter mit drei Sensoren unterwegs, so kann er X- und T-Kreuzungen erkennen und diese unterscheiden. 3 Um alle T-Kreuzungen bzw. alle X-Kreuzungen in jeweils einer Routine bearbeiten zu können, legen wir fest, dass im Erkennungsalgorithmus sichergestellt werden muss, dass sich der mittlere Sensor zwischen den beiden möglichen Wegen (T-Kreuzung) bzw. auf dem mittleren der drei Wege (X-Kreuzung) befindet, bevor die Kreuzungsroutine aufgerufen wird. Wert Aktion an nächster X-Kreuzung Aktion an nächster T-Kreuzung 2 Den rechten Weg nehmen. 1 Den mittleren Weg nehmen. Den rechten Weg nehmen. 0 Den linken Weg nehmen. Den linken Weg nehmen. Abbildung 5.3: Mögliche Werte für die Variable direction. Wir deklarieren weiterhin eine Variable direction, in der gespeichert wird, in welche Richtung der Roboter an der nächsten Kreuzung fahren soll. Mögliche Werte hierfür sind in Abbildung 5.3 dargestellt. Mit dem folgenden Code schlägt der Roboter die vorgegebene Richtung ein. Dazu macht er nichts weiter als sich so lange in die entsprechende Richtung zu drehen bis der mittlere Sensor (erneut) Weiß zeigt. NQC-Code 7: 3sensor/cr handling.nqc 3 vgl. Abschnitt

24 5 Bewegungsalgorithmen /* 3sensor/cr_handling.nqc * * Methoden zur Krezungshandhabung * */ int direction;... void xcross() { // Die Richtung die der Roboter an der // naechsten Kreuzung einschlagen soll // X-Kreuzung... if (direction==2) { // rechten Weg nehmen right_until (SENS_RIGHT>=WHITE); // rechtsdrehen bis rechter sensor weiss right_until ( SENS_MID>=WHITE); // rechtsdrehen bis mittlerer sensor weiss if (direction==1) { if (direction==0) { left_until (SENS_LEFT>=WHITE); left_until ( SENS_MID>=WHITE); // mittleren Weg nehmen // wir stehen schon richtig. // linken Weg nehmen // linksdrehen bis rechter sensor weiß // linksdrehen bis mittlerer sensor weiß void tcross() { // T-Kreuzung... if (direction>0) { right_until (SENS_MID>=WHITE); if (direction==0) { left_until(sens_mid>=white); // rechten Weg nehmen // linken Weg nehmen 24

25 6 Erkennungsalgorithmen 6 Erkennungsalgorithmen Auch in diesem Abschnitt werden wir die Fälle zwei Sensoren und drei Sensoren getrennt betrachten, da diese in den Erkennungsmöglichkeiten und -methoden große Unterschiede aufweisen. 6.1 zwei Sensoren Zwar gelingt es uns mit relativ einfachen Methoden einer beliebigen Linie zu folgen, 4 jedoch gestaltet sich die Krezungserkennung mit zwei Sensoren ungleich schwieriger Kreuzung Mit dem folgenden Programm versuchen wir Kreuzungen mit zwei Sensoren zu erkennen. NQC-Code 8: 2sensor/cross.nqc /* 2sensor/cross.nqc * * Der Roboter faehrt eine Linie entlang wie in 2sensor/line.nqc, wenn * beide Sensoren Weiss zeigen wird angenommen dass wir uns auf einer * Kreuzung befinden und ein Ton wird ausgegeben. * */ #include "kurven.nqc" int crossing=0; Die Variable crossing wird auf 1 gesetzt, sobald der Roboter eine Kreuzung erkennt und dann nach zwei Sekunden wieder auf 0 gesetzt. Dies dient dazu eine Kreuzung nicht doppelt zu erkennen: Wir nehmen an, dass der Roboter mindestens zwei Sekunden braucht, um von einer Kreuzung zur nächsten zu gelangen. task main() { // Sensortypen setzen SetSensor(SENS_LEFT, SENSOR_LIGHT); SetSensor(SENS_RIGHT, SENSOR_LIGHT); Jetzt beginnt die Hauptschleife des Programms, die endlos durchlaufen wird und auf die Sensorwerte reagiert: while(1) { linkskurve(); // Die Hauptschleife. Der folgende Teil ist gegenüber dem Programm aus dem letzten Abschnitt neu hinzugekommen. Wir überprüfen, ob beide Sensoren auf Weiß stehen. Ist dies der Fall und die Variable crossing noch 0 (der Roboter hat also in den letzten zwei Sekunden nicht schon eine Kreuzung erkannt), geben wir einen Ton aus. 4 vgl. Abschnitt

26 6 Erkennungsalgorithmen // Die Kreuzungsroutine. if ((SENS_LEFT>=WHITE) && (SENS_RIGHT>=WHITE)) { if (crossing==0) { // Kreuzung erkannt. PlayTone(440,3); Anschließend nehmen wir den rechten Weg, wie in Abschnitt beschrieben. On(ENG_LEFT + ENG_RIGHT); SetPower(ENG_LEFT,5); SetPower(ENG_RIGHT,5); Fwd(ENG_LEFT); Fwd(ENG_RIGHT); until(sens_left<= WHITE); until(sens_left>= WHITE); Off(ENG_LEFT + ENG_RIGHT); // Ende Kreuzungsroutine. Wenn wir gerade eine Kreuzung passiert haben wird die folgende Rechtskurve sofort abgebrochen, da sich der linke Sensor schon über einer weißen Linie befindet und somit SENS_LEFT>=WHITE ist. Wenn nicht steht der rechte Lichtsensor jetzt auf der weißen Linie und der Roboter fährt die nötige Rechtskurve, um der Linie zu folgen. rechtskurve(); // Ende Hauptschleife. //Ende Task Das folgende Task wird beim ersten Erkennen einer Kreuzung aufgerufen und sorgt dafür, dass crossing für zwei Sekunden 1 ist und dann auf 0 gesetzt wird. task delay() { crossing=1; Wait(200); crossing=0; Dieses Programm erkennt ca. 70% der Kreuzungen in einem Labyrinth. Das Problem besteht darin, dass, während der Roboter eine Kurve fährt, der innere Lichtsensor nicht überprüft wird. So ist es möglich, dass zum Beispiel in einer Linkskurve der linke Sensor eine Abzweigung passiert, der rechte Sensor sich währenddessen aber ausschließlich über der schwarzen Tafelfläche befindet, sodass die Kreuzung übergangen wird. Unsere Bemühungen dieses Problem zu beheben hatten leider keinen Erfolg. Es ist zwar möglich in einer Kurve auch den inneren Sensor abzufragen, jedoch muss man dabei bedenken dass der innere Sensor zu Beginn der Kurve schon über einer weißen Linie steht. Ein weiterer Ansatz ist, den inneren Sensor erst nach einer festgelegten Zeit abzufragen. Hier tritt allerdings das Problem auf, dass der innere Sensor unter Umständen eine längere Zeit über der weißen Linie bleibt, zum Beispiel in einer differenzierbaren Kurve, oder dass er erneut über die weiße Linie fährt. 26

27 6 Erkennungsalgorithmen Abbildung 6.1: Der Roboter übergeht eine Kreuzung Sackgasse Die Erkennung einer Sackgasse gestaltet sich in unserem Programm sehr einfach, wenn man bedenkt, dass der Roboter an einer Sackgasse eine sehr große Kurve fahren wird, bis sich der äußere Sensor wieder über der weißen Linie befindet, von der der Roboter gekommen ist. Wir können also ab einem gewissen Zeitpunkt annehmen dass sich der Roboter gerade an einer Sackgasse befindet. Folgender Code realisiert diese Erkennung: NQC-Code 9: Sackgassenerkennung (zwei Sensoren) int sackgasse; // globale Variable task sg() { sackgasse=0; Wait(150); // nach 1,5 Sekunden wird angenommen, // dass wir uns an einer Sackgasse befinden sackgasse=1; In unseren Kurvenalgorithmus fügen wir folgende Zeilen ein: NQC-Code 10: Kurve mit Sackgassenerkennung (zwei Sensoren) //LINKSKURVE On(ENG_LEFT + ENG_RIGHT); SetPower(ENG_LEFT,2); SetPower(ENG_RIGHT,7); Fwd(ENG_LEFT); Fwd(ENG_RIGHT); start sg; // Sackgassen-Check until(sens_left>=white); 27

28 6 Erkennungsalgorithmen Off(ENG_LEFT + ENG_RIGHT); if (sackgasse==1) PlayTone(550,10); //Hier wurde eine Sackgasse erkannt Voraussetzung für dieses Verfahren ist, dass sich in der Umgebung der Sackgasse keine weiteren Wege befinden, über die der Roboter versehentlich fahren könnte. 6.2 drei Sensoren In der Verwendung von drei Sensoren mit zwei verschiedenen Zuständen (weiß und schwarz) ergeben sich 2 3 = 8 verschiedene Eingangsmöglichkeiten. Erkennt das Programm eine dieser Eingangsmöglichkeiten versucht es daraus abzuleiten, welche Labyrinthstruktur sich gerade unter dem Roboter befindet Kreuzung Im Folgenden beschreiben wir welche Möglichkeiten sich bei den verschiedenen Eingangszuständen (in der Reihenfolge linker Sensor-mittlerer Sensor-rechter Sensor) für den gerade befahrenen Teil des Labyrinths ergeben. schwarz-weiß-schwarz In diesem Fall gehen wir davon aus, dass sich der Roboter auf einem Weg befindet. (Abbildung 6.2) Abbildung 6.2: Der Roboter ist auf dem richtigen Weg. schwarz-schwarz-schwarz Hier ergeben sich zwei Möglichkeiten: Entweder der Roboter ist nach links oder rechts vom Weg abgewichen (aber nicht so weit, dass der äußere Sensor schon auf der weißen Linie ist) oder er befindet sich an einer Sackgasse (siehe nächster Abschnitt). (Abbildung 6.3) a) b) Abbildung 6.3: a) Der Roboter ist an einer Sackgasse angelangt. b) Der Roboter ist vom Weg abgekommen. 28

29 6 Erkennungsalgorithmen schwarz-schwarz-weiß Der Roboter ist vom Weg abgekommen. Der rechte Sensor befindet sich entweder über dem Weg oder über einer Abzweigung. (Abbildung 6.4) a) b) Abbildung 6.4: a) Der Roboter ist vom Weg abgekommen. b) Der Roboter ist an einer Kreuzung vom Weg abgekommen. weiß-schwarz-schwarz Der Roboter ist vom Weg abgekommen. Der linke Sensor befindet sich entweder über dem Weg oder über einer Abzweigung. weiß-schwarz-weiß Der Roboter befindet sich an einer T-Kreuzung. Der rechte Sensor erkennt den rechten Weg, der linke den linken. Dies kann keine X-Kreuzung sein, da alle Abzweigungen einen Winkel von 90 haben. Der mittlere Sensor müsste also auch Weiß anzeigen. (Abbildung 6.5) Abbildung 6.5: Der Roboter fährt auf eine Kreuzung. weiß-weiß-weiß Diese Eingangskombination deutet darauf hin, dass sich der Roboter entweder an einer X-Kreuzung, an einer T-Kreuzung oder am Ziel befindet. (Abbildung 6.6) a) b) Abbildung 6.6: a) Der Roboter befährt eine T-Kreuzung. b) Der Roboter ist an einer X- Kreuzung angekommen. 29

30 6 Erkennungsalgorithmen weiß-weiß-schwarz Der Roboter befindet sich entweder an einer Abzweigung (der linke Sensor auf dem linken Weg, der mittlere Sensor auf dem rechten Weg), an einem Weg (beide Sensoren auf dem selben Weg), an einer X-Kreuzung (der rechte Sensor befindet sich dann direkt neben einer der Abzweigungen) oder am Ziel (der rechte Sensor befindet sich kurz vor der Zielfläche). (Abbildung 6.7) a) b) c) Abbildung 6.7: a) Der Roboter befindet sich an einer T-Kreuzung. b) Der Roboter fährt auf einem Weg. c) Der Roboter ist an einer X-Kreuzung angekommen. schwarz-weiß-weiß Der Roboter befindet sich entweder an einer Abzweigung (der mittlere Sensor auf dem linken Weg, der rechte Sensor auf dem rechten Weg), an einem Weg (beide Sensoren auf dem selben Weg), an einer X-Kreuzung (der linke Sensor befindet sich dann direkt neben einer der Abzweigungen) oder am Ziel (der linke Sensor befindet sich kurz vor der Zielfläche). Wie wir diese Möglichkeiten unterscheiden, behandeln wir anhand des folgenden Quellcodes. NQC-Code 11: 3sensor/crossing.nqc /* * 3sensor/crossing.nqc * * v1.0 (Kreuzungserkennung, Sackgassenerkennung, Erkennung des Labyrinthendes, * Speicherung der Kreuzungen) * * Der Roboter mit 3 Sensoren soll moeglichst genau eine Linie abfahren und dabei * Kreuzungen erkennen * */ #include "defs.nqc" #include "stack.nqc" #include "cr_handling.nqc" int timer; // Die Makros // Der Stack // Die Kreuzungsbehandlung // Der Timer zur Sackgassenerkennung task main() { way_back=0; // Diese Variable ist auf dem Rueckweg 1 sackgasse=0; // Diese Variable ist 1 wenn wir die Kreuzung // schon befahren haben 30

31 6 Erkennungsalgorithmen Die gerade initialisierten Variablen werden in Abschnitt 8 besprochen. //Set Sensor Types SetSensor (SENS_LEFT, SENSOR_LIGHT); SetSensor (SENS_MID, SENSOR_LIGHT); SetSensor (SENS_RIGHT, SENSOR_LIGHT); //Set Motor Power SetPower (ENG_LEFT, 4); SetPower (ENG_RIGHT, 5); while (1) { // Die Hauptschleife /***************************** ** schwarz-schwarz-schwarz ** *****************************/ if (SENS_LEFT < WHITE && SENS_MID < WHITE && SENS_RIGHT < WHITE) { Hier zeigen alle Sensoren Schwarz an. Der Roboter dreht sich nach links, bis der mittlere oder der rechte Sensor Weiß anzeigt. Wenn dieser Vorgang länger als eine halbe Sekunde dauert, gehen wir davon aus, dass der Roboter gerade eine Sackgasse passiert hat (siehe nächster Abschnitt). start TimeIt; // Sackgassenerkennung left_until (SENS_MID >= WHITE SENS_RIGHT >= WHITE); stop TimeIt; if (timer > 5) // Sackgassenerkennung { sackg (); // Sackgassenerkennung /***************************** ** schwarz-schwarz-weiss ** *****************************/ if (SENS_LEFT < WHITE && SENS_MID < WHITE && SENS_RIGHT >= WHITE) { Im Folgenden befindet sich nur der rechte Sensor auf einer Kreidelinie. Wir drehen den Roboter nach rechts, bis der mittlere Sensor wieder Weiß anzeigt. Ob wir uns an einer Kreuzung befinden oder nicht, wird jetzt durch die anderen Fälle geklärt. right_until (SENS_MID >= WHITE); /***************************** ** schwarz-weiss-schwarz ** 31

32 6 Erkennungsalgorithmen *****************************/ if (SENS_LEFT < WHITE && SENS_MID >= WHITE && SENS_RIGHT < WHITE) { Wenn nur der mittlere Sensor Weiß anzeigt, kann der Roboter weiterfahren, bis sich dieser Zustand ändert: fwd_until (SENS_MID < WHITE SENS_LEFT >= WHITE SENS_RIGHT >= WHITE); /***************************** ** schwarz-weiss-weiss ** *****************************/ if (SENS_LEFT < WHITE && SENS_MID >= WHITE && SENS_RIGHT >= WHITE) { Der folgende Fall ist der komplizierteste. Der mittlere und der rechte Sensor zeigen Weiß an, der linke zeigt Schwarz an. Wir müssen nun herausfinden ob wir uns an einem Weg, an einer T-Kreuzung, an einer X-Kreuzung oder am Ziel befinden. Dazu fahren wir vorwärts, bis der linke Sensor auch Weiß anzeigt oder der rechte Sensor Schwarz anzeigt. Sollte der linke Sensor Weiß anzeigen, so gilt dies nun für alle Sensoren und wir können zum Fall weiß-weiß-weiß übergehen. fwd_until (SENS_RIGHT < WHITE SENS_LEFT >= WHITE); if (SENS_LEFT >= WHITE) goto CASE111; Wenn nicht, befinden wir uns entweder an einer nicht differenzierbaren Kurve oder an einer T-Kreuzung. Wenn wir uns an einer T-Kreuzung befinden, sind wir gerade mit dem rechten Sensor über die Abzweigung hinaus gefahren, sollten allerdings mit dem mittleren Sensor noch auf der Linie stehen. Wenn wir uns an einer Kurve befinden, ist der mittlere Sensor entweder schon auf der Tafelfläche, oder steht auf der Ecke der Kurve. Wir fahren also ein kleines Stück vor und fragen den mittleren Sensor ab. Wenn dieser immernoch Weiß anzeigt, befanden wir uns an einer Kreuzung und sind die Linie entlang gefahren. fwd_wait (60); if (SENS_MID >= WHITE) // T-Kreuzung { right_until (SENS_MID < WHITE); tcross (); goto END011; Wenn nicht, befanden wir uns entweder an einer Kurve und sind darüber hinausgefahren oder wir befanden uns an einer Kreuzung und der mittlere Sensor ist beim Vorwärtsfahren von der Linie abgekommen. Wir drehen den Roboter also eine halbe Sekunde nach links, um herauszufinden, ob ein Weg vorhanden ist. 32

33 6 Erkennungsalgorithmen start TimeIt; left_until (SENS_MID >= WHITE timer > 5 SENS_RIGHT >= WHITE); stop TimeIt; Wenn der mittlere oder rechte Sensor Weiß anzeigt, haben wir einen Weg gefunden. Wir befanden uns also an einer Kreuzung und rufen die entsprechende Routine auf. if (SENS_MID >= WHITE SENS_RIGHT >= WHITE) { if (SENS_RIGHT >= WHITE) right_until (SENS_MID >= WHITE); right_until (SENS_MID < WHITE); tcross (); goto END011; Wenn nicht, befinden wir uns entweder ganz links vom Weg oder er existiert gar nicht und wir befinden uns an einer nicht differenzierbaren Kurve. Wir drehen den Roboter also nach rechts. Wenn der mittlere Sensor zuerst einen Weg erkennt, befand sich dieser vorher zwischen mittlerem und rechtem Sensor, also links von der Abzweigung. Wir haben somit eine Kurve ausgeschlossen. Wenn der rechte Sensor zuerst einen Weg erkennt gehen wir davon aus dass wir eine Kurve gefahren sind. right_until (SENS_MID >= WHITE SENS_RIGHT >= WHITE); if (SENS_MID >= WHITE) { right_until (SENS_MID < WHITE); tcross (); goto END011; END011:NOP; //No Operation. Dies erlaubt Spruenge an diese Stelle. /***************************** ** weiss-schwarz-schwarz ** *****************************/ if (SENS_LEFT >= WHITE && SENS_MID < WHITE && SENS_RIGHT < WHITE) { Hier befindet sich nur der linke Sensor auf einer Kreidelinie. Wie im umgekehrten Fall drehen wir den Roboter nach rechts, bis der mittlere Sensor wieder Weiß anzeigt. Ob wir uns an einer Kreuzung befinden oder nicht, wird durch die anderen Fälle geklärt. left_until (SENS_MID >= WHITE); /***************************** ** weiss-schwarz-weiss ** *****************************/ 33

34 6 Erkennungsalgorithmen { if (SENS_LEFT >= WHITE && SENS_MID < WHITE && SENS_RIGHT >= WHITE) In diesem Fall können wir davon ausgehen, dass wir uns an einer T-Kreuzung befinden. tcross (); /***************************** ** weiss-weiss-schwarz ** *****************************/ if (SENS_LEFT >= WHITE && SENS_MID >= WHITE && SENS_RIGHT < WHITE) { Dieser Fall funktioniert genau wie der Fall schwarz-weiß-weiß und wird hier nicht mehr näher erläutert. fwd_until (SENS_LEFT < WHITE SENS_RIGHT >= WHITE); if (SENS_RIGHT >= WHITE) goto CASE111; fwd_wait (60); if (SENS_MID >= WHITE) // T-Kreuzung { left_until (SENS_MID < WHITE); tcross (); goto END110; start TimeIt; right_until (SENS_MID >= WHITE timer > 5 SENS_LEFT >= WHITE); stop TimeIt; if (SENS_MID >= WHITE SENS_LEFT >= WHITE) // T-Kreuzung { if (SENS_LEFT >= WHITE) left_until (SENS_MID >= WHITE); left_until (SENS_MID < WHITE); tcross (); goto END110; left_until (SENS_MID >= WHITE SENS_LEFT >= WHITE); if (SENS_MID >= WHITE) // T-Kreuzung { right_until (SENS_MID < WHITE); tcross (); goto END110; END110:NOP; 34

35 6 Erkennungsalgorithmen /***************************** ** weiss-weiss-weiss ** *****************************/ if (SENS_LEFT >= WHITE && SENS_MID >= WHITE && SENS_RIGHT >= WHITE) { Zeigen alle drei Sensoren Weiß, befinden wir uns entweder an einer T-Kreuzung, an einer X-Kreuzung oder am Ziel. Um das herauszufinden, lassen wir den Roboter geradeaus fahren bis der linke und der rechte Sensor Schwarz anzeigen. Wenn das länger als eine halbe Sekunde dauert, gehen wir davon aus, dass wir gerade über die Zielfläche gefahren sind und rufen die entsprechende Routine auf. CASE111:start TimeIt; fwd_until (SENS_LEFT < WHITE && SENS_RIGHT < WHITE); stop TimeIt; if (timer > 5) { finish (); goto END111; Wenn nicht, müssen wir herausfinden, ob der Roboter gerade eine T-Kreuzung oder eine X- Kreuzung befährt. Dazu lassen wir den Roboter ein Stück vorwärts fahren und ihn dann nach dem mittleren Weg suchen. Dazu drehen wir den Roboter nach links, bis einer der drei Sensoren Weiß anzeigt. fwd_wait (10); if (SENS_MID >= WHITE) goto XCROSS; //drehe links bis links auf weiss oder rechts auf weiss left_until (SENS_LEFT >= WHITE SENS_MID >= WHITE SENS_RIGHT >= WHITE); Zeigt der rechte oder der mittlere Sensor Weiß an, haben wir den mittleren Weg gefunden und können die X-Kreuzungsroutine aufrufen. if (SENS_RIGHT >= WHITE) { right_until (SENS_MID >= WHITE); if (SENS_MID >= WHITE) goto XCROSS; Signalisiert nur der linke Sensor Weiß, drehen wir den Roboter nach rechts, bis der rechte oder der mittlere Sensor eine Kreidelinie erkennt. Ist es der mittlere, so haben wir den mittleren Weg der Kreuzung gefunden und rufen die entsprechende Methode auf. Wenn nicht, befinden wir uns an einer T-Kreuzung. right_until (SENS_RIGHT >= WHITE SENS_MID >= WHITE); if (SENS_MID < WHITE) 35

36 6 Erkennungsalgorithmen goto TCROSS; //wenn mittlerer sensor weiß gesehen hat haben wir eine X-Kreuzung, //wenn nicht eine T-Kreuzung XCROSS:xcross (); goto END111; TCROSS:tcross (); END111:NOP; //No Operation //End of Main Loop Das Task TimeIt erhöht die Variable timer alle 0,1 Sekunden um 1. Es wird benutzt, um eine zeitliche Abbruchbedingung in eine vom Sensorwert abhängige Bewegung einzubauen. task TimeIt () { timer = 0; while (1) { Wait (10); timer++; Sackgasse Eine notwendige, aber nicht hinreichende Bedingung dafür, dass sich der Roboter an einer Sackgasse befindet, ist, dass alle drei Sensoren Schwarz signalisieren. Ist dies der Fall, sucht der Roboter nach dem Weg, indem er sich hin und her dreht und dabei die Sensorwerte überprüft. Hat sich der Roboter für eine bestimmte Zeit in eine Richtung gedreht, lässt sich davon ausgehen, dass er um annähernd 180 gewendet hat und wieder den Weg erkennt, von dem er gekommen ist. Er ist also an einer Sackgasse angelangt und tritt jetzt den Rückweg an Ziel Kommt der Roboter am Ziel an, signalisieren früher oder später alle drei Sensoren Weiß. Ist dies der Fall, müssen wir ausschließen, dass wir fälschlicherweise eine Kreuzung als Ziel erkennen. Dazu lassen wir den Roboter vorwärts fahren, bis die beiden äußeren Sensoren Schwarz erkennen. Da dies an einer Kreuzung nicht sehr lange dauern kann (der Roboter muss nur über die Abzweigungen hinausfahren), können wir nach einer bestimmten Zeit davon ausgehen, dass das Ziel erreicht wurde und eine entsprechende Routine aufrufen. 36

37 7 Suchalgorithmen 7 Suchalgorithmen In den vorangegangenen Abschnitten haben wir unser Labyrinth ausschließlich als Kreidelinie auf der Tafelfläche betrachtet. Es ist für den Suchalgorithmus jedoch nicht von Belang, wie viele Kurven der Roboter auf dem Weg zum Ziel fährt oder wie oft er dabei vom Weg abkommt. Dementsprechend wenden wir uns jetzt einer abstrakteren Betrachtungsweise zu. Ein, nach unseren Spezifikationen erstelltes, Labyrinth lässt sich auf folgende fünf Strukturen reduzieren: T-Kreuzungen X-Kreuzungen Sackgassen Ziel Wegen zwischen Kreuzungen, Sackgassen und Ziel Betrachtet man ein Labyrinth im Hinblick auf diese fünf Objekte, erkennt man sehr leicht, dass es sich als Baumstruktur, relativ zur Startposition des Roboters, darstellen lässt. 5 Die T- und X-Kreuzungen stellen hierbei die Knoten des Baumes dar, die Sackgassen und das Ziel sind die Blätter und die Wege dazwischen sind die Kanten des Baumes. Abbildung 7.1 zeigt das Labyrinth aus Abbildung 1.1 schematisch und als Baum. a) b) Abbildung 7.1: a) Das Labyrinth aus Abb.1.1 schematisch. b) Das Labyrinth aus Abb 1.1 als Baum. Mit Hilfe dieser Abstraktion ist es uns möglich, bekannte Suchalgorithmen für Bäume auf unser Labyrinth anzuwenden. Im Folgenden werden wir die zwei gebräuchlichsten vorstellen und darlegen, warum wir uns für den ersten der beiden entscheiden. 5 vgl. [21], Kap. 9: Trees 37

38 7 Suchalgorithmen 7.1 Tiefensuche (Depth-First-Search) Die Tiefensuche (auch Depth-First-Search oder Backtracking genannt) ist ein rekursiver Algorithmus, um eine Baumstruktur nach einem bestimmten Element zu durchsuchen. Um die Tiefensuche auf einen Baum anzuwenden, geht man folgendermaßen vor: Man beginnt mit dem Wurzelknoten. Ist dieser der gesuchte Knoten, ist man fertig. Ist es nicht der gesuchte Knoten, wendet man das Verfahren auf alle Unterbäume des Wurzelknotens an. Der Algorithmus terminiert, wenn der gesuchte Knoten gefunden wurde oder wenn alle Knoten durchsucht wurden. Abbildung 7.2 illustriert die Tiefensuche am Labyrinth aus Abbildung 7.1. Man erkennt sehr gut, dass der Roboter an jeder Kreuzung den linken Weg einschlagen muss, um eine Tiefensuche durchzuführen. Insofern ist dieser Algorithmus trivial zu implementieren. a) b) Abbildung 7.2: a) Die Tiefensuche am Labyrinth. b) Die Tiefensuche am Baum. In unserem endgültigen Steuerprogramm werden wir an jeder Kreuzung den rechten Weg nehmen. Ebensogut könnte man den linken Weg nehmen. Beide Methoden funktionieren (im Durchschnitt) gleich gut. 7.2 Breitensuche (Breadth-First-Search) Die Breitensuche (oder Breadth-First-Search) auf einem Baum läßt sich folgendermaßen beschreiben: Es existiert eine Warteschlange von Knoten, die sequentiell überprüft wird. Als erstes wird der Wurzelknoten in der Warteschlange gespeichert. Wenn ein Knoten überprüft wird werden alle seine Kinderknoten ans Ende der Warteschlange gestellt. Der Algorithmus terminiert wenn das Ziel gefunden wurde oder wenn die Warteschlange keine Knoten mehr enthält. Abbildung 7.3 zeigt, wie die Breitensuche im Labyrinth aus Abbildung 7.1 funktioniert. Man erkennt, dass der Steueraufwand für den Roboter ungleich höher ist als bei der Tiefensuche. 7.3 Fazit Da wir bei Beginn unserer Suche keine Aussage darüber treffen können, wo im Labyrinth/Baum sich unser Ziel befindet, müssen beide Algorithmen im Durchschnitt gleich viele Knoten 38

39 7 Suchalgorithmen a) b) Abbildung 7.3: a) Die Breitensuche am Labyrinth. b) Die Breitensuche am Baum. Die gepunkteten Linien stehen für Wege, die nicht mehr gegangen werden, da das Ziel schon gefunden wurde. Sie entsprechen aber der konsequenten Ausführung des Algorithmus entsprechen. durchsuchen, um es zu finden. Allerdings ist die Tiefensuche in unserem Labyrinth erheblich einfacher zu implementieren als die Breitensuche, weshalb wir diese in unserem Programm verwenden. 39

40 8 Speicherung des Labyrinths 8 Speicherung des Labyrinths Für die Realisierung der Vorgabe kehre, nachdem das Ziel erreicht wurde, auf schnellstem Weg zum Ausgangspunkt zurück ist es nötig, dem Roboter ein Gedächtnis zu geben, in dem er sich den Aufbau des Labyrinths merkt. In den folgenden Abschnitten besprechen wir, wie dieses Gedächtnis von uns realisiert und implementiert wird. Im letzten Abschnitt des Kapitels erläutern wir, wie der Roboter die Informationen im Gedächtnis nutzt, um ohne Umwege vom Ziel zurück zum Ausgangspunkt zu finden. 8.1 Speichern einer Kreuzung Angenommen unser Roboter befindet sich auf dem Rückweg vom Ziel zum Ausgangspunkt. Wenn er auf eine Kreuzung trifft, weiß er a priori nicht, welche der Abzweigungen zum Ausgangspunkt führt und welche zu Sackgassen. Oder, wenn wir das Labyrinth als Baum betrachten, weiß er nicht welche Kante nach oben, zum Elternknoten führt und welche zu weiteren Kindknoten. Abbildung 8.1 illustriert dieses Problem. a) b) Abbildung 8.1: a) Der Roboter weiß nicht, welcher Weg zum Ausgangspunkt zurück führt, beziehungsweise b) welche Kante zum Wurzelknoten führt. Der Roboter muss sich also für jede Kreuzung, die auf dem Weg zum Ausgangspunkt liegt, merken, in welcher Richtung sich der Ausgangspunkt befindet. Dazu legen wir eine Integer- Variable im RCX an und speichern darin, wie oft der Roboter schon an diese Kreuzung zurückgekehrt ist. Da der Roboter auf dem Weg zum Ziel immer den rechten Weg nimmt, legt diese Variable eindeutig fest, wie der Roboter vom aktuellen Pfad zurück zum Ausgangspunkt gelangt. Beispiel: Der Roboter befindet sich auf dem Rückweg vom Ziel und erkennt eine X-Kreuzung. Die entsprechende Variable hat den Wert 2. Das heißt auf dem Weg zum Ziel hat der Roboter an dieser Kreuzung zuerst den rechten Weg genommen und die Variable auf 0 gesetzt, ist dann zurückgekehrt und hat (relativ zur Startposition) den mittleren Weg genommen und die Variable auf 1 gesetzt. Auch auf diesem Pfad hat er das Ziel nicht gefunden, sondern ist zur Kreuzung zurückgekehrt, hat die Variable auf 2 gesetzt und den (vom Ausgangspunkt aus gesehen) linken Weg genommen. Der Roboter weiß also, dass er den rechten Weg nehmen muss, um zum Ausgangspunkt zu finden. Abbildung 8.1 zeigt eine grafische Darstellung dieses Beispiels. Ist der Roboter zum dritten Mal zu einer X-Kreuzung oder zum zweiten Mal zu einer T- Kreuzung zurückgekehrt, so kann diese nicht auf dem direkten Weg vom Ausgangspunkt zum 40

41 8 Speicherung des Labyrinths a) b) c) d) Abbildung 8.2: a) Der Roboter kommt zum ersten Mal an die Kreuzung und nimmt den rechten Weg. Die Kreuzungsvariable wird auf 0 gesetzt. b) Bei der ersten Rückkehr zur Kreuzung wird die Kreuzungsvariable auf 1 gesetzt. Der Roboter befährt jetzt den Weg gegenüber dem Weg zum Ausgangspunkt. c) Bei der zweiten Rückkehr wird die Kreuzungsvariable auf 2 gesetzt. Der vom Ausgangspunkt aus linke Weg wird jetzt befahren. d) Der Roboter hat das Ziel gefunden und kehrt zur Kreuzung zurück. Anhand der Kreuzungsvariable erkennt er, dass er den rechten Weg nehmen muss, um zum Ausgangspunkt zu gelangen. Ziel liegen. Deshalb kann die entsprechende Variable gelöscht werden, um Speicherplatz zu sparen. 8.2 Speichern mehrer Kreuzungen auf dem Stack In diesem Abschnitt beschäftigen wir uns damit, wie der Roboter die Kreuzungsvariablen so speichert, dass sie beim Rückweg ohne großen Aufwand jeweils zur passenden Kreuzung aufgerufen werden können. Dazu besprechen wir zuerst das Konzept eines Stack. Ein Stack ist eine Struktur, die dazu dient, mehrere Datenwörter des gleichen Typs zu speichern. Dabei werden die Daten wie auf einem Stapel (deutsch für Stack) übereinandergelegt. Auf dem Stack sind (im Allgemeinen) nur zwei Operationen definiert: push und pop. Der Befehl push legt ein Datenwort auf den Stapel. Die Anweisung pop nimmt das oberste Datenwort vom Stack und stellt es zur Verfügung. Eine formale Definition und ein ausführliches Beispiel für einen Stack finden sich in [25], Kap Angenommen wir hätten auf dem Weg zum Ziel alle benötigten Kreuzungsvariablen in der richtigen Reihenfolge auf dem Stack gespeichert, dann könnten wir diese auf dem Rückweg nacheinander vom Stack herunternehmen, um zum Ausgangspunkt zurück zu finden. Dies zu realisieren ist nicht weiter schwer. Sobald der Roboter an eine Kreuzung kommt, die er noch nicht befahren hat, legt er eine Kreuzungsvariable mit dem Wert 0 an, legt diese auf den Stack und nimmt den rechten Weg. Kommt er an eine Sackgasse, oder ist er zum dritten Mal zu einer X-Kreuzung beziehungsweise zum zweiten Mal zu einer T-Kreuzung zurückgekehrt, wird die nächste Kreuzung, auf die er trifft, eine schon befahrene sein. Also nimmt der Roboter das oberste Element vom Stapel, erhöht es um 1 und schreibt es zurück auf den Stack, außer die erkannte Kreuzung ist eine T-Kreuzung und die Variable ist vorher schon 1 oder die erkannte Kreuzung ist eine X-Kreuzung und die Variable ist vorher schon 2. In diesem Fall liegt die Kreuzung nicht auf dem direkten Weg zwischen Start und Ziel und kann deshalb vergessen werden. Folgender Code implementiert das Verfahren: NQC-Code 12: 3sensor/cr handling.nqc 41

42 8 Speicherung des Labyrinths /* 3sensor/cr_handling.nqc * * Methoden zur Kreuzungsbewältigung * */ int way_back=0; int direction; int sackgasse=0; // Zeigt an ob Hinweg oder Rueckweg // Zeigt die Richtung an der naechsten Kreuzung an // Zeigt an ob wir von einer Sackgasse kommen Die Variable sackgasse wird auf 1 gesetzt, wenn der Roboter eine Sackgasse passiert oder an einer Kreuzung schon alle Wege befahren hat und jetzt zu einer schon bekannten Kreuzung zurückkehrt. void xcross() { if (way_back==0) { direction=2; if (sackgasse==0) { push(0); else { pop(); if (returnvalue==0) { push(1); sackgasse=0; if (returnvalue==1) { push(2); sackgasse=0; if (returnvalue==2) { sackgasse=1; else { pop(); if (returnvalue==-1) { backatstart(); direction=returnvalue; // Hinweg // rechts fahren // neue Kreuzung -> Speichern // X-Kreuzung -> rechter Weg // alte Kreuzung // wir nehmen die oberste Variable vom Stack // kommen vom rechten Weg // kommen vom mittleren Weg // kommen vom linken Weg // Rueckweg // wir nehmen die oberste Variable vom Stack // der Stack war schon leer, // also sind wir am Ausgangspunkt angekommen... Hier wurden die schon in Abschnitt besprochenen Routinen der Übersichtlichkeit halber weggelassen. 42

43 8 Speicherung des Labyrinths // Ende X-Kreuzung void tcross() { if (way_back==0) { direction=2; if (sackgasse==0) { push(0); else { pop(); if (returnvalue==0) { push(1); sackgasse=0; if (returnvalue==1) { sackgasse=1; else { pop(); if (returnvalue==-1) { backatstart(); direction=returnvalue; // T-Kreuzung // Hinweg // rechts fahren // neue Kreuzung -> Speichern // T-Kreuzung -> rechter Weg // alte Kreuzung // wir nehmen die oberste Variable vom Stack // kommen vom rechten Weg // kommen vom linken Weg // wir nehmen die oberste Variable vom Stack // der Stack war schon leer, // also sind wir am Ausgangspunkt angekommen... Auch hier wurden die Routinen aus Abschnitt der Üersichtlichkeit halber weggelassen. // Ende T-Kreuzung 8.3 Implementierung des Stack Die Implementierung des Stack gestaltet sich recht simpel. Wir allokieren zuerst einen zusammenhängenden Speicherbereich mit Platz für 20 Integer-Variablen, 6 indem wir ein Array dieser Größe anlegen. Desweiteren legen wir die globale Variable stackpointer an, die den momentanen Index des obersten Elements angibt. Ist der Stack leer, soll stackpointer den Wert -1 enthalten. NQC-Code 13: 3sensor/stack.nqc /* 3sensor/stack.nqc * * Der Stack zur Speicherung der Kreuzungen. 6 siehe Abschnitt

44 8 Speicherung des Labyrinths * */ int stackpointer=-1; int stack[20]; int returnvalue; // Der Stackpointer. // Der Datenbereich fuer den Stack // Der Rueckgabewert fuer die pop-anweisung. Jetzt definieren wir die Stackoperationen push und pop: void push(int val) { // Ein Element auf den Stack legen if (stackpointer>=19) return; // Der Stack ist voll! stackpointer++; stack[stackpointer]=val; return; Die Funktion push testet zuerst, ob der Stack schon voll ist. Sollte das der Fall sein so geschieht nichts weiter. Ist der Stack noch nicht voll, wird der Stackpointer um eins erhöht (zeigt jetzt also auf die Speicherstelle über dem bisher obersten Element des Stapels) und die Variable val, die der Funktion übergeben wurde, wird an die Stelle des Stackpointers geschrieben. void pop() { if (stackpointer<0) { returnvalue= -1 ; return; returnvalue=stack[stackpointer]; stackpointer--; return; // Ein Element vom Stack nehmen. // Der Stack ist noch leer. Sollte der Stack leer sein, gibt die Funktion pop den Wert -1 zurück. Ansonsten wird das oberste Element des Stapels zurückgegeben und der Stackpointer um 1 erniedrigt. 8.4 Rückfahrt Sobald der Roboter das Ziel erreicht hat, wird die globale Variable way_back von 0 auf 1 gesetzt. Erkennt der Roboter eine Kreuzung und ist way_back 1, wird das oberste Element vom Stack gelesen und in die direction-variable kopiert. 7 Durch die geschickte Definition der zu direction gehörigen Aktionen 8 wählt der Roboter jetzt die Richtung, die zum Ausgangspunkt führt. 9 7 vgl. Abschnitt Abbildung vgl. auch Abbildung

45 9 Addons 9 Addons 9.1 Start ab beliebigem Startpunkt Bisher war der Startpunkt des Roboters auf einen Punkt am Rande des Labyrinths festgelegt. Der zu erkundende Teil und das Ziel mussten sich vor dem Roboter befinden. In diesem Addon werden wir das Steuerprogramm so verändern, dass unser Roboter von einem beliebigen Punkt im Labyrinth aus das Ziel findet, auch wenn es hinter ihm liegt und anschließend zu diesem Punkt zurückkehrt. Der Roboter muss also nicht nur das vor ihm liegende Labyrinth abfahren, sondern auch den Teil der hinter ihm liegt und sich dabei merken, von welchem Punkt er gestartet ist Suchalgorithmus Bisher hat der Roboter das Labyrinth nach dem Backtracking-Algorithmus durchsucht. 10 Diese Methode ist auch in der neuen Situation praktikabel. Das Labyrinth besteht aus zwei Teilen, dem Bereich der sich zu Anfang vor dem Roboter befindet und dem Bereich hinter dem Roboter, die beide jeweils mit dem Backtracking-Algorithmus durchsucht werden können und durch den Ausgangspunkt miteinander verbunden sind. Wir erhalten eine neue Baumstruktur mit dem Ausgangspunkt als Wurzelknoten. Abbildung 9.1: Die erweiterte Baumstruktur für beliebigen Startpunkt. Hat der Roboter den vorderen Teil des Labyrinths abgesucht, fährt er über den Ausgangspunkt zum hinteren Teil und durchsucht diesen Speicherung des Labyrinths Für die Position des Ziels im Labyrinth ergeben sich zwei Möglichkeiten: Es kann sich vor dem Roboter oder hinter dem Roboter befinden (relativ zur Startposition). Im ersten Fall funktioniert das schon vorhandene Steuerprogramm ohne Änderungen. Der Roboter speichert erkannte Kreuzungen, bis er das Ziel gefunden hat und liest sie auf der 10 vgl. Kap

46 9 Addons Rückfahrt vom Stack, bis dieser leer ist. Der Roboter ist dann am Ausgangspunkt angekommen (beziehungsweise an der dahinterliegenden Kreuzung) und stoppt. Im zweiten Fall erkundet der Roboter den vorderen Teil des Labyrinths ohne das Ziel zu finden, kommt zum Ausgangspunkt zurück und hat den Stack zu diesem Zeitpunkt schon wieder vollständig geleert, da keine bisher befahrene Kreuzung auf dem Weg zum Ziel liegt. Der Roboter kann also einfach die erste Kreuzung im hinteren Teil des Labyrinths befahren und speichern. Ein Problem stellt allerdings die Variable sackgasse dar. Diese wurde an der zuletzt befahrenen Kreuzung im vorderen Teil auf 1 gesetzt, weshalb der Roboter jetzt auf dem leeren Stack nach einer Kreuzungsvariable sucht. 11 Der leere Stack gibt per Definition immer -1 zurück. Da dieser Wert nicht als Kreuzungsvariable verwendet wird und der Stack nur am Ausgangspunkt des Roboters leer ist, können wir also beim Rückgabewert -1 davon ausgehen, dass der Roboter zum ersten Mal an der ersten Kreuzung im hinteren Teil ankommt. Wir nehmen also den rechten Weg, speichern eine 0 auf dem Stack und starten damit die Suche im hinteren Teil. 11 vgl. Kap.8 46

47 9 Addons 9.2 Maximum der Kreuzungsanzahl finden In diesem Addon wollen wir die Beschränkung auf ein Labyrinth mit höchstens 20 Kreuzungen aufheben und herausfinden, wie viele Kreuzungen der Roboter maximal speichern kann. Bisher haben wir für den Stack, auf dem die Kreuzungen gespeichert werden, 20 Integer- Variablen allokiert. Da der RCX auf 32 globale Variablen beschränkt ist (von denen einige schon unabhängig vom Stack verwendet werden) und die Stack-Variablen global gespeichert werden müssen, lässt sich mit einer simplen Vergrößerung des Stacks, durch allokieren weiterer Variablen, kaum eine Verbesserung erreichen. Um signifikant mehr Kreuzungen bearbeiten zu können muss der Roboter also mehr als eine Kreuzungsvariable in einer Integer-Variablen speichern. Das ist möglich, da eine Integer- Variable im RCX 16 Bit breit ist, für eine Kreuzungsvariable allerdings höchstens 2 Bit gebraucht wird. Wir können also maximal 8 Kreuzungsvariablen pro Integer-Variable speichern. Mit der bisherigen Allokationsgröße für den Stack kommen wir damit auf 160 Kreuzungen, die sich der Roboter merken kann Implementation des größeren Stack Folgender Code realisiert einen Stack mit 2-Bit Kreuzungsvariablen: NQC-Code 14: 3sensor/stack.nqc /* 3sensor/stack.nqc * * Der Stack zur Speicherung der Kreuzungen. * */ Die Konstante STACKSIZE definiert die Größe des Stack in Integer-Variablen. STACKMAX ist der größte Index, der noch innerhalb des Stack liegt. #define STACKSIZE 20 #define STACKMAX STACKSIZE*8-1 int stackpointer=-1; // Der Stackpointer. int stack[stacksize]; // Der Datenbereich fuer den Stack int returnvalue; // Der Rueckgabewert fuer die pop-anweisung. void push(int val) { // Ein Element auf den Stack legen if (stackpointer>=stackmax) return; // Der Stack ist voll! stackpointer++; Die Variable i gibt an, in welche Integer-Variable der neue Wert geschrieben werden soll. Diese wird dann bitweise um 2 Stellen nach links verschoben, sodass die beiden rechten Stellen (least significant bits) unbenutzt sind. Der Wert wird durch eine Addition dort gespeichert. int i = stackpointer/8; stack[i] <<= 2; stack[i]+= val; 47

48 9 Addons return; void pop() { if (stackpointer<0) { returnvalue= -1 ; return; // Ein Element vom Stack nehmen. // Der Stack ist noch leer. Die Variable i zeigt an, in welcher Integer-Variable der oberste Stackeintrag liegt. Dieser ist immer in den letzten zwei Bits gespeichert (siehe push-anweisung). Die entsprechende Integer- Variable wird also mit der dezimalen 3 (entspricht einer binären ) bitweise UND-geschaltet, sodass die ersten 14 Bits 0 sind und die letzten beiden ihrem ursprünglichen Wert entsprechen. Wir erhalten den gewünschten Wert. int i=stackpointer/8; returnvalue=(stack[i] & 3); stack[i] >>= 2; stackpointer--; return; Test des größeren Stack Mit folgenden Programm testen wir den größeren Stack: NQC-Code 15: stacktest.nqc /* stacktest.nqc * * Testet den erweiterten Stack. * */ #include "stack.nqc" task main() { int i=0; int j=0; while (i<116) { push(i%4); i++; PlayTone(500,10); i=0; while (i<116) { pop(); SetUserDisplay(returnvalue,0); 48

49 9 Addons Wait(50); i++; Das Programm schreibt eine Reihe von 2-Bit Zahlen auf den Stack, liest sie anschließend wieder aus und zeigt sie im Display des RCX an. Der Test verlief fehlerfrei Probleme Leider konnten wir den größeren Stack nicht in unser Kreuzungsprogramm einbauen. Das Problem liegt im begrenzten Instruktionsspeicher des RCX. Durch den größeren Code in den push- und pop-anweisungen wird das Programm zu groß und NQC gibt beim Versuch, das Programm auf den RCX zu übertragen, eine Fehlermeldung aus. 49

50 Literatur Literatur Kapitel 2: Bausteine [1] Mindstorms RIS 2.0, [2] RCX Internals, kekoa/rcx/. [3] LEGO Mindstorms bei Wikipedia, Mindstorms. [4] Die Pulsweitenmodulation bei Wikipedia, [5] Mindstorm Labyrinthroboter im SS 05, lego2/hardware.html. [6] Mikrocontroller bei Wikipedia, [7] Bus (Datenverarbeitung) bei Wikipedia, %28Datenverarbeitung%29 [8] RCX Manual, Kapitel 3: Roboter [9] Bauanleitung für den Rover, [10] LeoCAD Homepage, [11] Unsere Projekt-Homepage, legolab2/ Kapitel 4: Installation von NQC [12] NQC - Not Quite C, [13] legos/brickos, [14] lejos - Java for the RCX, 50

51 Literatur [15] flex - The Fast Lexical Analyzer, [16] GNU Bison, [17] GNU M4, [18] Koders.com: legousbtower.h, [19] Koders.com: legousbtower.c, [20] LEGO Mindstoms RCX 2 Beta SDK, Kapitel 7: Suchalgorithmen [21] Kenneth H. Rosen: Discrete Mathematics and Its Applications, McGraw-Hill, 2003, ISBN [22] Die Tiefensuche als Algorithmus der Woche im Informatikjahr 2006, algorithmus/algo5.php. [23] Die Tiefensuche bei Wikipedia, [24] Die Breitensuche bei Wikipedia, Kapitel 8: Speicherung des Labyrinths [25] Peter Bastian: Informatik I Programmieren und Softwaretechnik, peter/html/inf1.html. 51

52 Literatur Glossar Deviceknoten Ein Deviceknoten ist eine spezielle Datei im Dateisystem, die die Kommunikation mit einem externen Gerät (wie dem Mindstorms-USB-Tower) ermöglicht. f lex Flex ist ein Werkzeug um lexikalische Scanner zu erzeugen. Es wird zur Installation von NQC benötigt. GN U Bison GNU Bison ist ein Parser-Generator. Ein Parsergenerator erzeugt Unterprogramme zur grammatikalischen Analyse anderer Computerprogramme, Parser genannt. GNU Bison wird zur Installation von NQC benötigt. GN U M 4 GNU M4 ist ein Makroprozessor, also ein Computerprogramm, das Zeichenfolgen innerhalb eines Textes durch andere Zeichenfolgen ersetzt. Es ermöglicht z.b. die Erstellung und Benutzung von wiederverwertbaren Textbausteinen in Textverarbeitungsprogrammen. GNU M4 wird zur Installation von GNU Bison benötigt. Kernel Der Kernel ist der zentrale Bestandteil eines Betriebssystems. In ihm ist die Prozessund Datenorganisation festgelegt, auf der alle weiteren Softwarebestandteile des Betriebssystems aufbauen Kernelmodul Ein Kernelmodul ist ein Teil des Kernels, der im laufenden Betrieb geladen und entfernt werden kann. Für die Kommunikation zwischen NQC und Mindstorms- USB-Tower ist das Kernelmodul legousbtower erforderlich. legos LegOS ist ein Open Source Project das eine C und C++ Programmierumgebung für das LEGO Mindstorms Robotics Kit bereitstellt. lejos LeJOS ist eine Java-basierte alternative Firmware für den LEGO Mindstorms RCX Mikrocontroller. N QC(N otquitec) Not Quite C ist eine simple Sprache mit C-ähnlicher Syntax die benutzt werden kann um den LEGO Mindstorms RCX zu programmieren. RCX Der RCX ist der zentrale Bestandteil eines Lego Mindstorms Roboters. In ihm befindet sich das Programm, das den Roboter steuert. T Kreuzung Eine Kreuzung, an der drei Wege zusammentreffen. X Kreuzung Eine Kreuzung, an der vier Wege zusammentreffen. 52