Update

ArduinoOutput/preproc/ctags_target_for_gcc_minus_e.cpp

@@ -1,7 +1,7 @@
 # 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 # 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018 
 //Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe 
 //der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen). 
 //Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
@@ -58,6 +58,7 @@ void BlueSetup()
   Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
   Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
   if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");
+  if(Serial1.available() > 0) Serial1.read(); //Verwerfen der alten Informationen aus dem Puffer
 }
 
 //Setup Liniensensoren
@@ -66,27 +67,27 @@ void LineSetup()
   ledYellow(1);
   lineSensors.initThreeSensors(); //Initialisierung von 3 Sensoren (max 5)
 
-  // //Kalibrierung
-  // uint32_t total[3] = {0, 0, 0};  
-  // for(uint8_t i = 0; i < 120; i++)                                
-  // {
-  //   if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
-  //   else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
-  //   lineSensors.read(lineOffset);  
-  //   total[0] += lineOffset[0];
-  //   total[1] += lineOffset[1];
-  //   total[2] += lineOffset[2];
-    lineSensors.calibrate();
-  // }
-  // motors.setSpeeds(0, 0);
-  // lineOffset[0] = total[0] / 120;
-  // lineOffset[1] = total[1] / 120;
-  // lineOffset[2] = total[2] / 120;
+  //Kalibrierung
+  uint32_t total[3] = {0, 0, 0};
+  for(uint8_t i = 0; i < 120; i++)
+  {
+    if (i > 30 && i <= 90) motors.setSpeeds(106, -100);
+    else motors.setSpeeds(-106, 100);
+    lineSensors.read(lineOffset);
+    total[0] += lineOffset[0];
+    total[1] += lineOffset[1];
+    total[2] += lineOffset[2];
+   lineSensors.calibrate();
+  }
+  motors.setSpeeds(0, 0);
+  lineOffset[0] = total[0] / 120;
+  lineOffset[1] = total[1] / 120;
+  lineOffset[2] = total[2] / 120;
   ledYellow(0);
 
-  lineOffset[0] = 240;
-  lineOffset[1] = 120;
-  lineOffset[2] = 220;
+  // lineOffset[0] = 240;
+  // lineOffset[1] = 120;
+  // lineOffset[2] = 220;
 }
 
 //Setup Abstandssensoren
@@ -106,8 +107,8 @@ void GyroSetup()
     ledRed(1);
     while(1)
     {
-      //Serial1.println("Fehler Drehbewegungssensors");
-      delay(1000);
+      Serial.println("Fehler Drehbewegungssensors");
+      delay(5000);
     }
   }
   gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
@@ -139,8 +140,8 @@ void CompassSetup()
     ledRed(1);
     while(1)
     {
-      //Serial1.println("Fehler Beschleunigungssensors");
-      delay(1000);
+      Serial.println("Fehler Beschleunigungssensors");
+      delay(5000);
     }
   }
   compass.enableDefault();
@@ -296,27 +297,26 @@ void EncoderUpdate()
 void Kontaktvermeiden()
 {
   //Serial1.println("Kontaktvermeiden");
-  Serial1.println(1);
+  Serial1.print(1);
   ledRed(1);
-
   motors.setSpeeds(0, 0);
-  delay(500);
+  delay(1000);
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
-    motors.setSpeeds(-54, -50);
-    if(lineValue[0] > 150 || lineValue[2] > 170) break;
+    motors.setSpeeds(-53, -50);
+    if(lineValue[0] > 150 || lineValue[2] > 120) break;
   }
-  delay(500);
+  motors.setSpeeds(0, 0);
   //Serial1.println("Vermeiden beendet");
-  Serial1.println(0);
+  Serial1.print(0);
 }
 
 //Funktion Hindernisumfahrung
 void Hindernisumfahren()
 {
   //Serial1.println("Hindernisumfahren"); 
-  Serial1.println(1);
+  Serial1.print(1);
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
@@ -329,14 +329,14 @@ void Hindernisumfahren()
     GyroUpdate();
     LineUpdate();
     motors.setSpeeds(27, 100);
-    if(lineValue[2] > 170 && lineValue[2] < 450) break;
+    if(lineValue[2] > 120 && lineValue[2] < 500) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[2] < 170)
+  while(lineValue[2] < 120)
   {
     LineUpdate();
-    motors.setSpeeds(107, 100);
+    motors.setSpeeds(106, 100);
     //if(lineValue[0] > MARKLINE0) break;
   }
   //rechts drehen
@@ -345,12 +345,12 @@ void Hindernisumfahren()
   {
     GyroUpdate();
     LineUpdate();
-    if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
-    else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 100);
-    else motors.setSpeeds(107, 25);
+    if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
+    else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 100);
+    else motors.setSpeeds(106, 25);
   }
   //geradeaus fahren
-  motors.setSpeeds(107, 100);
+  motors.setSpeeds(106, 100);
 
   //Gegenverkehr beachten
   ProxUpdate();
