Added Filters for Proximity and Line Sensor Values; Solved Problem with Function TimeUpdate

ArduinoOutput/libraries/MedianFilter/MedianFilter.cpp.d

+c:\Users\Carolin\Downloads\VSCode\Zumo32U4\ArduinoOutput\libraries\MedianFilter\MedianFilter.cpp.o: \
+ C:\Users\Carolin\Documents\Arduino\libraries\MedianFilter\MedianFilter.cpp \
+ C:\Users\Carolin\Documents\Arduino\libraries\MedianFilter\MedianFilter.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/Arduino.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/binary.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/WCharacter.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/WString.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/HardwareSerial.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/Stream.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/Print.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/Printable.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/USBAPI.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/Arduino.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/USBDesc.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\cores\arduino/USBCore.h \
+ C:\Users\Carolin\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.6.20\variants\leonardo/pins_arduino.h

ArduinoOutput/preproc/ctags_target_for_gcc_minus_e.cpp

 # 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 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 
-//dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
-//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
-//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
-//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
-//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth). 
-//Das LCD kann bei Bluetoothnutzung nicht verwendet werden. 
-
-# 14 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 08.05.2018 
+//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
+/*Kurzbeschreibung: 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 
+
+dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
+
+Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
+
+genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
+
+Für die Filterung der Messwerte werden Medianfilter genutzt. Der Zumo kommuniziert über ein 
+
+Bluetooth-Modul (HC-05) mit anderen Geräten. Die Kommunikation erfolgt seriell ('SERIAL' für USB, 
+
+'SERIAL1' für Bluetooth). Das LCD kann bei Bluetoothnutzung nicht verwendet werden.*/
+# 14 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 # 15 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
+# 16 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
+# 17 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
 
 Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
 Zumo32U4LineSensors lineSensors; //Liniensensoren
 Zumo32U4Encoders encoders; //Motorencoder
 LSM303 compass; //Beschleunigungssensor x-Achse
 L3G gyro; //Drehbewegungssensor z-Achse
-# 38 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+
+MedianFilter LineFilter0(3, 0); //erstellen der Filter für Liniensensoren
+MedianFilter LineFilter1(3, 0); //Medianfilter geben mittleren Wert einer Reihe aus
+MedianFilter LineFilter2(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
+MedianFilter ProxFilter0(3, 0); //erstellen der Filter für Abstandssensoren
+MedianFilter ProxFilter1(3, 0);
+MedianFilter ProxFilter2(3, 0);
+MedianFilter ProxFilter3(3, 0);
+# 48 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 int16_t lineValue[3]; //Liniensensoren
 uint16_t lineOffset[3]; //von links (0) nach rechts (2)
 
   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);
+    if (i > 30 && i <= 90) motors.setSpeeds(104, -100);
+    else motors.setSpeeds(-104, 100);
     lineSensors.read(lineOffset);
     total[0] += lineOffset[0];
     total[1] += lineOffset[1];
 void ProxUpdate()
 {
   proxSensors.read();
-  proxValue[0] = proxSensors.countsLeftWithLeftLeds();
-  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValue[2] = proxSensors.countsFrontWithRightLeds();
-  proxValue[3] = proxSensors.countsRightWithRightLeds();
+  proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
+  proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
+  proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
+  proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
 }
 
 //Update Liniensensoren
 void LineUpdate()
 {
   uint16_t lineRaw[3];
-  lineSensors.read(lineRaw);
-  lineValue[0] = lineRaw[0] - lineOffset[0];
-  lineValue[1] = lineRaw[1] - lineOffset[1];
-  lineValue[2] = lineRaw[2] - lineOffset[2];
+  lineSensors.read(lineRaw); //lese Messwerte der Liniensensoren aus
+  lineValue[0] = LineFilter0.in(lineRaw[0] - lineOffset[0]); //ziehe Offsetwerte von Messwerten ab und gib diese in Filter ein
+  lineValue[1] = LineFilter1.in(lineRaw[1] - lineOffset[1]); //erhält neue mittlere Werte der Filter
+  lineValue[2] = LineFilter2.in(lineRaw[2] - lineOffset[2]); //"LineFilter.out" um gefilterte Werte auszugeben ohne neue Werte einzugeben
 }
 
 //Update Drehbewegungssensor
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
-    motors.setSpeeds(-53, -50);
+    motors.setSpeeds(-52, -50);
     if(lineValue[0] > 150 || lineValue[2] > 120) break;
   }
   motors.setSpeeds(0, 0);
   turnAngle = 0;
   rotationAngle = 0;
   GyroUpdate();
