Ziel war es, den Roboter draußen selbstständig Bilder auf den Boden zeichnen zu lassen. Dabei sollte der Roboter sich mit einer dem DGPS (Globales Positionssystem mit Differentialsignal) angelehnten Methode und per Odometrie positionieren und mit umweltfreundlicher Sprühkreide den Boden markieren. Die Grafiken solltem dem Roboter als Vektorgrafiken vorliegen und von ihm zu einer Punktmenge verarbeitet werden, welche dann mit glatten Kurven angefahren werden sollte, um einen ruckelfreies Bild zu erzeugen. Dazu ist jedoch zu sagen, dass wir sicher nicht die ersten waren, die auf diese Idee kamen. Alleine aus dem Robotiklabor sind malokal (Link folgt noch), LageMaro(http://www.mintgruen.tu-berlin.de/robotikWiki/doku.php?id=projektews2013:lagemaro:start), und der Graffiti-Bot zu nennen, obwohl Letzterer an Wände statt auf den Boden zeichnet. Was unser Konzept von diesen Robotern hervorheben soll, ist der Anspruch, den Roboter draußen und ohne Positionierungshilfen wie zum Beispiel eine Kamera fahren zu lassen, weshalb wir ihn auch besonders robust gebaut haben. Er zeichnet sich durch ein robustes Fahrwerk aus, welches auch leichtes Gelände bewältigen kann. Auch das „Werkzeug“ der Wahl zum zeichnen ist den Anforderungen an „etwas“ größere Motive angepasst - anstelle eines Filzschreibers sprüht unser Bodenmarkierspray auf Baumharzbasis einen etwa 6cm breiten Strahl auf den Boden.

Wichtig ist zunächst festzustellen, dass der Roboter (planmäßig) noch nicht fertiggestellt wurde . Viele wichtige Komponenten sind schon voll funktionsfähig, während andere noch optimiert oder überhaupt erstellt werden müssen. Demzufolge wurde der Roboter mit all seinen Funktionen noch nicht getestet. Es ist noch Entwicklungsarbeit notwendig, deswegen ist diese Dokumentation einerseits eine Zusammenfassung des Geschafften, andererseits ein Ausblick auf noch abzuarbeitende Arbeitsschritte.

Die Gesamtaufgabe lässt sich in mehrere Teilaufgaben separieren: Die Positionierung, die Wegfindung, die Fortbewegung und schließlich das Zeichnen mit der Sprühdose. Dabei stets präsent sind außerdem die Motorregelung sowie die Kommunikation der einzelnen Hardwarekomponenten untereinander.

• Raspberry Pi (RasPi)
  • „Hirn“ des Roboters
  • Wertet Sensor- und Odometriedaten aus, errechnet daraus die Radgeschwindigkeiten und überprüft, ob die Dose sprühen soll
  • Gibt Daten an den Steuerungs-Arduino weiter
• Mikrocontroller für den Motor und die Dose (Arduino Nano) (Steuerungs-Arduino)
  • Empfängt Befehle vom RasPi und führt diese aus
  • Steuert Motoren und Sprühdosenaktor an
  • Sendet Radgeschwindigkeiten an RasPi
• Mikrocontroller für die Sensoren (Arduino Nano) (Sensor-Arduino)
  • Registriert Sensordaten von Ultraschall-Entfernungsmesser, Kompass und GPS und sendet diese an den Raspberry Pi

• Getriebemotoren mit Quadraturencodern und großen Felgen mit Offroad- Reifen
• Auswertung und Ansteuerung
• Leistungsendstufe (H-Brücke)
• mitlaufendes Vorderrad

• Sprühdose
• Schubmagnet
• Umlenkhebel
• Sprühdosenhalterung
• Leistungsendstufe

• Sensoren
• DGPS-ähnliche Methode
• Auswertung (Ausblick)

• Algorithmus

• Holzbrett
• Diverse Winkel und Scharniere aus dem Baumarkt
• Gewindestangen
• Muttern
• Schrauben und Bolzen
• Unterlegscheiben
• Aluminiumrohr
• Blech
• Schlauchschellen

Im Betrieb wird es später möglich sein, auf dem Computer eine Liste von Wegpunkten zu erzeugen und diese anschließend per WLAN an den Raspberry Pi zu senden. Dann fragt dieser die Positionsdaten vom Sensor- Arduino ab oder berechnet - bei der Verwendung einer relativen Positionierung - diese ausgehend von seinem Ursprungspunkt aus den Odometrischen Daten des Motor- Arduinos und eines Kompasses, berechnet daraus die Geschwindigkeiten für die beiden Räder und findet heraus, ob die Sprühdose gedrückt werden muss. Diese Werte werden an den Motor- Arduino gesendet, welcher daraus die gewünschte Geschwindigkeit einregelt und den Schubmagneten der Sprühdose ansteuert. Die Kommunikation der einzelnen Komponenten läuft seriell über eine UART (siehe dazu auch http://www.mikrocontroller.net/articles/UART). Der Sensor- Arduino wird dabei direkt über eine Pegelreduktion an die Schnittstelle des Raspberry senden, der Motorcontroller ist über einen FTDI USB/Serial Wandler (Arduino Board) angeschlossen. Der zweite USB-Port wird für den WLAN-Stick benötigt.

HIER DIE DIAGRAMME

Die Motorsteuerung ist ein Closed-Loop System, was bedeutet, dass sie sich ständig selbst überwacht. Wir geben also eine Sollgeschwindigkeit vor, welche dann kontinuierlich nachgeregelt wird. Dazu brauchen wir erst einmal die an den Motoren angebrachten Quadraturencoder.

Unsere Getriebemotoren sind „50:1 Metal Gearmotor 37Dx54L mm with 64 CPR Encoder“ von Pololu. Sie sind für eine Nennspannung von 12V spezifiziert, laufen dabei mit 200upm am Getriebeausgang im Leerlauf und haben eine Übersetzung von 50:1.

Quadraturencoder messen mithilfe zweier Hallsensoren die Bewegung einer magnetischen Scheibe.

Durch die Frequenz der Pegeländerungen lässt sich die Drehzahl ermitteln, durch den Versatz der beiden Ausgänge des Encoders die Drehrichtung. Die Quadraturencoder sind an den Motorwellen befestigt, weswegen wir auf eine recht hohe Auflösung von 64*50=3200 Messungen pro Radumdrehung zurückgreifen können. Um Hardwareinterrupts an unserem Microcontroller einzusparen, verwenden wir allerdings nur die halbe Auflösung, der zweite Ausgang des Quadraturencoders wird natürlich trotzdem ausgewertet, um die Drehrichtung sowie ein mögliches Prellen / Schwingen des Hallsensors zu ermitteln. Je ein Ausgang der Encoder ist also an einen Pin unseres Microcontrollers angeschlossen, der einen Interrupt auslösen kann. Es handelt sich um einen „Change“ Interrupt, was bedeutet, dass dieser sowohl bei steigender als auch bei fallender Flanke ausgelöst wird. Welche Fall vorliegt, ist dabei erst einmal egal. Die Interrupt- Routine liest zunächst den Pegel beider Ausgänge des Encoders ein, überprüft, ob die Messung plausibel war und - für diesen Fall - die Drehrichtung und Geschwindigkeit. Dabei wird schlicht die Zeit in Mikrosekunden seit der letzten Messung zur Hilfe genommen. Also: $\frac{\pi{} \cdot 0,12}{zeitunterschied \cdot 32 \cdot 50}$ ergibt die Geschwindigkeit in Metern / Sekunde. Die Drehrichtung wird - wie schon erwähnt - aus dem Versatz beider Signale gewonnen. Steigt die Flanke am ersten Ausgang, während der zweite Ausgang negativ ist, dreht der Motor in die eine Richtung, ist der zweite Ausgang dabei positiv, dreht der Motor in die andere Richtung. Bei fallender Flanke am ersten Ausgang funktioniert das genauso, allerdings natürlich mit jeweils inversem Pegel auf dem zweiten Ausgang.

Quelle: https://www.pololu.com/product/1444

Die Regelung des Motors auf die Sollgeschwindigkeit übernimmt ein PID- Regler, der auf der Implementierung von http://rn-wissen.de/wiki/index.php/Regelungstechnik basiert. Der PID- Regler besteht aus einem Proportionalglied P (schnelle Regelung des Ausgangssignals ohne Berücksichtigung des Restfehlers), einem Integralglied I (Aufintegrierung des Fehlers zum Ausgleich des Restfehlers, aber relativ langsam) und einem Diffentialglied D (Berechnung der Änderungsgeschwindigkeit, verbessert die Sprungantwort bei kleinen Sprüngen und gleicht Verzögerungen im Regelkreis aus).

Die Feinabstimmung des Reglers erfolgte empirisch. Weil es nur geringe oder gar keine Verzögerung gibt, kommt der Differentialanteil allerdings ebenfalls kaum oder gar nicht zum Einsatz. Erst Tests im Gelände werden dann die finale Abstimmung ergeben. Den „idealen“ Regelkreis haben wir natürlich bisher nicht geschaffen, aber es war ein großer Sprung von einem ursprünglich selbst entwickelten Regelkreis mit Integration und Regelkreisgesamtverstärkung zum jetzigen Regler.

Alte, schwingende Regelung mit Regelkreisgesamtverstärkung

P - Regler. Orange = Drehzahl, blau = Leistung.

PI- Regler Orange = Drehzahl, blau = Leistung.

Die Leistungsendstufe als Schnittstelle zwischen der Regelung und den Elektromotoren ist derzeit eine „Motomama“ von Iteadstudio. Diese basiert auf dem L298N von STMicroelectronics. Mit einem maximalen Strom von 2,5A pro Kanal ist diese Endstufe allerdings mit unseren Getriebemotoren eigentlich überlastet - diese haben einen Blockierstrom von etwa 6A. In Aussicht steht der Wechsel auf eine kräftigere, modernere Endstufe mit Feldeffekttransistoren - oder der Verbleib einer anderen L298N im Roboter. Trotz aller Überlastungen hat diese betagte Konstruktion unsere Belastungen bislang überstanden und sie ist wirklich sehr „wirtschaftlich“ aus Fernost zu beziehen (ca. 3€ inkl. Versand).

Die Sprühdose wird mit zwei Stahlwinkeln aus dem Baumarkt gehalten. Die Winkel sind mit der Grundplatte verschraubt, zwischen ihnen wird die Dose von zwei Schlauchschellen festgehalten. Der Sprühkopf der Dose hängt unten und muss nun nach oben gedrückt werden, um den Sprühvorgang auszulösen. Dafür ist ein großes Scharnier auf der Grundplatte befestigt. Auf der beweglichen Seite befindet sich ein 10mm großes Loch in der Mitte, durch das der Sprühkopf gestekt wird. Wenn nun der Hebel - also das Scharnier - nach oben geklappt wird, löst das Dosenventil aus und sprüht. Zwei Gründe erfordern jedoch einen weiteren Hebel: Die erforderliche Kraft ist zu groß für unseren Schubmagneten und er schiebt eben, anstatt zu ziehen. Deshalb kommt ein etwa zehn Zentimeter langer Umlenkhebel zum Einsatz, der die erforderliche Kraft noch einmal drittelt, den Weg erneut verlängert. Wenn man die Reibung vernachlässigt, haben wir also eine Hebelkonstruktion, welche die zum Sprühen erforderliche Kraft nun sechstelt.

Die Ansteuerung des Schubmagneten erfolg mit einem N- Kanal Mosfet von International Rectifier vom Typ IRL3705N. Ein separater Treiber für den Mosfet kommt aufgrund der relativ geringen Frequenz der von uns verwendeten Pulsweitenmodulation nicht zum Einsatz, das Gate ist über einen Vorwiderstand von 150 Ohm (Der maximale Strom am Ausgang des ATMEGA beträgt 40mA - also I = U / I, 5V / 0.04A = 125 Ohm) direkt an den Mikrocontroller angeschlossen. Parallel dazu wird das Gate über einen 10 Kiloohm Pulldown- Widerstand auf Masse gezogen - als Schutz für den Fall, dass das Gate sonst schweben könnte und der Mosfet „aus Versehen“ den Schubmagneten ansteuern könnte. Die Gatekapazität des Mosfet beträgt bei einer Ansteuerung mit 5V 12nF. Beim gewählten Vorwiderstand von 150 Ohm beträgt die Ladezeit des Mosfet- Gate 1,8 Mikrosekunden. Während dieser zeit befindet sich der Transistor in einem teilleitenden Zustand. Das sind nur etwa 0,5% der Einschaltdauer jedes Pulses bei einer PWM- Frequenz von 500Hz, weshalb hier kein Bedarf für einen Treiber besteht. Würden wir die Frequenz erhöhen - zum Beispiel um durch eine Taktung im Ultraschallbereich die Geräusche der Regelung zu eliminieren, sähe das sicherlich anders aus.

Unser Schubmagnet ist für 10 Watt bei 3V spezifiziert. Natürlich reicht diese Leistung nicht aus, weshalb wir ihn „etwas“ stärker belasten. Das ist auch durchaus laut der Spezifikation vorgesehen - natürlich nicht für eine längere Einschaltdauer. Zunächst geben wir die volle Leistung des 14,8V LiPo an unseren Schubmagneten ab. Das sind bei einem (dem Datenblat entnommenen) ohmschen Widerstand von 0,9 Ohm und unter Vernachlässigung der Induktivität etwa 250 Watt. Bei einem gemessenen Widerstand von 1 Ohm wären es ungefähr 225 Watt. Das befindet sich außerhalb der auf dem Datenblatt genannten Werte - für 200 Watt ist eine maximale Einschaltdauer von 5 Sekunden sowie eine relative Einschaltdauer von 5% angegeben. Davon abgesehen liegt diese Leistung nur für 80ms am Schubmagneten an, um ihn vollständig auszusteuern und das Dosenventil zu aktivieren. Danach greift die Pulsweitenmodulation ein, welche die Leistung bei etwa 500hz mit einer Einschaltdauer von 20% moduliert. Es war uns nicht mit Gewissheit möglich, die Induktivität der Spule zu ermitteln. Messungen ergaben 300uH - doch dabei traten einige Ungereimtheiten auf. Nehmen wir einmal diese 300uH an - nach einer Simulation in LTspice würde sich eine Leistung von etwa 20-25W ergeben. Die maximale Einschaltdauer für diese Leistung beträgt laut Datenblatt 360 Sekunden - 6 Minuten. Nun verringert sich diese Zeit natürlich bei bei besonders vielen Betätigungen des Dosenaktuators mit weniges Abkühlphasen. Momentan gibt es, abgesehen vom Schutz, der eine bereits aktivierte Dose nicht noch einmal mit 100% Last des Schubmagneten aktivieren kann, keine weiteren Sicherheitsvorkehrungen. Theoretisch könnte man mit besonders provokant erstellten Motiven (kurze Strichellinien mit wenigen Ruhepausen) den Schubmagneten zum überhitzen bringen, bevor die Dose leer ist. Hier wären bei einer marktreifen Konstruktion noch weitere Überlegungen nötig, zum Beispiel das Aufintegrieren der Einschaltdauer zur Vermeidung einer thermischen Überlastung der Spule.

Da unser Roboter sich möglichst autonom bewegen können soll, ist der Plan, dass er sich mit Odometrie, einem Kompass, mit Abstandssensporen und mit GPS navigiert. Dabei werden das GPS, der Kompass und die Abstandssensoren von einem extra Arduino angesteuert und ausgewertet.

Das Hauptproblem von GPS ist die Ungenauigkeit. Da nur Zugriff auf das zivile SPS (Standart Positioning Service) besteht, kann mit einer Genauigkeit von acht Metern gerechnet werden. Dazu kommen noch Fehler wie Empfängerrauschen und Toposphährenfehler u.Ä. (http://de.wikipedia.org/wiki/Global_Positioning_System#Genauigkeit_der_Positionsbestimmung ). Eine Möglichkeit zur Verbesserung der Präzision könnte die Errichtung eines selbstgebauten DGPS (Differential Global Positioning System) mit zwei GPS-Empfängern sein. Dabei könnte man dann mit der relativen Position des Roboters zur Basisstation arbeiten, der GPS- Drift dürfte sich im Idealfall (für beide Empfänger - mobil wie stationär gleich) herausrechnen lassen.

Als Abstandssensoren kommen die Ultraschallsensoren HC - SR04 (vgl. http://www.micropik.com/PDF/HCSR04.pdf ) zum Einsatz, welcher auf 15 Grad breite Abstände zwischen 2 cm und 4 m messen kann, wodurch Kollisionen mit beweglichen und unbeweglichen Objekten meistens vermieden werden können. Dabei arbeiten die Senoren ähnlich wie bei Fledermäusen und messen die Zeit aus, welche ein 40kHz-Ton benötigt, um nach Erzeugung wieder empfangen zu werden. Aus dieser Zeit lässt sich dann der Abstand zum Objekt, welches am nächsten am Sensor ist, ausrechnen. Die zugehörige Formel ist:$l=\frac{1}{2}t*v_{Schall}\qquad{} l\text{: Abstand } (cm), t\text{: gemessene Zeit } (\mu s), v_{Schall}\text{: Schallgeschwindigkeit } (\approx 0.034cm/ \mu s)$

$ l = \frac{t}{58}\qquad{}, l\text{: Länge in cm, }t\text{: gemessene Zeit in Mikrosekunden}$

Hier eine Beispielhafte Verkabelung von zwei Ultraschallsensoren an den Sensor-Arduino, erstellt mit Fritzing. Der dazugehörige Code ist sehr modular aufgebaut, so dass ohne Probleme noch weitere dieser günstigen Sensoren angesteuert werden können

Anzumerken ist hierbei, dass immer nur ein Sensor auf einmal angesteuert und ausgewertet wird, da es sonst durch zeitgleiches Aussenden des 40Hz-Schalls zu fehlerhaften Messungen kommen kann.

Bisherige Idee zur Bestimmung der Position per Odometrie ist ein Programm, welches ab dem Zeitpunkt jedes Befehls, die der RasPi an den Steuerungsarduino übergibt, die Länge des gefahrenen Weges misst und über die Radgeschwindigkeiten den aktuell gefahrenen Radius ermittelt. Daraus lässt sich dann aus der Ausgangsposition und der Ausgangsrichtung die neue Position und die neue Richtung ermitteln. Dannach wird der Berrechnungschritt mit den neuen Werten wiederholt. Dabei kann man den Kompass zur Verifizierung der Richtung einsetzen. Die für die Berechnungen wichtigen Formeln werden unten eingeführt.

Die Wegfindung wurde als Regelung in Processing 2.2.1 (https://processing.org/) implementiert und wird auf dem Raspberry Pi laufen. Bei jeder Berechnung werden die aktuell verfügbaren Werte für Position und Ausrichtung des Roboters genommen und in Abhängigkeit von den eingespeicherten abzufahrenden Wegpunkten die Geschwindigkeit und der zu fahrende Radius ermittelt und daraus die Geschwindigkeiten für die beiden Getriebemotoren errechnet. Dabei wird eine Idee von Felix Bonowski angewandt:

Methode ist, um die abzufahrenden Wegpunkte (hier B, C, D, K der Roboter wird als Punkt A dargestellt) und um den Roboter Toleranzzonen zu legen (in der Abbildung die schwarzen Kreise) und die Kreise mit Tangenten zu verbinden. Diese bilden dann den Weg, den der Roboter rein theoretisch fahren darf. Der Sollwert der Geschwindigkeit wird dann so gesetzt, dass der Roboter immer so bremsen kann, dass er innerhalb der Toleranzgrenzen stehen bliebe, würde er sofort beginnen zu bremsen und dabei seine Richtung nicht mehr ändern. Dafür wird der Schnittpunkt R zwischen der Außengrenze und der Richtung des Roboters ermittelt und der Abstand zwischen Roboter und Schnittpunkt in die entsprechende Geschwindigkeit umgerechnet. So wird dafür gesorgt, dass der Roboter innerhalb des Toleranzbreiches bleibt. Hierbei handelt es sich um einen Prportional-Regler. Der zu fahrende Radius wird in Abhängigkeit des Differenzwinkels $\alpha$ zwischen der Ist- und der Sollrichtung berechnet. Dabei handelt es sich zunächst nur um einen Proportional-Regler, welcher vielleicht noch um eine Integral-Komponente erweitert wird, um Hysterese-bedingte Schwankungen zu vermeiden. Dies lässt sich aber erst im Feldversuch überprüfen.

Simulation der Wegfindung in Processing

In der aktuellen Version werden die Positionen des Roboters und der Wegpunkte als PVector gespeichert. Das ist ein praktischer Datentyp von Processing, der zwei oder drei floats speichern kann (entsprechend den Komponenten eines Vektors). Damit werden die Tangenten an den Toleranzkreisen ausgerechnet, dann der Schnittpunkt zwischen den diesen, woraufhin dann nacheinander jeder Abschnitt der so entsstehenden Umrandung auf Schnittpunkt mit der Roboterrichtungsgerade geprüft wird.

Aus dem Radius und der Geschwindigkeit werden dann folgendermaßen die Geschwindigkeiten für die beiden Räder ausgerechnet. Grundlage ist eine Formel, die recht einfach herzuleiten ist und in dem Skript zu Mohamed Oubbatis Vorlesung „Einführung in die Robotik“ ausführlicher ab Seite 29 behandelt wird. (https://www.uni-ulm.de/fileadmin/website_uni_ulm/iui.inst.130/Mitarbeiter/oubbati/RobotikWS1113/OubbatiSkript.pdf)

$\frac{v_l}{v_r} = \frac{r - d}{r + d} = x$

$v_l\text{: Geschwindigkeit linkes Rad}$, $v_r\text{: Geschwindigkeit rechtes Rad}$, $r\text{: Radius}$, $d\text{: halbe Breite Roboter}$, $x = \frac{r - d}{r + d}$

$v_{ges} = \frac{v_l+v_r}{2}$

$v_{ges}\text{: Gesamtgeschwindigkeit}$

$v_r = 2 \cdot v_{ges} - v_l$

$\frac{v_l}{2 \cdot v_{ges} - v_l} = x$

$v_l = (2 \cdot v_{ges} - v_l) \cdot x$

$v_l = \frac{2 \cdot v_{ges} \cdot x}{1 + x}$

$v_r = 2 \cdot v_{ges} \cdot (1- \frac{x}{1+x})$

Des weiteren gilt für die Winkelgeschwindigkeit des Roboters

$\omega{} = \frac{v_{ges}}{r}$

Damit kann zwischen den verschiedenen Größen Radgeschwindigkeiten, Gesamtgeschwindigkeit, Winkelgeschwindigekeit umgerechnet werden.

Hierbei ist anzumerken, dass der Programmcode, der dies umsetzt, sich gerade in Überarbeitung befindet, noch aus mehreren Codeschnipseln besteht und zum Zeitpunkt der ersten Abgabe noch nicht fertiggestellt ist. Im Moment ist ein Processing-Sketch vorhanden, das die Geschwindigkeit und eine Winkelgeschwindigkeit über einen undefinierten Zeitraum ausrechnet, und ein Python-Skript, das aus dem Radius und der Gesamtgeschwindigkeit auf etwas andere Art die Geschwindigkeiten für die beiden Räder ausrechnet. Jedoch hat Juri sich entschieden, den Code komplett neu unter Verwendung von Objektorientierung zu schreiben, um ihn übersichtlicher zu machen und um ihn effizienter zu machen. Gegenwärtig wird mit mehreren Arrays gearbeitet, die in jedem Schritt ausgelesen und bearbeitet werden, was die dazu führt, dass die CPU-Last von javaw.exe (der Process, der hinter Java, bzw. Processing steht) auf meinem Intel© i5-430M (mit 2,26 Ghz, 3M Cache, 2 Kerne, 4 Threads) einen Thread komplett auslastet - trotzdem sinken nach einiger Zeit die Frameraten unter 60FPS, wodurch anzuzweifeln ist, dass der ARM11 des Raspberry Pi das Programm flüssig laufen lässt. Spätestens zum Zeitpunkt der Veröffentlichgung wird der Code vorraussichtlich vervollständigt und zur Verfügung gestellt sein. Dann kann auch ein Problem behoben werden, welches aus der Berechnung der Grenzen auf die genannte Art auftritt und die Sprühdose wird vernünftig implementiert sein…

Wie man in dieser Abbildung gut erkennen kann, gibt es keinen Schnittpunkt zwischen Tangente und Richtung des Roboters, wenn der folgende Wegpunkt auf der anderen Seite liegt. Sowieso wird die Toleranzzone ab einem gewissen Punkt zu groß. Das Problem wird gelöst, in dem von der Toleranzzone ein Stück abgeschnitten wird, beziehungsweise die Toleranzzone mit dem Kreis beendet wird. Was einfacher zu programmieren ist, muss noch überprüft werden.

Zum Schluss steht der Praxistest sowieso noch aus, auch wenn man die Roboterbewegungen mit dem aktuellen Programm einigermaßen simulieren kann.

Wie schon erwähnt, ist dieser Roboter noch nicht fertig. Bis jetzt bekommen wir es gerade mal hin, den Roboter über ein paar Python-Skripte geradeaus, rückwärts und im Kreis fahren lassen und ihn sich auf der Stelle drehen zu lassen. Wir haben noch keine Positionierung und konnten deswegen die Wegfindung noch nicht testen. Dafür funktioniert das, was wir testen konnten, sehr gut. Die Motoren regeln jede Ungleichmäßigkeit heraus, der Roboter fährt nahezu perfekte Kreise, die Sprühdose wird wirkungsvoll angesteuert. Was jetzt noch zu tun ist, ist die Neuprogrammierung der Wegfindung, der Anschluss des Sensor-Arduinos und die Umsetzung der Positionierung. Möglicherweise könnte man auch ein Processing eine Bedienungsoberfläche für Touchscreens machen, das ist aber noch Zukunftmusik. Und das Gehäuse, welches nach nachhaltigen Upcycling-Gesichtspunkten aus dem Gehäuse eines kaputten Miele-Staubsaugers bestehen wird, muss noch fertig gebaut werden.

Ansonsten sind wir recht zufrieden, mit dem was wir geschafft haben. Zwar haben unsere Programmieraufgaben länger gedauert als nötig gewesen wäre, aber sie waren sehr lehrreich. Ansonsten sind wir gar nicht so schlecht im Zeitplan

Gantt-Diagramm unseres Projekts, erstellt mit GanttProject. Die schwarzen Linien zeigen den Stand der einzelnen Schritte an.

// melok motorsteuerung - von tim rogasch 2014-2015
//Variablen motorsteuerung
 
const float radumfang = 0.377;
 
float hall1Strecke = 0;
boolean hall1Richtung = true;
int hall1A = 0;
int hall1B = 0;
int hall1BCache = 5;  //Wert, den er sonst nie annimmt
unsigned long hall1Zeitcache;
float hall1Speed = 0;
boolean hall1ErsteMessung = true;
float motor1Leistung = 0.0;
 
float hall2Strecke = 0;
boolean hall2Richtung = true;
int hall2A = 0;
int hall2B = 0;
int hall2BCache = 5;  //Wert, den er sonst nie annimmt
unsigned long hall2Zeitcache;
float hall2Speed = 0;
boolean hall2ErsteMessung = true;
float motor2Leistung = 0.0;
 
//boolean motor1Richtung = true;
 
 
float sollGeschwindigkeit1 = 0;      //WICHTIGES ZEUG
float sollGeschwindigkeit2 = -0;      //WICHTIGES ZEUG
 
unsigned long regelungsZeitcache;
float regelung1Fehler = 0;
float regelung1Fehlercache = 0;
float regelung1Fehlersumme = 0;
float regelung2Fehler = 0;
float regelung2Fehlercache = 0;
float regelung2Fehlersumme = 0;
 
//Regelungswerte
const float kp  = 300;
const float ki   = 0.01; //0.01
const float kd  = 0.00;
 
 
//dose
boolean doseIst = false;
boolean doseSoll = false;
 
 
//Pins
int in1Pin = 7;
int in2Pin = 8;
int pwm1Pin = 9;
int in3Pin = 11;
int in4Pin = 12;
int pwm2Pin = 10;
 
int mosfetPin = 6;
 
 
//serielle Kommunikation
 
String inputString = "";   //string für seriellen input
boolean stringComplete = false;  // flag für kompletten string (zeilenumbruch erfolgt)
 
 
 
void setup() {
  pinMode(in1Pin, OUTPUT); //IN1 Motor 1
  pinMode(in2Pin, OUTPUT); //IN2 Motor 1
  pinMode(pwm1Pin, OUTPUT); //PWM Motor 1
  pinMode(in3Pin, OUTPUT); //IN3 Motor 2
  pinMode(in4Pin, OUTPUT); //IN4 Motor 2
  pinMode(pwm2Pin, OUTPUT); //PWM Motor 2
 
 
  pinMode(2, INPUT); //Pin 2 ist ein Input (Hall1 A)
  pinMode(4, INPUT); //Pin 4 ist ein Input (Hall1 B)
 
  pinMode(3, INPUT); //Pin 3 ist ein Input (Hall2 A)
  pinMode(5, INPUT); //Pin 5 ist ein Input (Hall2 B)
 
  attachInterrupt(0, Hall1Change, CHANGE); //Interrupt 0 ist an Pin 2
  attachInterrupt(1, Hall2Change, CHANGE); //Interrupt 1 ist an Pin 3
 
  pinMode(mosfetPin, OUTPUT); 
 
  regelungsZeitcache = micros();
 
  //serial
  Serial.begin(9600);
  inputString.reserve(200);
}
 
 
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    if (inChar == '\n') {                                      //Zeilenumbruch
 
//      Serial.println(inputString);
 
      if (inputString.length() > 0) {
 
        String motor1wert;
        String motor2wert;
        String dosenwert;
        char motor1buf[32];
        char motor2buf[32]; 
 
        //hier zerlegen
 
        //motor1wert
        motor1wert = inputString.substring(0,(inputString.indexOf(';')));
        motor1wert.toCharArray(motor1buf, sizeof(motor1buf));
        sollGeschwindigkeit1 = atof(motor1buf);
 
        //kürzen
        inputString = inputString.substring((inputString.indexOf(';') + 1));
 
        //motor2wert
        motor2wert = inputString.substring(0,(inputString.indexOf(';')));
        motor2wert.toCharArray(motor2buf, sizeof(motor2buf));
        sollGeschwindigkeit2 = -(atof(motor2buf));
 
        //kürzen
        inputString = inputString.substring((inputString.indexOf(';') + 1));
 
        //dosenwert
        dosenwert = inputString.substring(0,1);
        Serial.println(dosenwert);
        if (dosenwert == "1") {
          DoseEinschalten();
        }
        else {
          DoseAbschalten();
        }
 
      }
      //string leeren
      inputString = ""; 
      Serial.flush();
      StatusSenden();
 
 
    }
    if(inChar != '\n'){
      inputString += inChar;
    }
  }
}
 
 
void loop() {    //Hauptschleife
 
  if (micros() > (regelungsZeitcache + 1000)) { //1000hz
    RegelungKanal1();
    RegelungKanal2();
    //    Serial.print(int(motor1Leistung));
    //    Serial.print(";");
    //    Serial.println(hall1Speed);
    regelungsZeitcache = micros(); //Zeitcache neu schreiben
  }
 
}
 
 
void StatusSenden() {
    Serial.print(hall1Strecke,6);
    Serial.print(';');
    Serial.print(hall2Strecke,6);
    Serial.print('\n');
}
 
 
void DoseEinschalten() {
  if (doseIst == false) {
    analogWrite(mosfetPin,255);
    delay(80);  
    analogWrite(mosfetPin,50);  
    doseIst = true;
  }
}
 
void DoseAbschalten() {
  analogWrite(mosfetPin,0); 
  doseIst = false;
}
 
 
void RegelungKanal1()
{
  //Erkennung, ob der Motor steht
  if (micros() > (hall1Zeitcache+100000)) {
    hall1Speed = 0;
    hall1ErsteMessung = true;
  }
  //Regelung
 
  //Dieser Motor läuft "rückwärts"!
  // sollGeschwindigkeit1 = (-sollGeschwindigkeit1);
 
  regelung1Fehler = sollGeschwindigkeit1 - hall1Speed;
  if (regelung1Fehler > 50) {
    regelung1Fehler = 50;
  } //Deckeln
  if (regelung1Fehler < -50) {
    regelung1Fehler = -50;
  }  //Deckeln
  regelung1Fehlersumme = regelung1Fehlersumme + regelung1Fehler;
  motor1Leistung = kp*regelung1Fehler + ki*(micros() - regelungsZeitcache)*regelung1Fehlersumme + kd/(micros()-regelungsZeitcache) * (regelung1Fehler - regelung1Fehlercache);	//Reglergleichung
  regelung1Fehlercache = regelung1Fehler;
  if (motor1Leistung > 255) {
    motor1Leistung = 255;
  }  //Deckeln
  if (motor1Leistung < -255) {
    motor1Leistung = -255;
  }  //Deckeln
  //Motor drehen lassen
  if (motor1Leistung > 0) {        //Vorwärts
    digitalWrite(in1Pin, HIGH); // High, Low = Vorwärts; Low, High = Rückwärts, Beide an = Bremse!
    digitalWrite(in2Pin, LOW);
  }
  if (motor1Leistung < 0) {        //Rückwärts
    digitalWrite(in1Pin, LOW); // High, Low = Vorwärts; Low, High = Rückwärts, Beide an = Bremse
    digitalWrite(in2Pin, HIGH);
  }
  if (motor1Leistung == 0) {        //Stillstand
    digitalWrite(in1Pin, HIGH); // High, Low = Vorwärts; Low, High = Rückwärts, Beide an = Bremse
    digitalWrite(in2Pin, HIGH);
  }
  analogWrite(pwm1Pin,int(abs(motor1Leistung)));
}
 
void RegelungKanal2()
{
  //Erkennung, ob der Motor steht
  if (micros() > (hall2Zeitcache+100000)) {
    hall2Speed = 0;
    hall2ErsteMessung = true;
  }
  //Regelung
  regelung2Fehler = sollGeschwindigkeit2 - hall2Speed;
  if (regelung2Fehler > 50) {
    regelung2Fehler = 50;
  } //Deckeln
  if (regelung2Fehler < -50) {
    regelung2Fehler = -50;
  }  //Deckeln
  regelung2Fehlersumme = regelung2Fehlersumme + regelung2Fehler;
  motor2Leistung = kp*regelung2Fehler + ki*(micros() - regelungsZeitcache)*regelung2Fehlersumme + kd/(micros()-regelungsZeitcache) * (regelung2Fehler - regelung2Fehlercache);	//Reglergleichung
  regelung2Fehlercache = regelung2Fehler;
  if (motor2Leistung > 255) {
    motor2Leistung = 255;
  }  //Deckeln
  if (motor2Leistung < -255) {
    motor2Leistung = -255;
  }  //Deckeln
  //Motor drehen lassen
  if (motor2Leistung > 0) {        //Vorwärts
    digitalWrite(in3Pin, HIGH); // High, Low = Vorwärts; Low, High = Rückwärts, Beide an = Bremse!
    digitalWrite(in4Pin, LOW);
  }
  if (motor2Leistung < 0) {        //Rückwärts
    digitalWrite(in3Pin, LOW); // High, Low = Vorwärts; Low, High = Rückwärts, Beide an = Bremse
    digitalWrite(in4Pin, HIGH);
  }
  if (motor2Leistung == 0) {        //Stillstand
    digitalWrite(in3Pin, HIGH); // High, Low = Vorwärts; Low, High = Rückwärts, Beide an = Bremse
    digitalWrite(in4Pin, HIGH);
  }
  analogWrite(pwm2Pin,int(abs(motor2Leistung)));
}
 
 
 
 
void Hall1Change()
{
  //Variablen deklarieren
  boolean illegal = false;
  unsigned long zeitunterschied = 0;
  //Ausgänge des Hallsensors einlesen
  hall1A = digitalRead(2);
  hall1B = digitalRead(4);
  //Illegalflag setzen
  //Was ist illegal? Nun, da ich hier die Gesetze schreibe:
  //Der Wert A ändert sich ohne eine Änderung des Wertes B. Dies passiert immer, wenn der Encoder prellt / schwingt oder an einem tatsaechlichen Wendepunkt.
    //In beiden Fällen muss hier die Geschwindigkeit null betragen.
  if (hall1B == hall1BCache) {
    illegal = true;
  }
  //Die erste Messung ist auch immer illegal!
  if (hall1B == hall1Zeitcache) {
    illegal = true;
    hall1ErsteMessung = false;
  }
  //Drehrichtung ermitteln
  if (illegal == false) {
    if (hall1A  == true) {
      if (hall1B == true) {
        hall1Richtung = false;
      }
      else {
        hall1Richtung = true;
      }
    }
    else {           //A ist negativ
      if (hall1B == true) {
        hall1Richtung = true;
      }
      else {
        hall1Richtung = false;
      }
    }  
  }
  //Geschwindigkeit ausrechnen, wenn die Messung nicht illegal war
  if (illegal == false) {
    zeitunterschied = micros() - hall1Zeitcache;
    //Serial.println(zeitunterschied);
    //(1/(zeitunterschied*32*50))*pi*0,12=236
    hall1Speed = 1/(float(zeitunterschied)/236); //Vereinfacht
  }
  else {
    hall1Speed = 0;
  }     
  //bei Rückwärtsdrehung wird der Wert nun negativ!
  if (hall1Richtung == false) {
    hall1Speed = -(hall1Speed);
  }
  //Den Cache neu beschreiben
  hall1BCache = hall1B;
  hall1Zeitcache = micros();
}
 
void Hall2Change()
{
  //Variablen deklarieren
  boolean illegal = false;
  unsigned long zeitunterschied = 0;
  //Ausgänge des Hallsensors einlesen
  hall2A = digitalRead(3);
  hall2B = digitalRead(5);
  //Illegalflag setzen
  //Was ist illegal? Nun, da ich hier die Gesetze schreibe:
  //Der Wert A ändert sich ohne eine Änderung des Wertes B. Dies passiert immer, wenn der Encoder prellt / schwingt oder an einem tatsaechlichen Wendepunkt.
    //In beiden Fällen muss hier die Geschwindigkeit null betragen.
  if (hall2B == hall2BCache) {
    illegal = true;
  }
  //Die erste Messung ist auch immer illegal!
  if (hall2B == hall2Zeitcache) {
    illegal = true;
    hall2ErsteMessung = false;
  }
  //Drehrichtung ermitteln
  if (illegal == false) {
    if (hall2A  == true) {
      if (hall2B == true) {
        hall2Richtung = false;
      }
      else {
        hall2Richtung = true;
      }
    }
    else {           //A ist negativ
      if (hall2B == true) {
        hall2Richtung = true;
      }
      else {
        hall2Richtung = false;
      }
    }  
  }
  //Geschwindigkeit ausrechnen, wenn die Messung nicht illegal war
  if (illegal == false) {
    zeitunterschied = micros() - hall2Zeitcache;
    //Serial.println(zeitunterschied);
    //(1/(zeitunterschied*32*50))*pi*0,12=236
    hall2Speed = 1/(float(zeitunterschied)/236); //Vereinfacht
  }
  else {
    hall2Speed = 0;
  }     
  //bei Rückwärtsdrehung wird der Wert nun negativ!
  if (hall2Richtung == false) {
    hall2Speed = -(hall2Speed);
  }
  //Den Cache neu beschreiben
  hall2BCache = hall2B;
  hall2Zeitcache = micros();
}

Ein erster unvollständiger Code wird durch noch einen unvollständigeren Code ersetzt

//////////////////////////////////////////////////////////////////////////////////////// 
/////Neu ausgearbeitete Wegfindung des Roboters melok, noch nicht abgeschlossen/////////
////////////////////////////////////////////////////////////////////////////////////////
 
  int radius = 25;
  CustomDraw c = new CustomDraw();
  Border theborder = new Border();
  Robot melok = new Robot();
  float[] waypoints;
 
void setup() {
  size(800,600);
  background(255);
  noLoop(); //for debugging 
 
  String[] waypointStrg = loadStrings("waypoints.txt"); //importing waypoints from waypoints.txt, which is in sketch folder, Correct content: waypoint1x,waypoint1y,waypoint2x,waypoint2y...
  waypoints = float(split(waypointStrg[0], ','));
  println(waypoints);
 
  theborder.build();
 
  melok.position.set(100,200);
  melok.direction.set(1,0);
  melok.speed = (0);
 
}
 
void draw() {
 
  melok.posdraw();
  melok.dirdraw();
  melok.settargetspeed();
 
}
 
//building the border

class Border {
  Borderpart start;
  Borderpart pointer;
 
 
  Border() {
    start = new Borderpart();
    pointer = start;
  }
 
  float distanceto(Robot roboter) {
    if (isCrossing(roboter)) {
      return roboter.position.dist(start.crossPoint(roboter));
    } else {
      println("EPIC ERROR!!!111oneone Robot is not facing any Border");
      return 0;
    }
  }
 
  boolean isCrossing(Robot roboter){
    return start.borderisCrossed(roboter);
 
  }
 
  void build() {
 
    for (int i = 0; i<= waypoints.length - 2; i+=2) {          //creating Borderparts for each pair of following waypoints
      if (i==0) {
        PVector temp = new PVector(waypoints[i], waypoints[i+1]);   //some temp-vectors saving the positions of the waypoints
        PVector temp2 = new PVector(waypoints[i+2], waypoints[i+3]);
        PVector tan = new PVector(temp2.x-temp.x, temp2.y-temp.y);  //STEP 1: creating PVector between to waypoints
        tan.setMag(radius);                                         //STEP 2: set magnitude of PVector to radian of tolerance circles
        tan.rotate(PI/2);                                           //STEP 3: rotate PVector by 90 degrees (PI/2)
        pointer.left.beginpoint.set(temp);                          //STEP 4: set the beginpoints and endpoints to the coordinates of the waypoints
        pointer.left.beginpoint.add(tan);                           //STEP 5 either add or subtract the tan-vector
        pointer.left.endpoint.set(temp2);
        pointer.left.endpoint.add(tan);
        pointer.right.beginpoint.set(temp);
        pointer.right.beginpoint.sub(tan);
        pointer.right.endpoint.set(temp2);
        pointer.right.endpoint.sub(tan);
        println(pointer.left.beginpoint);
        c.ccircle(waypoints[i], waypoints[i+1], radius);
        pointer.borderdraw(); //draw border
      } else if (i >= waypoints.length-2) {
 
        c.ccircle(waypoints[i], waypoints[i+1], radius);
 
        pointer.nextPart = new Borderpart();
        pointer.nextPart.left.beginpoint.set(pointer.left.endpoint);
        pointer.nextPart.left.endpoint.set(pointer.right.endpoint);
 
        pointer.nextPart.borderdraw();
 
        pointer = start;
      } else {
        // though there are parts of the border which connect the endpoints and the beginpoints of the other parts and which cant by calculated like this.
        // that's and the fact we don't have something like .previousPart are the reasons we have to calculate with pointer.nextPart and pointer.nectPart.nextPart
        pointer.nextPart = new Borderpart();            //initializing pointer.nextPart and pointer.nextPart.nextPart
        pointer.nextPart.nextPart = new Borderpart();
 
        //calculating pointer.nextPart
 
          PVector temp = new PVector(waypoints[i], waypoints[i+1]);   //some temp-vectors saving the positions of the waypoints
        PVector temp2 = new PVector(waypoints[i+2], waypoints[i+3]);
        PVector tan = new PVector(temp2.x-temp.x, temp2.y-temp.y);  //STEP 1: creating PVector between to waypoints
        tan.setMag(radius);                                         //STEP 2: set magnitude of PVector to radian of tolerance circles
        tan.rotate(PI/2);                                           //STEP 3: rotate PVector by 90 degrees (PI/2)
        pointer.nextPart.nextPart.left.beginpoint.set(temp);                          //STEP 4: set the beginpoints and endpoints to the coordinates of the waypoints
        pointer.nextPart.nextPart.left.beginpoint.add(tan);                           //STEP 5 either add or subtract the tan-vector
        pointer.nextPart.nextPart.left.endpoint.set(temp2);
        pointer.nextPart.nextPart.left.endpoint.add(tan);
        pointer.nextPart.nextPart.right.beginpoint.set(temp);
        pointer.nextPart.nextPart.right.beginpoint.sub(tan);
        pointer.nextPart.nextPart.right.endpoint.set(temp2);
        pointer.nextPart.nextPart.right.endpoint.sub(tan);
        //calculating pointer.nextPart.nextPart
        //borderparts are added at the outer curve side to prevent holes in the border
        //the Tangent of the other side is intentionally kept empty (but beginpoint and endpoint will be initialized (empty vectors are still (0,0) we either have to search a more intelligent way or make sure this will never be any problem))
        if (isLeftfour(pointer.left.beginpoint, pointer.left.endpoint, pointer.nextPart.nextPart.left.beginpoint, pointer.nextPart.nextPart.left.endpoint)) 
        {
          pointer.nextPart.left.beginpoint.set(pointer.left.endpoint);
          pointer.nextPart.left.endpoint.set(pointer.nextPart.nextPart.left.beginpoint);
        } else {
          pointer.nextPart.right.beginpoint.set(pointer.right.endpoint);
          pointer.nextPart.right.endpoint.set(pointer.nextPart.nextPart.right.beginpoint);
        }
 
        c.ccircle(waypoints[i], waypoints[i+1], radius);
        pointer.nextPart.borderdraw();
        pointer.nextPart.nextPart.borderdraw();
        //set the pointer tp pointer.nextPart.nextPart so we can start this again
        pointer = pointer.nextPart.nextPart;
      }
      print(i);
      print("  ");
      println(waypoints.length-i);
    }
  }
}

class Borderpart {
  Borderpart nextPart;        //nextPart points to nect Part, which makes the Border to a linked list
  Tangent left;
  Tangent right;
 
  Borderpart() {
    left = new Tangent();
    right = new Tangent();
  }
 
  boolean borderpartisCrossed(Robot roboter) {
    println("Checking if borderpartisCrossed");
    boolean isCrossed;
    isCrossed = left.isCrossed(roboter)||right.isCrossed(roboter); //check, if left or right tangend will be crossed  
 
    return isCrossed;
  }
 
  boolean borderisCrossed(Robot roboter) {
    println("Checking if borderisCrossed");
    if (borderpartisCrossed(roboter)) {
      return true;
    } else {
      return nextPart.borderisCrossed(roboter);
    }
  }
 
  PVector crossPoint(Robot roboter) { //finds the first point, where borer is crossed by directional straight line
    if (borderpartisCrossed(roboter)) {
      PVector vector = new PVector(0, 0);
      if (left.isCrossed(roboter)) {
        return intersection(roboter.position.x, roboter.position.y, roboter.direction.x, roboter.direction.y, left.beginpoint.x, left.beginpoint.y, left.endpoint.x-left.beginpoint.x, left.endpoint.y-left.beginpoint.y);
      } else {
        return intersection(roboter.position.x, roboter.position.y, roboter.direction.x, roboter.direction.y, right.beginpoint.x, right.beginpoint.y, right.endpoint.x-right.beginpoint.x, right.endpoint.y-right.beginpoint.y);
      }
    } else {
      return nextPart.crossPoint(roboter);
    }
  }
 
  void borderdraw() { //draws border
    color(0);
    c.cline(left.beginpoint.x, left.beginpoint.y, left.endpoint.x, left.endpoint.y);
    c.cline(right.beginpoint.x, right.beginpoint.y, right.endpoint.x, right.endpoint.y);
  }
}

class Tangent {
  PVector beginpoint;               //Beginpoint of Tangent
  PVector endpoint;                 //Endpoint of Tangent
 
    Tangent() {
    beginpoint = new PVector();
    endpoint = new PVector();
  }
 
  boolean isCrossed(Robot roboter) { //checks if tangent will be crossed
    println("Checking if tangent isCrossed");
    if (endpoint == null|| beginpoint == null) {
      println("Tangent is not crossed, because there is no tangent");
      return false;
    } else {
        println("Beginpoint" + beginpoint + "Endpoint" + endpoint);
        stroke(255, 0, 0);
        c.cline(beginpoint.x, beginpoint.y, endpoint.x, endpoint.y);
      PVector crosspoint = new PVector();
      stroke(100, 100, 100);
      crosspoint = intersection(roboter.position.x, roboter.position.y, roboter.direction.x, roboter.direction.y, beginpoint.x, beginpoint.y, endpoint.x-beginpoint.x, endpoint.y-beginpoint.y);
      //Leider gibt es bei intersection() noch einen dicken Fehler(siehe dort), deshalb weren jetzt noch fehlerhafte Werte ausgespuckt
        println(crosspoint);
        c.ccircle(crosspoint.x, crosspoint.y, 5);
      if ((beginpoint.x <= crosspoint.x && crosspoint.x <= endpoint.x)||(beginpoint.x >= crosspoint.x && crosspoint.x >= endpoint.x)) {
          stroke(0, 0, 255);
          c.ccircle(crosspoint.x, crosspoint.y, 5);
          println("Tangent is crossed!");
        return true;
      } else {
          println("Tangent is not crossed.");
        return false;
      }
    }
  }
}

// Class of Robot
class Robot {
  PVector position;
  PVector direction;
  float speed;
  Boolean isSpraying;
 
  float targedspeedleft;
  float targedspeedright;
 
  int currentgoal;
 
  Robot() {
    position = new PVector(0, 0);
    direction = new PVector(0, 0);
    speed = 0;
    isSpraying = false;
    targedspeedleft = 0;
    targedspeedright = 0;
    currentgoal = 1;
  }
  void posdraw() {
    stroke(0, 255, 0);
    c.ccircle(position.x, position.y, 5);
  }
 
  void dirdraw() {
    stroke(100, 100, 100);
    c.cline(position.x, position.y, position.x+50*direction.x, position.y+50*direction.y);
  }
 
  void settargetspeed() { //main-method of robot
    goalcheck();
    float distance = theborder.distanceto(melok);
    float midspeed = sqrt(distance)/100;
    PVector desired = new PVector(waypoints[currentgoal], waypoints[currentgoal+1]);
    desired.sub(position);
    float anglechange = pow(PVector.angleBetween(desired, direction)/3, 2);
 
    if (isLefttwo(direction, desired)) {
      anglechange = -anglechange;
    }
 
    //Icomplete, TODO: calculate targed-speed of the two wheels
  }
 
  void goalcheck() { //checks f robot reched waypoint
    PVector waypoint = new PVector(waypoints[currentgoal], waypoints[currentgoal+1]);
    if (position.dist(waypoint)<radius) {
      currentgoal++;
      theborder.start = theborder.start.nextPart;
    }
  }
 
  void receiveData() {
  }
  void sendData() {
  }
}

class CustomDraw {


  CustomDraw() {
  }

  void cline(float Xpos1, float Ypos1, float Xpos2, float Ypos2) {
    line(Xpos1, Ypos1, Xpos2, Ypos2);
  }
  void ccircle(float x, float y, float r) {
    noFill();
    ellipse(x, y, 2*r, 2*r);
  }
}

boolean isLefttwo(PVector heading, PVector target) { //http://forum.processing.org/two/discussion/2463/turn-left-or-right
  return heading.y * target.x > heading.x * target.y;
}

boolean isLeftfour(PVector begin1, PVector end1, PVector begin2, PVector end2) { //frei nach http://forum.processing.org/two/discussion/2463/turn-left-or-right
  PVector one = new PVector();
  one.set(end1);
  one.sub(begin1);
  PVector two = new PVector();
  two.set(end2);
  two.sub(begin2);
  return one.y * two.x > one.x * two.y;
}

PVector intersection(float P1X, float P1Y, float D1X, float D1Y, float P2X, float P2Y, float D2X, float D2Y) { //this function calculates the Position of the crosspoint between two straight lines, input are the coordinates of the support vector and the irectional vector
  PVector intersection, P1, D1;
  intersection = new PVector(0, 0);
  P1 = new PVector(P1X, P1Y);
  D1 = new PVector(D1X, D1Y);
  println("P1 " + P1 + "D1" + D1);
  float t = (P2Y + (P1X-P2X)*(D2Y/D2X) - P1Y) / (D1Y - D1X*(D2Y/D2X));     //BUG! Leider habe ich hier einen großen Fehler entdeckt. Wenn eine Gerade senkrecht ist, ist die X-Koordinate ihres Richtungsvektors(D2X) null. also wird in diesem fall durch null geteilt. Dieser Fehler wird möglichst schnell behoben.
  println("nennernenner"+(D2Y/D2X));
  println("Nenner" +(D1Y - D1X*(D2Y/D2X)));
  println("t" + t );
  intersection = D1.get();
  intersection.mult(t);
  intersection.add(P1);
  return intersection;
}