@@ -395,15 +395,15 @@ void Hindernisumfahren()
   {
     GyroUpdate();
     LineUpdate();
-    motors.setSpeeds(107, 25);
-    if(lineValue[0] > 150 && lineValue[0] < 400) break;
+    motors.setSpeeds(106, 25);
+    if(lineValue[0] > 150 && lineValue[0] < 500) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[0] < 150)
   {
     LineUpdate();
-    motors.setSpeeds(107, 100);
+    motors.setSpeeds(106, 100);
     //if(lineValue[0] > MARKLINE0) break;
   }
   //links drehen
@@ -412,14 +412,14 @@ void Hindernisumfahren()
   {
     GyroUpdate();
     LineUpdate();
-    if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
-    else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
+    if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
+    else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 100);
     else motors.setSpeeds(27, 100);
   }
   //geradeaus fahren
   //Serial1.println("Umfahren beendet");
-  Serial1.println(0);
-  motors.setSpeeds(107, 100);
+  Serial1.print(0);
+  motors.setSpeeds(106, 100);
 }
 
 //Funktion Abbiegen
@@ -427,70 +427,65 @@ void Abbiegen()
 {
   ledYellow(1);
   //Serial1.println("Abbiegen");  
-  Serial1.println(1);
+  Serial1.print(1);
 
   //Markierung analysieren
   LineUpdate();
   bool leftCode; //links => 1
   bool rightCode; //rechts => 2
-  if(lineValue[0] > 400) leftCode = true;
+  if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > 450) rightCode = true;
+  if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   uint8_t randy; //geradeaus => 0
   if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
-  else if(leftCode == true && rightCode == false) randy = 1;
-  // else if(leftCode == true && rightCode == false) 
-  // {
-  //   randy = random(0, 2);                                                 //0, 1
-  //   if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
-  // }
-  else if(leftCode == false && rightCode == true) randy = 2;
-  // else if(leftCode == false && rightCode == true)
-  // {
-  //   randy = random(3);                                                    //0, (1), 2
-  //   if(randy == 0) randy = random(3);                                     //erhöht Wahrscheinlickeit abzubiegen
-  //   while(randy == 1) randy = random(3);                                  //!1 => 0, 2
-  // }
+  else if(leftCode == true && rightCode == false) //randy = 1;
+  {
+    randy = random(0, 2); //0, 1
+    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+  }
+  else if(leftCode == false && rightCode == true) //randy = 2;
+  {
+    randy = random(0, 2); //0, 1
+    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+    if(randy == 1) randy = 2; //!1 => 2
+  }
 
   //links Abbiegen (von der Verbindungsstrecke)
   if(randy == 1 && rightCode == true)
   {
     //Serial1.println("links Abbiegen von der Verbindungsstrecke"); 
     //geradeaus zur Mittellinie fahren
-    motors.setSpeeds(107, 100 +10);
+    motors.setSpeeds(106, 100 +10);
     lastUpdate = millis();
     TimeUpdate();
     while(eventTime < 300)
     {
       TimeUpdate();
-      //Serial1.println(eventTime); 
-      motors.setSpeeds(107, 100 +10);
+      motors.setSpeeds(106, 100 +10);
     }
     LineUpdate();
-    //Serial1.println("Linie suchen"); 
-    while(lineValue[0] < 150 && lineValue[2] < 170)
+    while(lineValue[0] < 150 && lineValue[2] < 120)
     {
       LineUpdate();
-      motors.setSpeeds(107, 100);
+      motors.setSpeeds(106, 100);
     }
     //links drehen
     turnAngle = 0;
     rotationAngle = 0;
     GyroUpdate();
-    //Serial1.println("Drehen"); 
     while(rotationAngle < 90)
     {
       GyroUpdate();
       LineUpdate();
-      if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
-      else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
+      if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
+      else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(53, 100);
       else motors.setSpeeds(27, 100);
     }
     //geradeaus fahren
-    motors.setSpeeds(107, 100);
+    motors.setSpeeds(106, 100);
   }
 
   //links Abbiegen (vom Rundkurs)
@@ -507,7 +502,7 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       motors.setSpeeds(27, 100);
-      if(driveRotation[1] > 1) break;
+      //if(driveRotation[1] > 1) break;
     }
     driveRotation[1] = 0;
     GyroUpdate();
@@ -516,23 +511,32 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       motors.setSpeeds(27, 100);
-      if(driveRotation[1] > 1) break;
+      //if(driveRotation[1] > 1) break;
     }
     //geradeaus fahren
-    motors.setSpeeds(107, 100);
+    motors.setSpeeds(106, 100);
     lastUpdate = millis();
     TimeUpdate();
     while(eventTime < 1000)
     {
       TimeUpdate();
       LineUpdate();
-      if(lineValue[2] > 170 && lineValue[2] < 450) break;
+      if(lineValue[2] > 120 && lineValue[2] < 500) break;
+    }
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
     }
   }
 
   //rechts Abbiegen
   else if(randy == 2 && rightCode == true)
   {
+    //Serial1.println("rechts Abbiegen"); 
     //rechts drehen
     turnAngle = 0;
     rotationAngle = 0;
@@ -543,8 +547,8 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       LineUpdate();
-      motors.setSpeeds(107, 25);
-      if(driveRotation[0] > 1 || (lineValue[0] > 150 && lineValue[0] < 400)) break;
+      motors.setSpeeds(106, 25);
+      if(lineValue[0] > 150 && lineValue[0] < 500) break;
     }
     GyroUpdate();
     lastUpdate = millis();