+  while(rotationAngle < 20)
+  {
+    GyroUpdate();
+    LineUpdate();
+    motors.setSpeeds(26, 100);
+  }
+  GyroUpdate();
   while(rotationAngle < 45)
   {
     GyroUpdate();
     LineUpdate();
-    motors.setSpeeds(27, 100);
+    motors.setSpeeds(26, 100);
     if(lineValue[2] > 120 && lineValue[2] < 500) break;
   }
   //geradeaus über Mittellinie fahren
   while(lineValue[2] < 120)
   {
     LineUpdate();
-    motors.setSpeeds(106, 100);
+    motors.setSpeeds(104, 100);
     //if(lineValue[0] > MARKLINE0) break;
   }
   //rechts drehen
   {
     GyroUpdate();
     LineUpdate();
-    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);
+    if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 0);
+    else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 100);
+    else motors.setSpeeds(104, 25);
   }
   //geradeaus fahren
-  motors.setSpeeds(106, 100);
+  motors.setSpeeds(104, 100);
 
   //Gegenverkehr beachten
   ProxUpdate();
     //Schritt 2: Hindernis passieren
     //Serial1.println("Aufholen"); 
     lastUpdate = millis();
-    while(proxValue[3] < 6 && eventTime < 3000)
+    while(proxValue[3] < 6)
     {
-      TimeUpdate();
       ProxUpdate();
       LineUpdate();
       Spurhalten();
+      TimeUpdate();
+      //Serial1.println(eventTime);
+      if(eventTime > 3000) break;
     }
-    //Serial1.println("Überholen"); 
+    //Serial1.println("Vorbeifahren"); 
     ProxUpdate();
     while(proxValue[3] == 6)
     {
     TimeUpdate();
     while(eventTime < 3000)
     {
-      //Serial1.println(eventTime); 
-      TimeUpdate();
       LineUpdate();
       Spurhalten();
+      TimeUpdate();
+      //Serial1.println(eventTime);
     }
   //}
 
   turnAngle = 0;
   rotationAngle = 0;
   GyroUpdate();
+  while(rotationAngle > -20)
+  {
+    GyroUpdate();
+    LineUpdate();
+    motors.setSpeeds(104, 25);
+  }
+  GyroUpdate();
   while(rotationAngle > -45)
   {
     GyroUpdate();
     LineUpdate();
-    motors.setSpeeds(106, 25);
+    motors.setSpeeds(104, 25);
     if(lineValue[0] > 150 && lineValue[0] < 500) break;
   }
   //geradeaus über Mittellinie fahren
   while(lineValue[0] < 150)
   {
     LineUpdate();
-    motors.setSpeeds(106, 100);
+    motors.setSpeeds(104, 100);
     //if(lineValue[0] > MARKLINE0) break;
   }
   //links drehen
     GyroUpdate();
     LineUpdate();
     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);
+    else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 100);
+    else motors.setSpeeds(26, 100);
   }
   //geradeaus fahren
   //Serial1.println("Umfahren beendet");
   Serial1.print(0);
-  motors.setSpeeds(106, 100);
+  motors.setSpeeds(104, 100);
 }
 
 //Funktion Abbiegen
   LineUpdate();
   bool leftCode; //links => 1
   bool rightCode; //rechts => 2
