======Projektdokumentation====== {{ :projektesose2016:robboard:fotoprofil.jpg |}} =====Einführung===== RoBoard ist ein Roboter, der die Tafel wischen kann, wobei unser Roboter vorrangig für Whiteboards ausgelegt ist. Er hält sich mit Hilfe von Magneträdern an der senkrechten Ebene. Er soll sich zu einem Bereich bewegen, der vom Benutzer am Rechner ausgewählt wurde und in diesem vorhandene Schrift entfernen. Durch die Bildauswertung eines Kamerabildes weiß der Roboter bereits bevor er sich bewegt, ob in dem Bereich Schrift vorhanden ist, sodass er sich im entgegengesetzten Fall gar nicht erst zu bewegen braucht, sondern nur eine Rückmeldung an den Benutzer gibt. Ist der Wischvorgang abgeschlossen, kehrt der Roboter zu seinem Ausgangspunkt zurück und ist erneut einsatzbereit. =====Physischer Aufbau des Roboters===== ====Fahrende Plattform==== {{:projektesose2016:robboard:konstruktiondraufsicht.png?300 |}} {{ :projektesose2016:robboard:fotodraufsicht.jpg?300|}} Die Plattform des Roboters besteht aus einer runden Scheibe Holz. Wir haben uns bewusst für diese Form entschieden, damit es zu keinen Problemen kommt, wenn der Roboter nah am Rand der Tafel fährt, da es hier bei Kurven dazu kommen könnte, dass die Ecken über die Tafel stehen. Auf der fahrenden Plattform ist die Halterung für den Schwamm befestigt, so dass der Schwamm vor dem Roboter hergeschoben wird, wenn dieser sich bewegt, außerdem ein W- LAN Modul und die Platine, auf der der Arduino und die restliche Elektronik gesteckt sind. Fortbewegt wird der Roboter mittels zweier Räder mit Getriebemotoren. Die Räder bestehen aus zwei Magnetscheiben, die wiederum von jeweils zwei Stahlscheiben umgeben sind. Dies dient der Umlenkung und Bündelung des Magnetfeldes. So haftet RoBoard an der Tafel und kann sich in alle Richtungen fortbewegen. Auf der Plattform sind außerdem zwei farbige Kreise angebracht, die der späteren Orientierung auf der Tafel dienen. ==Hierfür benöigte Materialien== * Arduino Nano * W- LAN Modul zur Datenübermittlung * Akku: x A * Kabel * 2 Getriebemotoren: Pololu 3243 * 2 Motortreibeplatinen: Pololu 1213 * 2 Räder mit Magnetkern und Stahlummantelung (Verstärkung und Umlenkung des Magnetfelds) * Holzplatte * Kabelbinder * Klettband * Schrauben, Muttern, etc. ==Hintergrundwissen== Wir mussten uns zuerst näher mit dem Drehmoment beschäftigen müssen, um die für den Motor benötigte Kraft zu berechnen. Ein weiteres Problem stellten die Räder dar, da wir kein Material finden konnten, das stark genug an der Tafel geheftet hat, um ein Abrutschen durch das Eigengewicht des Roboter zu verhindern. Unsere momentane Zwischenlösung besteht in doppelseitigem Klebeband, da so zumindest ein Zerkratzen der Tafel und ein Abrutschen des Roboters vermieden wird. {{ :projektesose2016:robboard:konstruktionprofil.png |}} ====Beweglicher Schwamm==== Der Schwamm ist beweglich gelagert, so dass man den Schwamm nach oben von der Tafel weg klappen kann. Hierdurch ist es möglich sein, den Roboter über die Tafel zu bewegen, ohne sie dabei zu wischen. Die Halterung ist deswegen durch ein Seil mit einem Servo an der Plattform verbunden, der durch ein Aufwickeln des Seils auf ein Laufrad den Schwamm abhebt. Wird der Schwamm abgesenkt ziehen ihn Federn, die zwischen der Plattform und der Halterung befestigt sind, gegen die Tafel, wodurch die benötigte Kraft zum Säubern erzeugt wird. ==Hierfür benötigte Materialien== * Servo zu Bewegung des Schwammes * Alustangen und Klemmringe für die Halterung des Schwamms * Schwamm für Whiteboard * Schrauben, Muttern, etc. ==Hintergrundwissen== Wir mussten uns eine Möglichkeit überlegen, wie wir den Schwamm flexibel befestigen können und er trotzdem genug Kraft aufbringen kann, um die Tafel zu säubern. Das haben wir nun durch die felxible Halterung und die Federn, die den Schwamm gegen die Tafel ziehen, gelöst. =====Bildverarbeitung===== Ein wesentlicher Punkt, der zum Funktionieren unserer Idee von Nöten war, war die Aufnahme der Tafel und der sich auf ihr befindenden Schrift. ====Bildaufnahme==== Die Tafel wird durch eine fest installierte Kamera aufgenommen. Wir haben hierfür das Modell x verwendet. Die Übertragung der Bilder der Kamera auf den Rechner erfolgt über ein USB-Kabel. ==Hierfür benöigte Materialien== * Kamera mit USB-Kabel ====Bildauswertung==== ===Schrifterkennung=== {{:projektesose2016:robboard:screenshot1.png?300 |}} Zuerst haben wir die Daten die das Kamerabild übermittelt, eingelesen und ein von uns erarbeitetes Processing-Programm zur Auswertung der Daten genutzt. Dieses betrachtet jedes einzelne Pixel und färbt es, wenn es unter dem Grenzwert x liegt weiß und wenn es darüber liegt schwarz. Als weitere Option lassen wir den roten und blauen Kreis, die auf der Plattform befestigt sind und zur Orientierung des Roboters dienen, jeweils rot und blau einfärben. Hierfür arbeiten wir den RGB-Werten eines jeden Pixels. Pixel, deren rot-Anteil größer als das 1.7-fache jeweils des blau- und grün-Anteils ist, werden rot gefärbt. Analog werden diejenigen Pixel, deren blau-Anteil den rot- und grün-Anteil um ein mindestens 1.7-faches übersteigt, blau eingefärbt. {{ :projektesose2016:robboard:rgbdiagramm.jpg?300|}} Pixel, deren RGB-Werte "innerhalb" des roten Körpers liegen, werden rot eingefärbt. Diejenigen innerhalb des blauen, blau und alle anderen schwarz bzw. weiß. Die Einfärbung der Kreise sorgt für eine bessere Übersichtlichkeit für den Nutzer. So entsteht ein, bis auf die besagten Kreise, schwarz-weißes Bild der Tafel. Dem Benutzer ist es möglich per Mausklick ein rechteckiges Feld auszuwählen. Das Programm überprüft dann, ob innerhalb dieses Kästchens Schrift, also schwarze Pixel vorhanden sind. Ist dies der Fall, sendet es ein Signal an den Roboter, so dass dieser weiß, dass er wischen muss. Fehlerhafte Signale könnten hierbei durch Flecken oder Spiegelungen verursacht werden. Um dem vorzubeugen, ist es in der Testphase wichtig, auf einen sauberen Hintergrund und gute Lichtverhältnisse zu achten. ---- ===Position des Roboters=== {{ :projektesose2016:robboard:screnshot2.png|}} Wie schon oben beschrieben, nutzen wir zwei farbige Kreise, um die Position des Roboters zu bestimmen. Das Programm berechnet aus den aufgenommenen roten Pixeln den Mittelpunkt und nutzt diesen als Orientierungswert für den Mittelpunkt des Roboters. Aus den aufgenommen blauen Pixeln wird ebenfalls der Mittelpunkt berechnet. Dieser dient als Orientierungswert für das vordere Ende des Roboters. Nun können wir bestimmen, in welche Richtung der Roboter schaut und so später berechnen, wie er zu dem ausgewählten Kästchen steht und welche Bewegungen er ausführen muss, um dorthin zu kommen. =====Datenverwertung===== In diesem Abschnitt geht es darum, die, durch Bildauswertung und Eingabe des Nutzers, gesammelten Daten auszuwerten, so dass diese für die Ansteuerung des Roboters genutzt werden können. Der Roboter bekommt, wie im Abschnitt „Schrifterkennung“ beschrieben, ein Signal gesendet, wenn in dem ausgewählten Bereich Schrift vorhanden ist. Nun geht es darum, den Weg zu der linken oberen Ecke des Kästchens auf Grundlage seiner aktuellen Position und Richtung zu berechnen. Mit Hilfe des roten und des blauen Punktes können wir berechnen, in welchem Winkel sich der Roboter zu dem ausgewählten Bereich befindet. Außerdem können wir die Distanz zwischen Roboter und Kästchen berechnen. Mit Hilfe dieser berechneten Werte und den bekannten Daten des Roboters, wie z.B. Radumfang und der Umfang der Holzplatte können wir berechnen, wie oft sich die Räder drehen müssen, um die gewollte Distanz bzw. den gewollten Winkel zurückzulegen. Diese Werte sind auch die, die dann für die Radansteuerung genutzt werden. ====Informationsfluss==== {{:projektesose2016:robboard:informationsfluss.jpg[[|}} ====Wegberechnung==== ==1. Um einen Winkel drehen== Will man den Roboter in eine bestimmte Richtung zum Beispiel in Richtung des Kästchens drehen (siehe Skizze), berechnet das Programm zunächst mithilfe des Arcustangens die Winkel $\alpha_1$ und $\alpha_2$. Aus der Differenz dieser Winkel ergibt sich der gesuchte Winkel $\alpha$. Für die Anzahl der Radumdrehungen, die nun benötigt werden, um den Roboter um diesen Winkel zu drehen, berechnet das Programm mithilfe der Formel: \[Strecke = \frac{winkel \cdot robumfang} {2 \cdot \pi}\] die zu fahrende Strecke in cm. Diese Strecke wird dann durch den Radumfang geteilt, um die Anzahl der Umdrehungen zu erhalten. {{:projektesose2016:robboard:wegberechnung_winkel.jpg|}} ==2. Eine gerade Strecke fahren== Wenn der Roboter eine gerade Strecke zwischen zwei Punkten fahren soll (siehe Skizze), muss man die Länge der Strecke, die man zunächst nur durch die Funktion dist(punkt1, punkt2) in Pixeln gegeben hat, in Zentimeter umrechnen. Dazu verwenden wir die farbigen Punkte auf dem Roboter, die mit 11cm einen festen Abstand haben. Mit der Gleichung \[ \frac {Punktdistanz} {11cm} = \frac {Zieldistanz} {Strecke} \] können wir nun durch Umstellen die zu fahrende Strecke in cm berechnen. Die Distanzen sind hierbei in Pixeln gegeben. Anschließend werden noch die gesuchte Anzahl an Radumdrehungen durch die Formel \[Umdrehungen = \frac{strecke} {Radumfang} \] berechnet. {{:projektesose2016:robboard:wegberechnung_strecke.jpg|}} =====Motoren===== ====Radansteuerung==== Die Radansteuerung beschäftigt sich mit der eigentlichen mechanischen Arbeit des Roboters. Die beiden Räder werden jeweils von einem Getriebemotor angetrieben. Wenn sich beide Räder gleich schnell in die gleiche Richtung drehen, fährt der Roboter entweder vorwärts oder rückwärts. Drehen sich beide Räder gleich schnell in entgegengesetzte Richtungen, dreht sich der Roboter auf der Stelle. Beide Motoren besitzen einen Encoder, der es möglich macht, die Anzahl der Umdrehungen der Räder zu zählen. Mit Hilfe eines Processing-Programms, das die zuvor berechneten Werte für Distanz und Winkel nutzt, und einem Arduino-Programms, das die Befehle von Processing „übersetzt“, konnten wir so die Räder ansteuern. In wesentliches Problem, welches sich hier aufgetan hat, bestand darin, dass sich die Räder, trotz gleich stark angelegter Spannung, nicht gleich schnell gedreht haben. Das lag vor allem an der unterschiedlichen Belastung der beiden Räder. Wir haben deswegen versucht, ein Programm zu schreiben, um die Geschwindigkeiten immer wieder abzufragen und aneinander anzupassen. Sobald der Roboter am Zielpunkt angelangt ist, sendet er ein Signal. ====Schwammansteuerung==== Wie bereits erwähnt, ist der Schwamm in einer beweglichen Halterung gelagert. Dadurch ist es möglich den Schwamm bei Bedarf von der Tafel abzuheben und so ohne zu wischen von einem Ort zum anderen zu gelangen. Das Hochheben erfolgt mittels eines Servos, der sich dreht und dadurch, dem Prinzip einer Seilwinde folgend, ein Seil auf ein Rad mit einer Laufrille aufwickelt. Sobald sowohl das Signal, dass Schrift vorhanden ist, als auch das Signal, dass der Roboter am Zielpunkt angekommen ist, gesendet wurden, wird der Schwamm abgesenkt. Als verstärkender Faktor wirken hier die Federn, die zwischen Schwammhalterung und Plattform angebracht sind und ziehen den Schwamm gegen die Tafel. Um den Servo ansteuern zu können, mussten wir ein Programm für Arduino schreiben, das die Befehle von Processing für den Arduino „übersetzt“ hat, der dann die entsprechenden Anweisungen an den Servo sendet. =====Kommunikation===== Dieser Teilbereich beinhaltet sowohl die Kommunikation, also Datenübertragung, zwischen Rechner und Arduino, als auch die Kommunikation zwischen Kamera und Rechner. Zwischen Kamera und Rechner wäre eine drahtlose Übertragung optimal, um unabhängig von der Position der Kamera den Rechner aufbauen zu können, ist aber nicht zwingend notwendig und wird von uns zunächst auch nicht verwendet. Die Datenübertragung zwischen Rechner und Arduino muss drahtlos erfolgen. Die zu übertragenden Daten sind bei der Kamera das Bild in Echtzeit, und bei dem Arduino die für die Rad- und Servoansteuerung berechneten Werte, sowie die von den Encodern eingelesenen Werte der Radumdrehungen. Wir nutzen hierfür das W-LAN Modul x. Für die Kommunikation mit dem Arduino mussten wir „Übersetzungsprogramme“ schreiben. ===Pinliste Arduino=== ^ Pin ^ Bauelement ^ | 2 | Motor 1 Hall A (Interrupt) | | 3 | Motor 2 Hall A (Interrupt) | | 5 | Motor 1 Input 1 | | 6 | Motor 1 Input 2 | | 7 | Motor 2 Hall B | | 8 | Motor 1 Hall B | | 9 | Servo | | 11 | Motor 2 Input 1 | | 12 | Motor 2 Input 2 | | A0 | WLAN | | A1 | WLAN | =====Endergebnis===== Die Bildverarbeitung funktioniert bereits sehr gut und auch die Wegberechnung ist fertig. Vor dem Blockkurs hatten wir noch Probleme mit der Radansteuerung von RoBoard, da wir auf keine gemeinsame Geschwindigkeit kommen und der Roboter dadurch immer eine kleine Kurve fuhr, statt geradeaus zu fahren. Nach einigen Änderungen am Programm, konnten wir das aber beheben. Aktuell hatten wir noch Probleme damit, den Roboter zu der linken oberen Ecke des ausgewaehlten Bereichs zu bewegen. Zwar kann der Roboter bei Testprogrammen alle Bewegungen problemlos ausfuehren, in dem Gesamtprogramm scheinen jedoch noch Fehler in der Wegberechnung oder Ansteuerung zu liegen. Einige mechanische Feinheiten müssten noch verbessert werden, wie z.B. die Befestigung des Akkus auf der Plattform. =====Links===== Bisheriger Programmcode: Arduinoprogramm: #include #include //Anpassen: const int minWert = 180; // untere Grenze für die Regelung der Motorausgänge const int maxWert = 220; // obere Grenze für die Regelung der Motorausgänge const int k = 20; //Proportionalitätsfaktor für die Regelung const unsigned int test = 0; //Debug-Modus. Für test = 1 wird eine Ausgabe über die HardwareSerial erzeugt //Pins: int M1In1 = 5; int M1In2 = 6; int M2In1 = 11; int M2In2 = 12; int M1EncA = 2; int M1EncB = 8; int M2EncA = 3; int M2EncB = 7; //Motor 1 unsigned long encoderPos1 = 0; //Umdrehungszähler des Rades an Motor1 mit 1 Umdrehung = 2370,72 Werte von encoderPos1 (Übersetzung = 98,78, Messungen pro HallSensordrehung = 24 -> 24 *98,78 = 2370,72) unsigned long messzeit1Alt = 0; //Zeitpunkt der letzten Messung durch den Hallsensor in mikrosekunden float messgeschwindigkeit1; //gemessen Geschwindigkeit durch den Hallsensor int hall1BAlt; //Wert von Hall1B bei der letzen Messung boolean ersteMessung1; //Ist Messung an Motor1 die erste Messung float motorAusgang1 = 200; //Standarausgabe an InputPin von Motor1 boolean fahrtRichtung1 = false; //Fahrtrichtung true = vorwaerts, false = rückwärts //Motor 2 unsigned long encoderPos2 = 0;//Umdrehungszähler des Rades an Motor2 mit 1 Umdrehung = 2370,72 Werte von encoderPos2 (Übersetzung = 98,78, Messungen pro HallSensordrehung = 24 -> 24 *98,78 = 2370,72) unsigned long messzeit2Alt = 0; //Zeitpunkt der letzten Messung durch den Hallsensor in mikrosekunden float messgeschwindigkeit2; //gemessen Geschwindigkeit durch den Hallsensor int hall2BAlt; //Wert von Hall2B bei der letzen Messung boolean ersteMessung2; //Ist Messung an Motor2 die erste Messung float motorAusgang2 = 200; //Standarausgabe an InputPin von Motor2 boolean fahrtRichtung2 = true; //Fahrtrichtung true = vorwaerts, false = rückwärts int a = 0; //Zähler für die Ausgabe float umdrehungen = 0; //Sollwert für die Radbewegung boolean fahren = false; //Soll der Roboter fahren? true = ja, false = nein Servo myservo; SoftwareSerial mySerial(A0, A1); //Wifly-Modul an A0 und A1 void setup() { pinMode(M1In1, OUTPUT); //In1 Motor 1 pinMode(M1In2, OUTPUT); //In2 Motor 1 pinMode(M2In1, OUTPUT); //In1 Motor 2 pinMode(M2In2, OUTPUT); //In2 Motor 2 pinMode(M1EncA, INPUT); //Motor 1 Encoder A pinMode(M1EncB, INPUT); //Motor 1 Encoder B pinMode(M2EncA, INPUT); //Motor 2 Encoder A pinMode(M2EncB, INPUT); //Motor 2 Encoder B attachInterrupt(digitalPinToInterrupt(M1EncA), Hall1Change, CHANGE); //Interrupt durch den Encoder an Motor 1 attachInterrupt(digitalPinToInterrupt(M2EncA), Hall2Change, CHANGE); //Interrupt durch den Encoder an Motor 2 myservo.attach(9); //Servo initialisieren Serial.begin(19200); //serielle Schnittstelle Initialisieren (Hardware) mySerial.begin(19200); //serielle Schnittstelle Initialisieren (Software / Wifly) Serial.println("Start"); messgeschwindigkeit1 = 0; messgeschwindigkeit2 = 0; } void loop() { if (test == 1) //Debugausgaben erzeugen { if (a >= 10000) //nur alle 10000 loop-Durchgänge eine Ausgabe erzeugen { Serial.print(messgeschwindigkeit1); Serial.print('\t'); Serial.print(encoderPos1); Serial.print('\t'); Serial.print(encoderPos1 / 2370.72); Serial.print('\t'); Serial.print(motorAusgang1); Serial.print('\t'); Serial.print(messgeschwindigkeit2); Serial.print('\t'); Serial.print(encoderPos2); Serial.print('\t'); Serial.print(encoderPos2 / 2370.72); Serial.print('\t'); Serial.print(motorAusgang2); Serial.print('\n'); a = 0; } a++; } if ((encoderPos1 > umdrehungen * 2370.72) || (encoderPos2 > umdrehungen * 2370.72)) // Wenn der Roboter die geforderte Anzahl an Runden gefahren ist { encoderPos1 = 0; //Umdrehungszähler zurücksetzen encoderPos2 = 0; //Umdrehungszähler zurücksetzen fahren = false; //stoppen mySerial.write('f'); //An Processing Signal für die Beendigung senden } Eingabe(); //Überprüfen ob ein Signal von Processing kam motorSteuerung(); //Räder steuern } void Eingabe() { if (mySerial.available()) // Wenn Daten empfangen wurden und zum Lesen bereitstehen { int val = mySerial.read(); // lese das erste Zeichen in der Warteschlange und speichere es in der Variable 'val' zwischen switch (val) { case 'R': // schwamm hoch for (int i = 20; i < 160; i++) //Servo drehen { myservo.write(i); delay(20); } break; case 'H': //schwamm runter for (int i = 160; i > 20; i--) //Servo drehen { myservo.write(i); delay(20); } break; case 'v': //vorwaerts fahren fahren = true; fahrtRichtung1 = true; fahrtRichtung2 = true; motorAusgang1 = 200; motorAusgang2 = 200; umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen break; case 'b': //rueckwaerts fahren fahren = true; fahrtRichtung1 = false; fahrtRichtung2 = false; motorAusgang1 = 200; motorAusgang2 = 200; umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen break; case 'r': //rechts drehen fahren = true; fahrtRichtung1 = true; fahrtRichtung2 = false; motorAusgang1 = 200; motorAusgang2 = 200; umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen break; case 'l': //links drehen fahren = true; fahrtRichtung1 = false; fahrtRichtung2 = true; motorAusgang1 = 200; motorAusgang2 = 200; umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen break; case 's': //stoppen fahren = false; umdrehungen = 0; break; default: break; } //mySerial.write(val); } } void Hall1Change() { boolean illegal = false; //Falls der Encoder prellt oder ähnliches kommt ein Signal, es solla ber keine Geschwindigkeit berechnet werden, da das Signal kein gültiges war //Encoderwerte einlesen int hall1A = digitalRead(M1EncA); int hall1B = digitalRead(M1EncB); if (ersteMessung1 == true) //Erste Messung ist immer illegal { illegal = true; ersteMessung1 = false; } if (hall1BAlt == hall1B) //Wenn sich HallA änder aber HallB nicht ist die Messung illegal { illegal = true; } unsigned long currTime1 = micros(); //aktueller Zeitpunkt if (illegal == false) { messgeschwindigkeit1 = 1000000 / float((currTime1 - messzeit1Alt) * 24 * 98.78); //Geschwindigkeit in Umdrehungen pro Sekunde if (hall1A == 1) { if (hall1B == 1) //Falls Hall1A = Hall1B, dann dreht der Motor rückwärts { messgeschwindigkeit1 *= -1; } } else { if (hall1B == 0) //Falls Hall1A = Hall1B, dann dreht der Motor rückwärts { messgeschwindigkeit1 *= -1; } } encoderPos1 += 1; //Umdrehungszähler erhöhen } //Vergleichsvariablen neu beschreiben hall1BAlt = hall1B; messzeit1Alt = currTime1; } void Hall2Change() { boolean illegal = false; //Falls der Encoder prellt oder ähnliches kommt ein Signal, es solla ber keine Geschwindigkeit berechnet werden, da das Signal kein gültiges war //Encoderwerte einlesen int hall2A = digitalRead(M2EncA); int hall2B = digitalRead(M2EncB); if (ersteMessung2 == true) //Erste Messung ist immer illegal { illegal = true; ersteMessung2 = false; } if (hall2BAlt == hall2B) //Wenn sich HallA änder aber HallB nicht ist die Messung illegal { illegal = true; } unsigned long currTime2 = micros(); //aktueller Zeitpunkt if (illegal == false) { messgeschwindigkeit2 = 1000000 / float((currTime2 - messzeit2Alt) * 24 * 98.78); //Geschwindigkeit in Umdrehungen pro Sekunde if (hall2A == 0) { if (hall2B == 1) //Falls Hall1A != Hall1B, dann dreht der Motor rückwärts, da die Motoren auf einer Scheibe angeordnet sind und damit der Roboter vorwärts fährt, muss immer ein Motor vorwärts, und einer Rückwärts drehen { messgeschwindigkeit2 *= -1; } } else { if (hall2B == 0) //Falls Hall1A != Hall1B, dann dreht der Motor rückwärts, da die Motoren auf einer Scheibe angeordnet sind und damit der Roboter vorwärts fährt, muss immer ein Motor vorwärts, und einer Rückwärts drehen { messgeschwindigkeit2 *= -1; } } encoderPos2 += 1; //Umdrehungszähler erhöhen } //Vergleichsvariablen neu beschreiben hall2BAlt = hall2B; messzeit2Alt = currTime2; } int regelung() { float difGeschwindigkeit = abs(abs(messgeschwindigkeit1) - abs(messgeschwindigkeit2)); //Differenz der Messgeschwindigkeiten float x = k * difGeschwindigkeit; //x ist der Wert um den der MotorAusgang1 geändert wird, abhängig von der Differenz der Messgeschwindigkeiten if (abs(messgeschwindigkeit1) > abs(messgeschwindigkeit2)) //Wenn Motor1 schneller als Motor2 ist wird Motor1 verllangsamt, sonst beschleunigt x *= -1; else if (abs(messgeschwindigkeit1) == abs(messgeschwindigkeit2)) //oder keine änderung vorgenommen return 1; motorAusgang1 += x; //Änderung der Kraft an Motor1 int differenz = motorAusgang1 - motorAusgang2; //Differenz der MotorAusgänge //Verschieben der MotorAusgäm+nge in den Bereich zwischen minWert und maxWert: if (differenz < 0) //motorAusgang1 < motorAusgang2 { if (motorAusgang1 < minWert) { motorAusgang1 = minWert; motorAusgang2 = minWert + abs(differenz); } else if (motorAusgang2 > maxWert) { motorAusgang2 = maxWert; motorAusgang1 = maxWert - abs(differenz); } } else if (differenz > 0) //motorAusgang1 > motorAusgang2 { if (motorAusgang2 < minWert) { motorAusgang2 = minWert; motorAusgang1 = minWert + abs(differenz); } else if (motorAusgang1 > maxWert) { motorAusgang1 = maxWert; motorAusgang2 = maxWert - abs(differenz); } } //Falls dadurch motorAusgang1 oder motorAusgang2 > 255 oder <0 geworden ist, verschieben in den Bereich zwischen 0 und 255: constrain(motorAusgang1, 0, 255); constrain(motorAusgang2, 0, 255); if (differenz < 255) { if (motorAusgang1 == 255) motorAusgang2 -= abs(differenz); else if (motorAusgang1 == 0) motorAusgang2 += abs(differenz); if (motorAusgang2 == 255) motorAusgang1 -= abs(differenz); else if (motorAusgang2 == 0) motorAusgang1 += abs(differenz); } return 1; } void motorSteuerung() { if (fahren == true) //Wenn er fahren soll { regelung(); //Motoren nachregeln if (fahrtRichtung1 == true) //Motor1 vorwärts { analogWrite(M1In1, int(motorAusgang1)); digitalWrite(M1In2, LOW); } else //Motor1 rückwärts { digitalWrite(M1In1, LOW); analogWrite(M1In2, int(motorAusgang1)); } if (fahrtRichtung2 == true) //Motor2 vorwärts { analogWrite(M2In1, int(motorAusgang2)); digitalWrite(M2In2, LOW); } else //Motor2 rückwärts { digitalWrite(M2In1, LOW); analogWrite(M2In2, int(motorAusgang2)); } } else //stoppen (fahren = false) { digitalWrite(M1In1, LOW); digitalWrite(M1In2, LOW); digitalWrite(M2In1, LOW); digitalWrite(M2In2, LOW); } } Processingprogramm: import processing.video.*; import processing.serial.*; import java.io.*; import java.net.*; String kameraName = "Logitech HD Webcam C270"; //Bei Alissa(mac): "USB-Kamera" Bei Theresa(windows): "Logitech HD Webcam C270" final int test = 0; //Debug Modus zur ausführlichen Ausgabe final int tafelbreite = 175; //in cm final int tafelhoehe = 114; //in cm final int high = 480; //Kamerahöhe final int wide = 640; //Kamerabreite final float schwammbreite = 16; final float radumfang = 19.3; final float robumfang = 2*16.5*PI; final float schwammabstand = 24; //ausgleichen vom ueberstehen des schwamm final float punktabstand = 11; //Abstandzwischen den farbeigen Punkten (Mittelpunkte) Capture video; Serial port; Socket clientSocket; DataOutputStream outToServer; InputStream inFromServer; boolean schwarz; boolean angekommen; int maus = 0; //Variable zum Hochzählen bei Mausklick, um verschieden Zustände zu unterscheiden int a = 50; //Seite a des ausgewählten Rechtecks per Mausklick int b = 50; //Seite b des ausgewählten Rechtecks per Mausklick int x_1, y_1, x_2, y_2; //Mauspositionen bei Klick 1 und 2 int threshold = 130; //Grenzwert, ob Pixel schwarz oder weiß wird color black = color(0); color white = color(255); color red = color(255, 0, 0); color blue = color(0, 0, 255); int numPixels; float rot_xPos = 0; float rot_yPos = 0; float blau_xPos = 0; float blau_yPos = 0; char val = ' '; //Befehlsvariable für Arduino int actualState = 0; //aktueller Modus des Arduino/Roboters int programState = 0; //Sollmodus des Arduino/Roboters float schwammumdrehungen = schwammbreite / radumfang; float punktdistanz = 0; float schwammdistanz = 0; void setup() { size(640, 580); strokeWeight(5); videoStart(); smooth(); /*println(Serial.list()); port = new Serial(this, Serial.list()[0], 9600);*/ //Wlan einrichten try { String serverAddress="192.168.0.100"; int serverPort=8000; clientSocket = new Socket(serverAddress, serverPort); outToServer = new DataOutputStream(clientSocket.getOutputStream()); inFromServer = clientSocket.getInputStream(); } catch(Exception e) { print("Verbindung aufbauen setup"); println(e); } if (video.available()) Bildfaerbung(); //Kamerabild lesen und Bild einfärben servo(); //Servo hochfahren delay(2000); punktdistanz = dist(rot_xPos, rot_yPos, blau_xPos, blau_yPos); //Umrechnungsfaktor Pixel <-> Zentimeter in echt schwammdistanz = (punktdistanz/punktabstand) * schwammabstand; //Umrechnung in cm } void draw() { float winkel = 0; char input = ' '; if (antwortArduino() == 'f') //Wenn Arduino sagt, er ist fertig... programState++; //...Sollmodus um 1 erhöhen -> in den nächsten Zustand gehen if (video.available()) { Bildfaerbung(); //Bild aufnehmen und einfärben //Bereich auswaehlen if (maus == 1) //Erster Klick: Erste Ecke des Rechtecks speichern { x_1 = mouseX; y_1 = mouseY; maus = 2; } if (maus == 2) //Rechteck mit erster Ecke und Maus malen { stroke(255, 0, 0); line(x_1, y_1, x_1, mouseY); line(x_1, y_1, mouseX, y_1); line(x_1, mouseY, mouseX, mouseY); line(mouseX, y_1, mouseX, mouseY); } if (maus == 3) //Zweiter Klick: Zweite Ecke des Rechecks speichern { x_2 = mouseX; y_2 = mouseY; //Falls die erste Ecke ncht oben links ist und die zweite Ecke unten Rechts, müssen Ecken getauscht werden, damit im Programm die erste Ecke oben links und die zweite Ecke unten rechts ist. if (x_2 < x_1) { int hilf = x_1; x_1 = x_2; x_2 = hilf; } if (y_2 < y_1) { int hilf = y_1; y_1 = y_2; y_2 = hilf; } //Seitenlängen berechnen a = x_2 - x_1; b = y_2 - y_1; if (test == 1) { print("a= "); print(a); print("\tb= "); print(b); print("\tx_1= "); print(x_1); print("\ty_1="); print(y_1); print("\tx_2 ="); print(x_2); print("\ty_2= "); println(y_2); } maus = 0; //Zurück auf Anfang für ein neues Kästchen bereit sein //Bereich auf Schwarz untersuchen schwarz = bereichTesten(a, b); println(schwarz); if (schwarz) { text("Schwarz vorhanden", 20, height - 60); } else { text("Kein Schwarz vorhanden", 20, height - 60); } //Roboter dreht sich Richtung linkes Ende des Kaestchens if (schwarz) { winkel = berechneWinkel(mouseX-punktdistanz, mouseY); //Drehwinkel berechnen println(winkel); // actualState = 1; roboterDrehen(winkel); schwarz = false; } if (actualState < programState) //Wenn aktueller Zustand kleiner als Sollzustand ist... { actualState++; //..in Sollzustand gehen if ((actualState > 7) && (programState > 7)) //Wenn beide Zustände > 7 -> Roboter ist fertig -> Zustände zurücksetzen { actualState = 0; programState = 0; } switch (actualState) { case 1: //zur linken oberen eck des kaestchens bewegen delay(3000); fahren(dist((int)rot_xPos, (int)rot_yPos, x_1-schwammdistanz, y_1)); delay(3000); case 2: //Roboter waagerecht drehen winkel = berechneWinkel(rot_xPos+1, rot_yPos); roboterDrehen(winkel); println(winkel); angekommen = true; println(angekommen); case 3: //Schwamm absenken servo(); case 4: //kaestchen abfahren kaestchenAbfahren(); angekommen = false; case 5: //Schwamm hoch servo(); case 6: //Roboter zurueck drehen winkel = berechneWinkel(100, 400); roboterDrehen(winkel); case 7: //Zum Startpunkt fahren fahren(dist((int)rot_xPos, (int)rot_yPos, 100, 400)); } } } if (maus == 0) { //Mit Klick fertig -> Kästchen malen, bis zum nächsten Klick noFill(); stroke(0, 255, 0); rect(x_1, y_1, a, b); } } fill(255); stroke(0); rect(0, height - 100, width, 100); fill(0); text("(x,y) = (" + mouseX + ", " + mouseY + ")", 20, height - 80); } void mouseClicked() { //Mauszähler erhöhen if (maus == 0) { maus = 1; } else if (maus == 2) { maus = 3; } } /*void keyReleased() { //Wlan Testfunktion try { outToServer.writeBytes("v3"); } catch (Exception e) { println(e); } }*/ void videoStart() { //Verfügbare Kameras suchen und ausgeben String[] cameras = Capture.list(); if (cameras.length == 0) { println("There are no cameras available for capture."); exit(); } else { println("Available cameras:"); for (int i = 0; i < cameras.length; i++) { println(i + " " + cameras[i]); } //Kamera initialisieren video = new Capture(this, 640, 480, kameraName, 30); //Kameraaufnahme starten video.start(); numPixels = video.width * video.height; //Anzahl der Pixel des Kamerabildes berechnen println(numPixels); } } void Bildfaerbung() { //Kamerbild und Pixel lesen video.read(); video.loadPixels(); float pixelBrightness; loadPixels(); //Variablen initialisieren int currPos = 0; color tempPixel; int rotxmax = 0; int rotxmin = wide; int rotymax = 0; int rotymin = high; int blauxmax = 0; int blauxmin = wide; int blauymax = 0; int blauymin = high; for (int y = 1; y <= 480; y++) //Y-Achse des Bildes durchgehen (Zeilenweise) { for (int x = 1; x <= 640; x++) //X-Achse des Bildes durchgehen (Spaltenweise) { currPos = ((y-1) * wide) + (x-1); //von x,y-Koordinaten in Array-Position umrechnen pixelBrightness = brightness(video.pixels[currPos]); //Helligkeit des Pixels speichern tempPixel = video.pixels[currPos]; //int pixelColor = video.pixels[currPos]; int r = (tempPixel >> 16) & 0xff; //Rotanteil des Pixels bestimmen int g = (tempPixel >> 8) & 0xff; //Grünanteil des Pixels bestimmen int b = tempPixel & 0xff; //Blauanteil des Pixels bestimmen if (r > 1.7*g && r > 1.7*b && r>50) //Wenn Rotanteil überwiegt... { pixels[currPos] = color(255, 20, 20); //...Pixel rot färben //Ersten und letzten rote Pixel bestimmen, sowhl auf der x-Achse, als auch auf der y-Achse if (x < rotxmin) rotxmin = x; else if (x > rotxmax) rotxmax = x; if (y < rotymin) rotymin = y; else if (y > rotymax) rotymax = y; } /* else if(g > 1.5*r && g > 1.5*b && g>20) pixels[currPos] = color(0, 255, 0);*/ else if (b > 1.7*r && b > 1.7*g && b>50) { //Wenn Blauanteil überwiegt... pixels[currPos] = color(0, 0, 255); //...Pixel blau färben //Ersten und letzten blauen Pixel bestimmen, sowhl auf der x-Achse, als auch auf der y-Achse if (xblauxmax) blauxmax = x; if (yblauymax) blauymax = y; } else if (pixelBrightness > threshold) { //Wenn Pixel heller als der Grenzwert ist... pixels[currPos] = white; //...Pixel weiß färben } else { //Ansonsten... pixels[currPos] = black; //...Pixel schwarz färben } } } updatePixels(); //Mittelpunkte des roten und des blauen Kreises rot_xPos = (rotxmin + rotxmax)/2; rot_yPos = (rotymin + rotymax)/2; blau_xPos = (blauxmin + blauxmax)/2; blau_yPos = (blauymin+blauymax)/2; //Mittelpunkte in Schwarz einzeichnen strokeWeight(5); stroke(0); point(rot_xPos, rot_yPos); point(blau_xPos, blau_yPos); //rot_xPos = 0; //rot_yPos = 0; //blau_xPos = 0; //blau_yPos = 0; } boolean bereichTesten(int seite_a, int seite_b) { for ( int i = x_1; i < seite_a + x_1; i++) { //x-Achse des Kästchens ablaufen for (int j = y_1; j < seite_b + y_1; j++) { //y-Achse des Kästchen ablaufens int testFarbe = get(i, j); float testHelligkeit = brightness(testFarbe); if (testHelligkeit < threshold) { //Falls Helligkeit am Punkt kleiner als der Grenzwert ist... return true; //...ist Schwarz vorhanden } //print(j); //print(" "); } //println(i); } return false; //Ansonsten ist kein Schwarz vorhanden } float berechneWinkel(float xPos, float yPos) { // berechnet winkel zwischen richtung des Roboters und gerade Mittelpunkt des Roboters und punkt xPos yPos float alphav = arctan(blau_xPos-rot_xPos, (height-blau_yPos)-(height-rot_yPos)); float alphaz = arctan(xPos-rot_xPos, (height-yPos)-(height-rot_yPos)); float winkel = alphaz-alphav; if (winkel > PI) { winkel = winkel - 2*PI; } else if (winkel < -PI) { winkel = winkel + 2*PI; } return winkel; } float arctan(float deltax, float deltay) { //Berechnung des Arkustangens von 0 bis 360 Grad, damit die Drehrichtung bestimmt werden kann if (deltay == 0) { if (deltax > 0) { return PI; } else { return -PI; } } else if (deltay > 0) { return atan(deltax/deltay); } else { return atan(deltax/deltay) + PI; } } void roboterDrehen(float winkel) { //gewünschte Radumdrehungen des Roboters berechnen float strecke = (winkel*robumfang) / (2*PI); float umdrehungen = strecke / radumfang; //Signal an Roboter schicken if (winkel < 0) //nach links drehen { val = 'l'; try { outToServer.writeBytes(val + str(umdrehungen)); //Beispiel: "l3.24" } catch (Exception e) { println(e); } } else if (winkel > 0) //nach rechts drehen { val = 'r'; try { outToServer.writeBytes(val + str(umdrehungen)); } catch (Exception e) { println(e); } } else if (winkel == 0) { //nichts tun bzw. stoppen val = 's'; try { outToServer.write(val); } catch (Exception e) { println(e); } } } void fahren(float zieldistanz) { //Vorwärts fahren //gewünschte Radumdrehungen berechnen float punktdistanz = dist(rot_xPos, rot_yPos, blau_xPos, blau_yPos); float strecke = (punktabstand/punktdistanz) * zieldistanz; float wegumdrehungen = strecke / radumfang; //Signal an Roboter schicken val = 'v'; try { outToServer.writeBytes(val + str(wegumdrehungen)); } catch (Exception e) { println(e); } delay(10000); } void kaestchenAbfahren() { int i = (int ) (b / schwammbreite) + 1; fahren(a); warten(); //Auf Rückmeldung des Roboters warten for (int j = 2; j Kommentare zum Code: Wir haben bei der letzten Bearbeitung gesehen, dass "programState" in Zeile 47 mit 1 statt mit 0 initialisiert wurde. Das haben wir nun korrigiert, allerdings könnten hier einer der Gründe für die fehlerhafte Umsetzung liegen.