@@ -554,17 +558,18 @@ void Abbiegen()
       GyroUpdate();
       LineUpdate();
       if(eventTime > 3000) break;
-      if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
+      if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
       //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
-      else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 50);
-      else motors.setSpeeds(107, 25);
+      else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 50);
+      else motors.setSpeeds(106, 25);
     }
   }
 
   //nicht Abbiegen, geradeaus fahren
   else
   {
-    motors.setSpeeds(107, 100);
+    //Serial1.println("nicht Abbiegen");
+    motors.setSpeeds(106, 100);
     lastUpdate = millis();
     TimeUpdate();
     while(eventTime < 1000)
@@ -575,57 +580,70 @@ void Abbiegen()
     }
   }
   //Serial1.println("Abbiegen beendet");
-  Serial1.println(0);
+  Serial1.print(0);
 }
 
 //Funktion Spurhalten
 void Spurhalten()
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > 150 && lineValue[0] < 400)
+  if(lineValue[0] > 150 && lineValue[0] < 500)
   {
     ledYellow(1);
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(107/eventSpeed, 25/eventSpeed);
-    delay(100);
+    motors.setSpeeds(106/eventSpeed, 25/eventSpeed);
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 100)
+    {
+      TimeUpdate();
+      LineUpdate();
+      if(lineValue[2] > 120) break;
+    }
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > 170 && lineValue[2] < 450)
+  else if(lineValue[2] > 120 && lineValue[2] < 500)
   {
     ledYellow(1);
     //Serial1.println("Spur nach links korrigieren");  
     motors.setSpeeds(27/eventSpeed, 100/eventSpeed);
-    delay(100);
-  }
-
-  //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > 100 && lineValue[1] < 350)
-  {
-    ledRed(1);
-    //Serial1.println("Spurfinden"); 
-    Serial1.println(1);
     lastUpdate = millis();
-    while(lineValue[0] < 150 && lineValue[2] < 170) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+    TimeUpdate();
+    while(eventTime < 100)
     {
       TimeUpdate();
       LineUpdate();
-      motors.setSpeeds(-107/eventSpeed, -100/eventSpeed);
-      if(eventTime > 3000) break;
+      if(lineValue[0] > 150) break;
     }
-    delay(100);
-    //Serial1.println("Spur gefunden"); 
-    Serial1.println(0);
   }
 
   //normale Fahrt
   else
   {
     ledGreen(1);
-    motors.setSpeeds(107/eventSpeed, 100/eventSpeed);
+    motors.setSpeeds(106/eventSpeed, 100/eventSpeed);
   }
 }
 
+//Funktion Spurfinden
+void Spurfinden()
+{
+  ledRed(1);
+  //Serial1.println("Spurfinden"); 
+  Serial1.print(1);
+  lastUpdate = millis();
+  while(lineValue[0] < 150 && lineValue[2] < 120) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+  {
+    TimeUpdate();
+    LineUpdate();
+    motors.setSpeeds(-106, -100);
+    if(eventTime > 3000) break;
+  }
+  //Serial1.println("Spur gefunden"); 
+  Serial1.print(0);
+}
+
 //Funktion Serielle Ausgabe
 void SerielleAusgabe()
 {
@@ -644,17 +662,17 @@ void SerielleAusgabe()
 
 void loop()
 {
-  //LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
+  //LoopTiming();                             //Zykluszeit beträgt max. 20ms im Idle
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
 
-  //Abfragen der Sensordaten
-  LineUpdate();
+  //Abfragen der Sensordaten                                 
   ProxUpdate();
   EncoderUpdate();
   GyroUpdate();
   CompassUpdate();
+  LineUpdate();
   TimeUpdate();
 
   //Funktionsauswahl
@@ -667,18 +685,20 @@ void loop()
   if(alarm == true)
   {
     //Serial1.println("Verstanden");
-    eventSpeed = 2;
-    if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > 400 || lineValue[2] > 450) motors.setSpeeds(0, 0);
+    eventSpeed = 1.4;
+    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > 500 || lineValue[2] > 500 || lineValue[1] > 100) motors.setSpeeds(0, 0);
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else
   {
-    eventSpeed = 1;
+    eventSpeed = 1.0;
     //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
-    if(proxValue[2] == 6) Hindernisumfahren();
-    else if(lineValue[0] > 400 && lineValue[0] < 1000 || lineValue[2] > 450 && lineValue[2] < 1000) Abbiegen();
+    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    else if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) Abbiegen();
+    else if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) Abbiegen();
+    else if(lineValue[1] > 100 && lineValue[1] < 300) Spurfinden();
     else Spurhalten();
   }