-  if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) leftCode = true;
+  if((lineValue[0] > 500 && lineValue[0] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) leftCode = true;
   else leftCode = false;
-  if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) rightCode = true;
+  if((lineValue[2] > 500 && lineValue[2] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   {
     //Serial1.println("links Abbiegen von der Verbindungsstrecke"); 
     //geradeaus zur Mittellinie fahren
-    motors.setSpeeds(106, 100 +10);
+    motors.setSpeeds(104, 100 +10);
     lastUpdate = millis();
     TimeUpdate();
-    while(eventTime < 300)
+    while(eventTime < 1000)
     {
       TimeUpdate();
-      motors.setSpeeds(106, 100 +10);
+      motors.setSpeeds(104, 100 +10);
     }
     LineUpdate();
     while(lineValue[0] < 150 && lineValue[2] < 120)
     {
       LineUpdate();
-      motors.setSpeeds(106, 100);
+      motors.setSpeeds(104, 100);
     }
     //links drehen
     turnAngle = 0;
       GyroUpdate();
       LineUpdate();
       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);
+      else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(52, 100);
+      else motors.setSpeeds(26, 100);
     }
     //geradeaus fahren
-    motors.setSpeeds(106, 100);
+    motors.setSpeeds(104, 100);
   }
 
   //links Abbiegen (vom Rundkurs)
     {
       GyroUpdate();
       EncoderUpdate();
-      motors.setSpeeds(27, 100);
+      motors.setSpeeds(26, 100);
       //if(driveRotation[1] > 1) break;
     }
     driveRotation[1] = 0;
     {
       GyroUpdate();
       EncoderUpdate();
-      motors.setSpeeds(27, 100);
+      motors.setSpeeds(26, 100);
       //if(driveRotation[1] > 1) break;
     }
     //geradeaus fahren
-    motors.setSpeeds(106, 100);
+    motors.setSpeeds(104, 100);
     lastUpdate = millis();
     TimeUpdate();
     while(eventTime < 1000)
       GyroUpdate();
       EncoderUpdate();
       LineUpdate();
-      motors.setSpeeds(106, 25);
+      motors.setSpeeds(104, 25);
       if(lineValue[0] > 150 && lineValue[0] < 500) break;
     }
     GyroUpdate();
       GyroUpdate();
       LineUpdate();
       if(eventTime > 3000) break;
-      if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
+      if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 0);
       //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
