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.

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.

• 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.

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.

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.

• Servo zu Bewegung des Schwammes
• Alustangen und Klemmringe für die Halterung des Schwamms
• Schwamm für Whiteboard
• Schrauben, Muttern, etc.

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.

Ein wesentlicher Punkt, der zum Funktionieren unserer Idee von Nöten war, war die Aufnahme der Tafel und der sich auf ihr befindenden Schrift.

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.

* Kamera mit USB-Kabel

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

Bisheriger Programmcode: Arduinoprogramm:

Steuerung_Arduino.zip

#include <Servo.h>
#include <SoftwareSerial.h>
 
//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:

Roboard_Processing.zip

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 (x<blauxmin) blauxmin = x;
        else if (x>blauxmax) blauxmax = x;
        if (y<blauymin) blauymin = y;
        else if (y>blauymax) 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<i; j++) 
  {
    if (j%2 == 0) 
    {
      roboterDrehen(PI/2);
      warten();
      val = 'v';
      try
      {
        outToServer.writeBytes(val + str(schwammumdrehungen));
      }
      catch (Exception e)
      {
        println(e);
      }
      delay(3000);
      roboterDrehen(PI/2);
      warten();
      fahren(a - 2 * schwammdistanz);
      warten();
    } else {
      println("else");
      roboterDrehen(-PI/2);
      warten();
      val = 'v';
      try
      {
        outToServer.writeBytes(val + str(schwammumdrehungen));
      }
      catch (Exception e)
      {
        println(e);
      }
      delay(3000);
      roboterDrehen(-PI/2);
      warten();
      fahren(a - 2 * schwammdistanz);
      warten();
    }
  }
}
 
void servo() {
  if (angekommen) //Servo Runter lassen
  {
    val = 'R';
    try
    {
      outToServer.write(val);
    }
    catch (Exception e)
    {
      println(e);
    }
    delay(1000);
  } else { //Servo hochfahren
    val = 'H';
    try
    {
      outToServer.write(val);
    }
    catch (Exception e)
    {
      println(e);
    }
    delay(1000);
  }
}
 
//empfaengt und speichert Variable vom Input
char antwortArduino()
{
  try
  {
    char input = ' ';
    while (inFromServer.available() != 0)
    {
      input = (char) inFromServer.read();
      return input;
    }
  }
  catch (Exception e)
  {
    println(e);
  }
  return ' ';
}
 
//wartet bis der Arduino seine Aktion zu Ende ausgefuehrt hat
void warten()
{
  char signal = ' ';
  do {
    signal = antwortArduino();
  } while (signal != 'f');
}

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.