-  //LoopTiming();                                                       
+  //LoopTiming();                                                     
 }

ArduinoOutput/sketch/Hauptprojekt.ino.cpp

@@ -2,7 +2,7 @@
 #line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 #line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018 
 //Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe 
 //der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen). 
 //Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
@@ -26,13 +26,13 @@ L3G                       gyro;         //Drehbewegungssensor z-Achse
 
 #define   MARKLINE0       150
 #define   MARKLINE1       100
-#define   MARKLINE2       170
-#define   SIGN0           400
-#define   SIGN1           350
-#define   SIGN2           450
+#define   MARKLINE2       120
+#define   SIGN0           500
+#define   SIGN1           300
+#define   SIGN2           500
 #define   MAXSPEED        400
-#define   FULLSPEEDLEFT   107
-#define   HALFSPEEDLEFT   54
+#define   FULLSPEEDLEFT   106
+#define   HALFSPEEDLEFT   53
 #define   SLOWSPEEDLEFT   27
 #define   FULLSPEEDRIGHT  100
 #define   HALFSPEEDRIGHT  50
@@ -70,48 +70,51 @@ bool      alarm = false;                //zeigt Manöver abhängig von btData
 //Setup Bluetoothverbindung
 #line 68 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void BlueSetup();
-#line 76 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 77 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineSetup();
-#line 105 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 106 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxSetup();
-#line 111 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 112 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroSetup();
-#line 144 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 145 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassSetup();
-#line 175 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 176 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void setup();
-#line 201 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 202 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void TimeUpdate();
-#line 207 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 208 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LoopTiming();
-#line 251 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 252 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxUpdate();
-#line 261 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 262 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineUpdate();
-#line 271 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 272 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroUpdate();
-#line 284 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 285 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassUpdate();
-#line 297 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 298 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void EncoderUpdate();
-#line 308 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 309 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Kontaktvermeiden();
 #line 328 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Hindernisumfahren();
 #line 438 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Abbiegen();
-#line 594 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 599 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurhalten();
 #line 642 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void Spurfinden();
+#line 660 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void SerielleAusgabe();
-#line 657 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 675 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void loop();
 #line 68 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void BlueSetup()
 {
-  Serial1.begin(9600);                      //Initialisierung mit Datengeschwindigkeit(Baud)
-  Serial1.setTimeout(10);                   //verkürzt Serial(1).read auf 10 ms statt 1000 ms
-  if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");      
+  Serial1.begin(9600);                              //Initialisierung mit Datengeschwindigkeit(Baud)
+  Serial1.setTimeout(10);                           //verkürzt Serial(1).read auf 10 ms statt 1000 ms
+  if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");   
+  if(Serial1.available() > 0) Serial1.read();       //Verwerfen der alten Informationen aus dem Puffer
 }           
 
 //Setup Liniensensoren
@@ -120,27 +123,27 @@ void LineSetup()
   ledYellow(1);
   lineSensors.initThreeSensors();           //Initialisierung von 3 Sensoren (max 5)
   
-  // //Kalibrierung
-  // uint32_t total[3] = {0, 0, 0};  
-  // for(uint8_t i = 0; i < 120; i++)                                
-  // {
-  //   if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
-  //   else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
-  //   lineSensors.read(lineOffset);  
-  //   total[0] += lineOffset[0];
-  //   total[1] += lineOffset[1];
-  //   total[2] += lineOffset[2];
-    lineSensors.calibrate();                                      
-  // }
-  // motors.setSpeeds(0, 0);
-  // lineOffset[0] = total[0] / 120;
-  // lineOffset[1] = total[1] / 120;
-  // lineOffset[2] = total[2] / 120;
+  //Kalibrierung
+  uint32_t total[3] = {0, 0, 0};  
+  for(uint8_t i = 0; i < 120; i++)                                
+  {
+    if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+    else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
+    lineSensors.read(lineOffset);  
+    total[0] += lineOffset[0];
+    total[1] += lineOffset[1];
+    total[2] += lineOffset[2];
+   lineSensors.calibrate();                                      
+  }
+  motors.setSpeeds(0, 0);
+  lineOffset[0] = total[0] / 120;
+  lineOffset[1] = total[1] / 120;
+  lineOffset[2] = total[2] / 120;
   ledYellow(0);
 
-  lineOffset[0] = 240;
-  lineOffset[1] = 120;
-  lineOffset[2] = 220;
+  // lineOffset[0] = 240;
+  // lineOffset[1] = 120;
+  // lineOffset[2] = 220;
 }
 
 //Setup Abstandssensoren
@@ -160,8 +163,8 @@ void GyroSetup()
     ledRed(1);
     while(1)
     {
-      //Serial1.println("Fehler Drehbewegungssensors");
-      delay(1000);
+      Serial.println("Fehler Drehbewegungssensors");
+      delay(5000);
     }
   }
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
@@ -193,8 +196,8 @@ void CompassSetup()
     ledRed(1);
     while(1)
     {
-      //Serial1.println("Fehler Beschleunigungssensors");
-      delay(1000);
+      Serial.println("Fehler Beschleunigungssensors");
+      delay(5000);
     }
   }
   compass.enableDefault();  