-      else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 50);
-      else motors.setSpeeds(106, 25);
+      else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 50);
+      else motors.setSpeeds(104, 25);
     }
   }
 
   else
   {
     //Serial1.println("nicht Abbiegen");
-    motors.setSpeeds(106, 100);
+    motors.setSpeeds(104, 100);
     lastUpdate = millis();
     TimeUpdate();
     while(eventTime < 1000)
 //Funktion Spurhalten
 void Spurhalten()
 {
+  uint16_t StartTime = millis();
+  uint16_t Update = millis();
+  uint16_t Time = Update - StartTime;
+
   //linke Linie erreicht, nach rechts fahren
   if(lineValue[0] > 150 && lineValue[0] < 500)
   {
     ledYellow(1);
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(106/eventSpeed, 25/eventSpeed);
-    lastUpdate = millis();
-    TimeUpdate();
-    while(eventTime < 100)
+    motors.setSpeeds(104/eventSpeed, 25/eventSpeed);
+    while(Time < 100)
     {
-      TimeUpdate();
+      Update = millis();
+      Time = Update - StartTime;
       LineUpdate();
       if(lineValue[2] > 120) break;
     }
   {
     ledYellow(1);
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(27/eventSpeed, 100/eventSpeed);
-    lastUpdate = millis();
-    TimeUpdate();
-    while(eventTime < 100)
+    motors.setSpeeds(26/eventSpeed, 100/eventSpeed);
+    while(Time < 100)
     {
-      TimeUpdate();
+      Update = millis();
+      Time = Update - StartTime;
       LineUpdate();
       if(lineValue[0] > 150) break;
     }
   else
   {
     ledGreen(1);
-    motors.setSpeeds(106/eventSpeed, 100/eventSpeed);
+    motors.setSpeeds(104/eventSpeed, 100/eventSpeed);
   }
 }
 
   {
     TimeUpdate();
     LineUpdate();
-    motors.setSpeeds(-106, -100);
+    motors.setSpeeds(-104, -100);
     if(eventTime > 3000) break;
   }
   //Serial1.println("Spur gefunden"); 
     eventSpeed = 1.0;
     //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
     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[0] > 500 && lineValue[0] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) Abbiegen();
+    else if((lineValue[2] > 500 && lineValue[2] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) Abbiegen();
     else if(lineValue[1] > 100 && lineValue[1] < 300) Spurfinden();
     else Spurhalten();
   }

ArduinoOutput/sketch/Hauptprojekt.ino.cpp

 #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 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 
-//dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
-//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
-//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
-//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
-//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth). 
-//Das LCD kann bei Bluetoothnutzung nicht verwendet werden. 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 08.05.2018 
+//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
+/*Kurzbeschreibung: 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 
+dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
+Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
+genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
+Für die Filterung der Messwerte werden Medianfilter genutzt. Der Zumo kommuniziert über ein 
+Bluetooth-Modul (HC-05) mit anderen Geräten. Die Kommunikation erfolgt seriell ('SERIAL' für USB, 
+'SERIAL1' für Bluetooth). Das LCD kann bei Bluetoothnutzung nicht verwendet werden.*/
 
 #include  <Wire.h>
 #include  <Zumo32U4.h>
+#include  <MedianFilter.h>
 
 Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
 Zumo32U4LineSensors       lineSensors;  //Liniensensoren
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
+MedianFilter LineFilter0(3, 0);         //erstellen der Filter für Liniensensoren
+MedianFilter LineFilter1(3, 0);         //Medianfilter geben mittleren Wert einer Reihe aus
+MedianFilter LineFilter2(3, 0);         //Fenstergröße: 3, Basiswerte: 0 0 0
+MedianFilter ProxFilter0(3, 0);         //erstellen der Filter für Abstandssensoren
+MedianFilter ProxFilter1(3, 0);
+MedianFilter ProxFilter2(3, 0);
+MedianFilter ProxFilter3(3, 0);
+
 #define   MARKLINE0       150
 #define   MARKLINE1       100
 #define   MARKLINE2       120
 #define   SIGN1           300
 #define   SIGN2           500
 #define   MAXSPEED        400
-#define   FULLSPEEDLEFT   106
-#define   HALFSPEEDLEFT   53
-#define   SLOWSPEEDLEFT   27
+#define   FULLSPEEDLEFT   104
+#define   HALFSPEEDLEFT   52
+#define   SLOWSPEEDLEFT   26
 #define   FULLSPEEDRIGHT  100
 #define   HALFSPEEDRIGHT  50
 #define   SLOWSPEEDRIGHT  25
 /*-----------------------------------------------------------------*/
 
 //Setup Bluetoothverbindung
-#line 68 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void BlueSetup();
-#line 77 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 87 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineSetup();
-#line 106 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 116 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxSetup();
-#line 112 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 122 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroSetup();
-#line 145 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 155 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassSetup();
-#line 176 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 186 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void setup();
-#line 202 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 212 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void TimeUpdate();
-#line 208 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 218 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LoopTiming();
-#line 252 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
-void ProxUpdate();
 #line 262 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
-void LineUpdate();
+void ProxUpdate();
 #line 272 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void LineUpdate();
+#line 282 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroUpdate();
-#line 285 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 295 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassUpdate();
-#line 298 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 308 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void EncoderUpdate();
-#line 309 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 319 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Kontaktvermeiden();
-#line 328 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 338 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Hindernisumfahren();
-#line 438 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 464 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Abbiegen();
-#line 599 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 625 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurhalten();
-#line 642 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 670 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurfinden();
-#line 660 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 688 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void SerielleAusgabe();
-#line 675 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 703 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void loop();
-#line 68 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void BlueSetup()
 {
   Serial1.begin(9600);                              //Initialisierung mit Datengeschwindigkeit(Baud)
 void ProxUpdate()
 {
   proxSensors.read();
-  proxValue[0] = proxSensors.countsLeftWithLeftLeds();
-  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
-  proxValue[3] = proxSensors.countsRightWithRightLeds(); 
+  proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
+  proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
+  proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());  
+  proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds()); 
 }
 
 //Update Liniensensoren
 void LineUpdate()
 {
   uint16_t  lineRaw[3];
-  lineSensors.read(lineRaw);
-  lineValue[0] = lineRaw[0] - lineOffset[0];
-  lineValue[1] = lineRaw[1] - lineOffset[1];
-  lineValue[2] = lineRaw[2] - lineOffset[2];
+  lineSensors.read(lineRaw);                                  //lese Messwerte der Liniensensoren aus
+  lineValue[0] = LineFilter0.in(lineRaw[0] - lineOffset[0]);  //ziehe Offsetwerte von Messwerten ab und gib diese in Filter ein
+  lineValue[1] = LineFilter1.in(lineRaw[1] - lineOffset[1]);  //erhält neue mittlere Werte der Filter
+  lineValue[2] = LineFilter2.in(lineRaw[2] - lineOffset[2]);  //"LineFilter.out" um gefilterte Werte auszugeben ohne neue Werte einzugeben
 }
 
 //Update Drehbewegungssensor
   turnAngle = 0;                    
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
+  while(rotationAngle < 20)          
+  {
+    GyroUpdate();  
+    LineUpdate();
+    motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+  }
+  GyroUpdate();                                                 
   while(rotationAngle < 45)          
   {
     GyroUpdate();  
     //Schritt 2: Hindernis passieren
     //Serial1.println("Aufholen"); 
     lastUpdate = millis();
-    while(proxValue[3] < 6 && eventTime < 3000)
+    while(proxValue[3] < 6)
     {
-      TimeUpdate();
       ProxUpdate();
       LineUpdate();
       Spurhalten();
+      TimeUpdate();
+      //Serial1.println(eventTime);
+      if(eventTime > 3000) break;
     }
-    //Serial1.println("Überholen"); 
+    //Serial1.println("Vorbeifahren"); 
     ProxUpdate(); 
     while(proxValue[3] == 6)
     {
     TimeUpdate();
     while(eventTime < 3000)
     {
-      //Serial1.println(eventTime); 
-      TimeUpdate();
       LineUpdate();
       Spurhalten();
+      TimeUpdate();
+      //Serial1.println(eventTime);
     }
   //}
   
   turnAngle = 0;                
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
+  while(rotationAngle > -20)          
+  {
+    GyroUpdate();  
+    LineUpdate();
+    motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
+  }
+  GyroUpdate();                                                 
   while(rotationAngle > -45)          
   {
     GyroUpdate();  
   LineUpdate(); 
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) leftCode = true;
+  if((lineValue[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) leftCode = true;
   else leftCode = false;
-  if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) rightCode = true;
+  if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
     lastUpdate = millis();
     TimeUpdate();
-    while(eventTime < 300)
+    while(eventTime < 1000)
     {
       TimeUpdate();
       motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
 //Funktion Spurhalten
 void Spurhalten()                                                   
 {
+  uint16_t StartTime = millis();
+  uint16_t Update = millis();
+  uint16_t Time = Update - StartTime;
+
   //linke Linie erreicht, nach rechts fahren
   if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)                      
   {
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
     motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
-    lastUpdate = millis();
-    TimeUpdate();
-    while(eventTime < 100)
+    while(Time < 100)
     {
-      TimeUpdate();
+      Update = millis();
+      Time = Update - StartTime;
       LineUpdate();
       if(lineValue[2] > MARKLINE2) break;
     }   
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
     motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);   
-    lastUpdate = millis();
-    TimeUpdate();
-    while(eventTime < 100)
+    while(Time < 100)
     {
-      TimeUpdate();
+      Update = millis();
+      Time = Update - StartTime;
       LineUpdate();
       if(lineValue[0] > MARKLINE0) break;
     }   
     eventSpeed = 1.0;
     //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
     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[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
+    else if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
     else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
     else Spurhalten();
   }

ArduinoOutput/sketch/Hauptprojekt.ino.cpp.d

  C:\Users\Carolin\Documents\Arduino\libraries\Zumo32U4/Zumo32U4LineSensors.h \
  C:\Users\Carolin\Documents\Arduino\libraries\Zumo32U4/QTRSensors.h \
  C:\Users\Carolin\Documents\Arduino\libraries\Zumo32U4/Zumo32U4Motors.h \
- C:\Users\Carolin\Documents\Arduino\libraries\Zumo32U4/Zumo32U4ProximitySensors.h
+ C:\Users\Carolin\Documents\Arduino\libraries\Zumo32U4/Zumo32U4ProximitySensors.h \
+ C:\Users\Carolin\Documents\Arduino\libraries\MedianFilter/MedianFilter.h

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

 //Verfassser: Felix Stange, 4056379, MET2016 
-//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 
-//dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
-//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
-//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
-//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
-//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth). 
-//Das LCD kann bei Bluetoothnutzung nicht verwendet werden. 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 08.05.2018 
+//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
+/*Kurzbeschreibung: 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 
+dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
+Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
+genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
+Für die Filterung der Messwerte werden Medianfilter genutzt. Der Zumo kommuniziert über ein 
+Bluetooth-Modul (HC-05) mit anderen Geräten. Die Kommunikation erfolgt seriell ('SERIAL' für USB, 
+'SERIAL1' für Bluetooth). Das LCD kann bei Bluetoothnutzung nicht verwendet werden.*/
 
 #include  <Wire.h>
 #include  <Zumo32U4.h>
+#include  <MedianFilter.h>
 
 Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
 Zumo32U4LineSensors       lineSensors;  //Liniensensoren
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
+MedianFilter LineFilter0(3, 0);         //erstellen der Filter für Liniensensoren
+MedianFilter LineFilter1(3, 0);         //Medianfilter geben mittleren Wert einer Reihe aus
+MedianFilter LineFilter2(3, 0);         //Fenstergröße: 3, Basiswerte: 0 0 0
+MedianFilter ProxFilter0(3, 0);         //erstellen der Filter für Abstandssensoren
+MedianFilter ProxFilter1(3, 0);
+MedianFilter ProxFilter2(3, 0);
+MedianFilter ProxFilter3(3, 0);
+
 #define   MARKLINE0       150
 #define   MARKLINE1       100
 #define   MARKLINE2       120
 #define   SIGN1           300
 #define   SIGN2           500
 #define   MAXSPEED        400
-#define   FULLSPEEDLEFT   106
-#define   HALFSPEEDLEFT   53
-#define   SLOWSPEEDLEFT   27
+#define   FULLSPEEDLEFT   104
+#define   HALFSPEEDLEFT   52
+#define   SLOWSPEEDLEFT   26
 #define   FULLSPEEDRIGHT  100
 #define   HALFSPEEDRIGHT  50
 #define   SLOWSPEEDRIGHT  25
 void ProxUpdate()
 {
   proxSensors.read();
-  proxValue[0] = proxSensors.countsLeftWithLeftLeds();
-  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
-  proxValue[3] = proxSensors.countsRightWithRightLeds(); 
+  proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
+  proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
+  proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());  
+  proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds()); 
 }
 
 //Update Liniensensoren
 void LineUpdate()
 {
   uint16_t  lineRaw[3];
-  lineSensors.read(lineRaw);
-  lineValue[0] = lineRaw[0] - lineOffset[0];
-  lineValue[1] = lineRaw[1] - lineOffset[1];
-  lineValue[2] = lineRaw[2] - lineOffset[2];
+  lineSensors.read(lineRaw);                                  //lese Messwerte der Liniensensoren aus
+  lineValue[0] = LineFilter0.in(lineRaw[0] - lineOffset[0]);  //ziehe Offsetwerte von Messwerten ab und gib diese in Filter ein
+  lineValue[1] = LineFilter1.in(lineRaw[1] - lineOffset[1]);  //erhält neue mittlere Werte der Filter
+  lineValue[2] = LineFilter2.in(lineRaw[2] - lineOffset[2]);  //"LineFilter.out" um gefilterte Werte auszugeben ohne neue Werte einzugeben
 }
 
 //Update Drehbewegungssensor
   turnAngle = 0;                    
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
+  while(rotationAngle < 20)          
+  {
+    GyroUpdate();  
+    LineUpdate();
+    motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+  }
+  GyroUpdate();                                                 
   while(rotationAngle < 45)          
   {
     GyroUpdate();  
     //Schritt 2: Hindernis passieren
     //Serial1.println("Aufholen"); 
     lastUpdate = millis();
-    while(proxValue[3] < 6 && eventTime < 3000)
+    while(proxValue[3] < 6)
     {
-      TimeUpdate();
       ProxUpdate();
       LineUpdate();
       Spurhalten();
+      TimeUpdate();
+      //Serial1.println(eventTime);
+      if(eventTime > 3000) break;
     }
-    //Serial1.println("Überholen"); 
+    //Serial1.println("Vorbeifahren"); 
     ProxUpdate(); 
     while(proxValue[3] == 6)
     {
     TimeUpdate();
     while(eventTime < 3000)
     {
-      //Serial1.println(eventTime); 
-      TimeUpdate();
       LineUpdate();
       Spurhalten();
+      TimeUpdate();
+      //Serial1.println(eventTime);
     }
   //}
   
   turnAngle = 0;                
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
+  while(rotationAngle > -20)          
+  {
+    GyroUpdate();  
+    LineUpdate();
+    motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
+  }
+  GyroUpdate();                                                 
   while(rotationAngle > -45)          
   {
     GyroUpdate();  
   LineUpdate(); 
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) leftCode = true;
+  if((lineValue[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) leftCode = true;
   else leftCode = false;
-  if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) rightCode = true;
+  if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
     lastUpdate = millis();
     TimeUpdate();
-    while(eventTime < 300)
+    while(eventTime < 1000)
     {
       TimeUpdate();
       motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
 //Funktion Spurhalten
 void Spurhalten()                                                   
 {
+  uint16_t StartTime = millis();
+  uint16_t Update = millis();
+  uint16_t Time = Update - StartTime;
+
   //linke Linie erreicht, nach rechts fahren
   if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)                      
   {
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
     motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
-    lastUpdate = millis();
-    TimeUpdate();
-    while(eventTime < 100)
+    while(Time < 100)
     {
-      TimeUpdate();
+      Update = millis();
+      Time = Update - StartTime;
       LineUpdate();
       if(lineValue[2] > MARKLINE2) break;
     }   
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
     motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);   
-    lastUpdate = millis();
-    TimeUpdate();
-    while(eventTime < 100)
+    while(Time < 100)
     {
-      TimeUpdate();
+      Update = millis();
+      Time = Update - StartTime;
       LineUpdate();
       if(lineValue[0] > MARKLINE0) break;
     }   
     eventSpeed = 1.0;
     //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
     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[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
+    else if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
     else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
     else Spurhalten();
   }