@@ -350,27 +353,26 @@ void EncoderUpdate()
 void Kontaktvermeiden()                                            
 {
   //Serial1.println("Kontaktvermeiden");
-  Serial1.println(1); 
+  Serial1.print(1); 
   ledRed(1);
-
   motors.setSpeeds(0, 0);
-  delay(500);
+  delay(1000);
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
     motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
     if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
   }
-  delay(500); 
+  motors.setSpeeds(0, 0);
   //Serial1.println("Vermeiden beendet");
-  Serial1.println(0); 
+  Serial1.print(0); 
 }
 
 //Funktion Hindernisumfahrung
 void Hindernisumfahren()                                              
 {
   //Serial1.println("Hindernisumfahren"); 
-  Serial1.println(1); 
+  Serial1.print(1); 
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
@@ -472,7 +474,7 @@ void Hindernisumfahren()
   }
   //geradeaus fahren
   //Serial1.println("Umfahren beendet");
-  Serial1.println(0); 
+  Serial1.print(0); 
   motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
 }
 
@@ -481,33 +483,31 @@ void Abbiegen()
 {
   ledYellow(1); 
   //Serial1.println("Abbiegen");  
-  Serial1.println(1); 
+  Serial1.print(1); 
 
   //Markierung analysieren
   LineUpdate(); 
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if(lineValue[0] > SIGN0) leftCode = true;
+  if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > SIGN2) rightCode = true;
+  if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   uint8_t randy;                                                          //geradeaus => 0
   if(leftCode == true && rightCode == true) randy = random(1, 3);         //1, 2
-  else if(leftCode == true && rightCode == false) randy = 1;
-  // else if(leftCode == true && rightCode == false) 
-  // {
-  //   randy = random(0, 2);                                                 //0, 1
-  //   if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
-  // }
-  else if(leftCode == false && rightCode == true) randy = 2;
-  // else if(leftCode == false && rightCode == true)
-  // {
-  //   randy = random(3);                                                    //0, (1), 2
-  //   if(randy == 0) randy = random(3);                                     //erhöht Wahrscheinlickeit abzubiegen
-  //   while(randy == 1) randy = random(3);                                  //!1 => 0, 2
-  // }
+  else if(leftCode == true && rightCode == false) //randy = 1;
+  {
+    randy = random(0, 2);                                                 //0, 1
+    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+  }
+  else if(leftCode == false && rightCode == true) //randy = 2;
+  {
+    randy = random(0, 2);                                                 //0, 1
+    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+    if(randy == 1) randy = 2;                                             //!1 => 2
+  }
 
   //links Abbiegen (von der Verbindungsstrecke)
   if(randy == 1 && rightCode == true) 
@@ -520,11 +520,9 @@ void Abbiegen()
     while(eventTime < 300)
     {
       TimeUpdate();
-      //Serial1.println(eventTime); 
       motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
     }
     LineUpdate();
-    //Serial1.println("Linie suchen"); 
     while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
     {
       LineUpdate();
@@ -534,13 +532,12 @@ void Abbiegen()
     turnAngle = 0;  
     rotationAngle = 0;
     GyroUpdate();
-    //Serial1.println("Drehen"); 
     while(rotationAngle < 90)
     {
       GyroUpdate();
       LineUpdate();
       if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT); 
-      else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+      else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT); 
       else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
     }
     //geradeaus fahren
@@ -561,7 +558,7 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      if(driveRotation[1] > 1) break;
+      //if(driveRotation[1] > 1) break;
     }
     driveRotation[1] = 0;
     GyroUpdate();
@@ -570,7 +567,7 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      if(driveRotation[1] > 1) break;
+      //if(driveRotation[1] > 1) break;
     }
     //geradeaus fahren
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
@@ -582,11 +579,20 @@ void Abbiegen()
       LineUpdate();
       if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
     }
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
   }
 
   //rechts Abbiegen
   else if(randy == 2 && rightCode == true)
   {
+    //Serial1.println("rechts Abbiegen"); 
     //rechts drehen
     turnAngle = 0;  
     rotationAngle = 0;
@@ -598,7 +604,7 @@ void Abbiegen()
       EncoderUpdate();
       LineUpdate();
       motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
-      if(driveRotation[0] > 1 || (lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)) break;
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
     }
     GyroUpdate();
     lastUpdate = millis();
@@ -618,6 +624,7 @@ void Abbiegen()
   //nicht Abbiegen, geradeaus fahren
   else 
   {
+    //Serial1.println("nicht Abbiegen");
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     lastUpdate = millis();
     TimeUpdate();
@@ -629,7 +636,7 @@ void Abbiegen()
     }
   }
   //Serial1.println("Abbiegen beendet");
-  Serial1.println(0); 
+  Serial1.print(0); 
 }
 
 //Funktion Spurhalten
@@ -641,7 +648,14 @@ void Spurhalten()
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
     motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
-    delay(100);    
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 100)
+    {
+      TimeUpdate();
+      LineUpdate();
+      if(lineValue[2] > MARKLINE2) break;
+    }   
   }
 
   //rechte Linie erreicht, nach links fahren
@@ -649,28 +663,16 @@ void Spurhalten()
   {
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);  
-    delay(100);  
-  }
-
-  //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1)                 
-  { 
-    ledRed(1); 
-    //Serial1.println("Spurfinden"); 
-    Serial1.println(1); 
+    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);   
     lastUpdate = millis();
-    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+    TimeUpdate();
+    while(eventTime < 100)
     {
       TimeUpdate();
       LineUpdate();
-      motors.setSpeeds(-FULLSPEEDLEFT/eventSpeed, -FULLSPEEDRIGHT/eventSpeed);
-      if(eventTime > 3000) break;
-    }
-    delay(100);    
-    //Serial1.println("Spur gefunden"); 
-    Serial1.println(0);  
-  }  
+      if(lineValue[0] > MARKLINE0) break;
+    }   
+  }
 
   //normale Fahrt
   else                                                                
@@ -680,6 +682,24 @@ void Spurhalten()
   }
 }
 
+//Funktion Spurfinden
+void Spurfinden()             
+{ 
+  ledRed(1); 
+  //Serial1.println("Spurfinden"); 
+  Serial1.print(1); 
+  lastUpdate = millis();
+  while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+  {
+    TimeUpdate();
+    LineUpdate();
+    motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+    if(eventTime > 3000) break;
+  } 
+  //Serial1.println("Spur gefunden"); 
+  Serial1.print(0);  
+}  
+
 //Funktion Serielle Ausgabe
 void SerielleAusgabe()                                                     
 {
@@ -698,17 +718,17 @@ void SerielleAusgabe()
 
 void loop() 
 {
-  //LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
+  //LoopTiming();                             //Zykluszeit beträgt max. 20ms im Idle
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
 
-  //Abfragen der Sensordaten
-  LineUpdate();                                         
+  //Abfragen der Sensordaten                                 
   ProxUpdate();
   EncoderUpdate();
   GyroUpdate();
   CompassUpdate();
+  LineUpdate();      
   TimeUpdate();
 
   //Funktionsauswahl
@@ -721,19 +741,20 @@ void loop()
   if(alarm == true)                                    
   {
     //Serial1.println("Verstanden");
-    eventSpeed = 2;
-    if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > SIGN0 || lineValue[2] > SIGN2) motors.setSpeeds(0, 0);  
+    eventSpeed = 1.4;
+    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
-    eventSpeed = 1;
+    eventSpeed = 1.0;
     //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
-    if(proxValue[2] == 6) Hindernisumfahren();
-    else if(lineValue[0] > SIGN0 && lineValue[0] < 1000 || lineValue[2] > SIGN2 && lineValue[2] < 1000) Abbiegen();
+    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    else if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
+    else if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
+    else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
     else Spurhalten();
   }
-  //LoopTiming();                                                       
+  //LoopTiming();                                                     
 }
-

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,5 +1,5 @@
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018 
 //Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe 
 //der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen). 
 //Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
@@ -23,13 +23,13 @@ L3G                       gyro;         //Drehbewegungssensor z-Achse
 
 #define   MARKLINE0       150
 #define   MARKLINE1       100
-#define   MARKLINE2       170
-#define   SIGN0           400
-#define   SIGN1           350
-#define   SIGN2           450
+#define   MARKLINE2       120
+#define   SIGN0           500
+#define   SIGN1           300
+#define   SIGN2           500
 #define   MAXSPEED        400
-#define   FULLSPEEDLEFT   107
-#define   HALFSPEEDLEFT   54
+#define   FULLSPEEDLEFT   106
+#define   HALFSPEEDLEFT   53
 #define   SLOWSPEEDLEFT   27
 #define   FULLSPEEDRIGHT  100
 #define   HALFSPEEDRIGHT  50
@@ -67,9 +67,10 @@ bool      alarm = false;                //zeigt Manöver abhängig von btData
 //Setup Bluetoothverbindung
 void BlueSetup()
 {
-  Serial1.begin(9600);                      //Initialisierung mit Datengeschwindigkeit(Baud)
-  Serial1.setTimeout(10);                   //verkürzt Serial(1).read auf 10 ms statt 1000 ms
-  if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");      
+  Serial1.begin(9600);                              //Initialisierung mit Datengeschwindigkeit(Baud)
+  Serial1.setTimeout(10);                           //verkürzt Serial(1).read auf 10 ms statt 1000 ms
+  if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");   
+  if(Serial1.available() > 0) Serial1.read();       //Verwerfen der alten Informationen aus dem Puffer
 }           
 
 //Setup Liniensensoren
@@ -78,27 +79,27 @@ void LineSetup()
   ledYellow(1);
   lineSensors.initThreeSensors();           //Initialisierung von 3 Sensoren (max 5)
   
-  // //Kalibrierung
-  // uint32_t total[3] = {0, 0, 0};  
-  // for(uint8_t i = 0; i < 120; i++)                                
-  // {
-  //   if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
-  //   else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
-  //   lineSensors.read(lineOffset);  
-  //   total[0] += lineOffset[0];
-  //   total[1] += lineOffset[1];
-  //   total[2] += lineOffset[2];
-    lineSensors.calibrate();                                      
-  // }
-  // motors.setSpeeds(0, 0);
-  // lineOffset[0] = total[0] / 120;
-  // lineOffset[1] = total[1] / 120;
-  // lineOffset[2] = total[2] / 120;
+  //Kalibrierung
+  uint32_t total[3] = {0, 0, 0};  
+  for(uint8_t i = 0; i < 120; i++)                                
+  {
+    if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+    else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
+    lineSensors.read(lineOffset);  
+    total[0] += lineOffset[0];
+    total[1] += lineOffset[1];
+    total[2] += lineOffset[2];
+   lineSensors.calibrate();                                      
+  }
+  motors.setSpeeds(0, 0);
+  lineOffset[0] = total[0] / 120;
+  lineOffset[1] = total[1] / 120;
+  lineOffset[2] = total[2] / 120;
   ledYellow(0);
 
-  lineOffset[0] = 240;
-  lineOffset[1] = 120;
-  lineOffset[2] = 220;
+  // lineOffset[0] = 240;
+  // lineOffset[1] = 120;
+  // lineOffset[2] = 220;
 }
 
 //Setup Abstandssensoren
@@ -118,8 +119,8 @@ void GyroSetup()
     ledRed(1);
     while(1)
     {
-      //Serial1.println("Fehler Drehbewegungssensors");
-      delay(1000);
+      Serial.println("Fehler Drehbewegungssensors");
+      delay(5000);
     }
   }
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
@@ -151,8 +152,8 @@ void CompassSetup()
     ledRed(1);
     while(1)
     {
-      //Serial1.println("Fehler Beschleunigungssensors");
-      delay(1000);
+      Serial.println("Fehler Beschleunigungssensors");
+      delay(5000);
     }
   }
   compass.enableDefault();  
@@ -308,27 +309,26 @@ void EncoderUpdate()
 void Kontaktvermeiden()                                            
 {
   //Serial1.println("Kontaktvermeiden");
-  Serial1.println(1); 
+  Serial1.print(1); 
   ledRed(1);
-
   motors.setSpeeds(0, 0);
-  delay(500);
+  delay(1000);
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
     motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
     if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
   }
-  delay(500); 
+  motors.setSpeeds(0, 0);
   //Serial1.println("Vermeiden beendet");
-  Serial1.println(0); 
+  Serial1.print(0); 
 }
 
 //Funktion Hindernisumfahrung
 void Hindernisumfahren()                                              
 {
   //Serial1.println("Hindernisumfahren"); 
-  Serial1.println(1); 
+  Serial1.print(1); 
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
@@ -430,7 +430,7 @@ void Hindernisumfahren()
   }
   //geradeaus fahren
   //Serial1.println("Umfahren beendet");
-  Serial1.println(0); 
+  Serial1.print(0); 
   motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
 }
 
@@ -439,33 +439,31 @@ void Abbiegen()
 {
   ledYellow(1); 
   //Serial1.println("Abbiegen");  
-  Serial1.println(1); 
+  Serial1.print(1); 
 
   //Markierung analysieren
   LineUpdate(); 
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if(lineValue[0] > SIGN0) leftCode = true;
+  if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > SIGN2) rightCode = true;
+  if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   uint8_t randy;                                                          //geradeaus => 0
   if(leftCode == true && rightCode == true) randy = random(1, 3);         //1, 2
-  else if(leftCode == true && rightCode == false) randy = 1;
-  // else if(leftCode == true && rightCode == false) 
-  // {
-  //   randy = random(0, 2);                                                 //0, 1
-  //   if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
-  // }
-  else if(leftCode == false && rightCode == true) randy = 2;
-  // else if(leftCode == false && rightCode == true)
-  // {
-  //   randy = random(3);                                                    //0, (1), 2
-  //   if(randy == 0) randy = random(3);                                     //erhöht Wahrscheinlickeit abzubiegen
-  //   while(randy == 1) randy = random(3);                                  //!1 => 0, 2
-  // }
+  else if(leftCode == true && rightCode == false) //randy = 1;
+  {
+    randy = random(0, 2);                                                 //0, 1
+    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+  }
+  else if(leftCode == false && rightCode == true) //randy = 2;
+  {
+    randy = random(0, 2);                                                 //0, 1
+    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+    if(randy == 1) randy = 2;                                             //!1 => 2
+  }
 
   //links Abbiegen (von der Verbindungsstrecke)
   if(randy == 1 && rightCode == true) 
@@ -478,11 +476,9 @@ void Abbiegen()
     while(eventTime < 300)
     {
       TimeUpdate();
-      //Serial1.println(eventTime); 
       motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
     }
     LineUpdate();
-    //Serial1.println("Linie suchen"); 
     while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
     {
       LineUpdate();
@@ -492,13 +488,12 @@ void Abbiegen()
     turnAngle = 0;  
     rotationAngle = 0;
     GyroUpdate();
-    //Serial1.println("Drehen"); 
     while(rotationAngle < 90)
     {
       GyroUpdate();
       LineUpdate();
       if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT); 
-      else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+      else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT); 
       else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
     }
     //geradeaus fahren
@@ -519,7 +514,7 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      if(driveRotation[1] > 1) break;
+      //if(driveRotation[1] > 1) break;
     }
     driveRotation[1] = 0;
     GyroUpdate();
@@ -528,7 +523,7 @@ void Abbiegen()
       GyroUpdate();
       EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      if(driveRotation[1] > 1) break;
+      //if(driveRotation[1] > 1) break;
     }
     //geradeaus fahren
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
@@ -540,11 +535,20 @@ void Abbiegen()
       LineUpdate();
       if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
     }
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
   }
 
   //rechts Abbiegen
   else if(randy == 2 && rightCode == true)
   {
+    //Serial1.println("rechts Abbiegen"); 
     //rechts drehen
     turnAngle = 0;  
     rotationAngle = 0;
@@ -556,7 +560,7 @@ void Abbiegen()
       EncoderUpdate();
       LineUpdate();
       motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
-      if(driveRotation[0] > 1 || (lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)) break;
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
     }
     GyroUpdate();
     lastUpdate = millis();
@@ -576,6 +580,7 @@ void Abbiegen()
   //nicht Abbiegen, geradeaus fahren
   else 
   {
+    //Serial1.println("nicht Abbiegen");
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     lastUpdate = millis();
     TimeUpdate();
@@ -587,7 +592,7 @@ void Abbiegen()
     }
   }
   //Serial1.println("Abbiegen beendet");
-  Serial1.println(0); 
+  Serial1.print(0); 
 }
 
 //Funktion Spurhalten
@@ -599,7 +604,14 @@ void Spurhalten()
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
     motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
-    delay(100);    
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 100)
+    {
+      TimeUpdate();
+      LineUpdate();
+      if(lineValue[2] > MARKLINE2) break;
+    }   
   }
 
   //rechte Linie erreicht, nach links fahren
@@ -607,28 +619,16 @@ void Spurhalten()
   {
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);  
-    delay(100);  
-  }
-
-  //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1)                 
-  { 
-    ledRed(1); 
-    //Serial1.println("Spurfinden"); 
-    Serial1.println(1); 
+    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);   
     lastUpdate = millis();
-    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+    TimeUpdate();
+    while(eventTime < 100)
     {
       TimeUpdate();
       LineUpdate();
-      motors.setSpeeds(-FULLSPEEDLEFT/eventSpeed, -FULLSPEEDRIGHT/eventSpeed);
-      if(eventTime > 3000) break;
-    }
-    delay(100);    
-    //Serial1.println("Spur gefunden"); 
-    Serial1.println(0);  
-  }  
+      if(lineValue[0] > MARKLINE0) break;
+    }   
+  }
 
   //normale Fahrt
   else                                                                
@@ -638,6 +638,24 @@ void Spurhalten()
   }
 }
 
+//Funktion Spurfinden
+void Spurfinden()             
+{ 
+  ledRed(1); 
+  //Serial1.println("Spurfinden"); 
+  Serial1.print(1); 
+  lastUpdate = millis();
+  while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+  {
+    TimeUpdate();
+    LineUpdate();
+    motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+    if(eventTime > 3000) break;
+  } 
+  //Serial1.println("Spur gefunden"); 
+  Serial1.print(0);  
+}  
+
 //Funktion Serielle Ausgabe
 void SerielleAusgabe()                                                     
 {
@@ -656,17 +674,17 @@ void SerielleAusgabe()
 
 void loop() 
 {
-  //LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
+  //LoopTiming();                             //Zykluszeit beträgt max. 20ms im Idle
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
 
-  //Abfragen der Sensordaten
-  LineUpdate();                                         
+  //Abfragen der Sensordaten                                 
   ProxUpdate();
   EncoderUpdate();
   GyroUpdate();
   CompassUpdate();
+  LineUpdate();      
   TimeUpdate();
 
   //Funktionsauswahl
@@ -679,18 +697,20 @@ void loop()
   if(alarm == true)                                    
   {
     //Serial1.println("Verstanden");
-    eventSpeed = 2;
-    if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > SIGN0 || lineValue[2] > SIGN2) motors.setSpeeds(0, 0);  
+    eventSpeed = 1.4;
+    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
-    eventSpeed = 1;
+    eventSpeed = 1.0;
     //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
-    if(proxValue[2] == 6) Hindernisumfahren();
-    else if(lineValue[0] > SIGN0 && lineValue[0] < 1000 || lineValue[2] > SIGN2 && lineValue[2] < 1000) Abbiegen();
+    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    else if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
+    else if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
+    else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
     else Spurhalten();
   }
-  //LoopTiming();                                                       
-}
+  //LoopTiming();                                                     
+}