Solved Problem with Cycletime

ArduinoOutput/preproc/ctags_target_for_gcc_minus_e.cpp

@@ -1,11 +1,11 @@
 # 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 21.02.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.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 die Beschleunigungssensoren (LSM303) erkannt. 
+//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 
@@ -22,11 +22,11 @@ Zumo32U4ButtonA buttonA; //Taste A
 Zumo32U4Encoders encoders; //Motorencoder
 LSM303 compass; //Beschleunigungssensor x-Achse
 L3G gyro; //Drehbewegungssensor z-Achse
-
+# 33 "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)
 
-uint8_t proxValue[4]; //Abstandssensoren v.l.(0)n.r.(3)
+uint8_t proxValue[4]; //Abstandssensoren v.l. (0) n.r. (3)
 
 int16_t encoderCounts[2]; //Anzahl der Umdrehungen
 int16_t driveRotation[2]; //von links (0) nach rechts (1)
@@ -37,17 +37,17 @@ int16_t turnRate;
 int16_t gyroOffset;
 uint16_t gyroLastUpdate;
 
-int32_t moveDisplay = 0; //Beschleunigung
-int32_t moveDistance = 0;
+//int32_t   moveDisplay = 0;              //Beschleunigung
+//int32_t   moveDistance = 0;   
 int16_t moveRate;
 int16_t compassOffset;
-uint16_t compassLastUpdate;
+//uint16_t  compassLastUpdate;
 
-uint16_t LastUpdate = 0; //Systemzeit
-uint16_t CycleTime = 0; //Zykluszeit
-int maxSpeed = 200; //Maximale Geschwindigkeit (max. 400)
-char dir; //Fahrtrichtung, Ereignis
+uint16_t lastUpdate = 0; //Systemzeit
+uint16_t eventTime = 0; //Zeit seit letzter Geschwindigkeitsänderung
 char report[200]; //Ausgabe
+int eventSpeed = 1; //vermindert die Geschwindigkeit bei Manövern
+char dir; //Fahrtrichtung, Ereignis
 String btData; //Gelesene Daten aus Bluetooth
 
 /*-----------------------------------------------------------------*/
@@ -62,8 +62,8 @@ void LineSetup()
   uint32_t total[3] = {0, 0, 0};
   for(uint8_t i = 0; i < 120; i++)
   {
-    if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
-    else motors.setSpeeds(-maxSpeed, maxSpeed);
+    if (i > 30 && i <= 90) motors.setSpeeds(100, -100);
+    else motors.setSpeeds(-100, 100);
     lineSensors.read(lineOffset);
     total[0] += lineOffset[0];
     total[1] += lineOffset[1];
@@ -88,7 +88,16 @@ void GyroSetup()
 {
   ledYellow(1);
   gyro.init();
-
+  if (!gyro.init())
+  {
+    //Fehler beim Initialisieren des Drehbewegungssensors
+    ledRed(1);
+    while(1)
+    {
+      Serial1.println((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Fehler Drehbewegungssensors"); &__c[0];})))));
+      delay(100);
+    }
+  }
   gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
   gyro.writeReg(L3G::CTRL4, 0b00100000); //2000dps Auflösung
   gyro.writeReg(L3G::CTRL5, 0b00000000); //Hochpassfilter ausgeschaltet
@@ -112,6 +121,16 @@ void CompassSetup()
 {
   ledYellow(1);
   compass.init();
+  if (!compass.init())
+  {
+    //Fehler beim Initialisieren des Beschleunigungssensors
+    ledRed(1);
+    while(1)
+    {
+      Serial.println((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Fehler Beschleunigungssensors"); &__c[0];})))));
+      delay(100);
+    }
+  }
   compass.enableDefault();
 
   //Kalibrierung
@@ -123,7 +142,7 @@ void CompassSetup()
   }
   compassOffset = total / 1024;
 
-  compassLastUpdate = micros();
+  //compassLastUpdate = micros();  
   ledYellow(0);
 }
 
@@ -133,13 +152,13 @@ void setup()
 {
   //Initialisierung der Bluetoothverbindung
   Serial1.begin(9600);
+  Serial1.setTimeout(10); //verkürzt Serial.readString auf 10 ms statt 1s
   if(Serial1.available()) Serial1.println("Verbindung hergestellt");
 
   //Initialisierung und Kalibrierung der Sensoren
   Serial1.println("Sensorkalibrierung");
   Wire.begin();
   delay(500);
-  LastUpdate = micros();
   ProxSetup();
   LineSetup();
   GyroSetup();
@@ -151,6 +170,8 @@ void setup()
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
+  motors.setSpeeds(100, 100);
+  lastUpdate = millis();
 }
 
 /*-----------------------------------------------------------------*/
@@ -158,9 +179,51 @@ void setup()
 //Update Durchlaufzeit
 void TimeUpdate()
 {
-  uint16_t m = micros();
-  CycleTime = m - LastUpdate;
-  LastUpdate = m;
+  uint16_t m = millis();
+  eventTime = m - lastUpdate;
+}
+
+void LoopTiming()
+{
+  const int AL = 100; // Arraylänge, NUR GERADE Zahlen verwenden!
+  static unsigned long LoopTime[AL];
+  static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;
+
+  if (Messung % 2 == 0) // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
+    {
+     LoopTime[Index] = millis();
+     Messung++;
+     Index++;
+     return; // Funktion sofort beenden, spart etwas Zeit
+    }
+
+  if (Messung % 2 == 1) // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
+    {
+     LoopTime[Index] = millis();
+     LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1]; // Loopdauer einen Index niedriger einspeichern wie aktuell
+     Messung++;
+    }
+
+  if (Index >= AL) // Array voll, Daten auswerten
+    {
+     for (int i = 0; i<AL; i++)
+       {
+        Min = ((Min)<(LoopTime[i])?(Min):(LoopTime[i]));
+        Max = ((Max)>(LoopTime[i])?(Max):(LoopTime[i]));
+        Avg += LoopTime[i];
+       }
+
+     Avg = Avg / AL;
+     Serial1.print((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Minimal       "); &__c[0];})))));Serial1.print(Min);Serial1.println(" ms  ");
+     Serial1.print((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Durchschnitt  "); &__c[0];})))));Serial1.print(Avg);Serial1.println(" ms  ");
+     Serial1.print((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Maximal       "); &__c[0];})))));Serial1.print(Max);Serial1.println(" ms  ");
+     SerielleAusgabe();
+     Min = 0xFFFF;
+     Max = 0;
+     Avg = 0;
+     Messung = 0;
+     Index = 0;
+    }
 }
 
 //Update Abstandssensoren
@@ -173,7 +236,7 @@ void ProxUpdate()
   proxValue[3] = proxSensors.countsRightWithRightLeds();
 }
 
-//Updaten Liniensensoren
+//Update Liniensensoren
 void LineUpdate()
 {
   uint16_t lineRaw[3];
@@ -186,8 +249,8 @@ void LineUpdate()
 //Update Drehbewegungssensor
 void GyroUpdate()
 {
-  gyro.read();
-  turnRate = gyro.g.z - gyroOffset;
+  gyro.read(); //Rohwert 10285 entspricht 90° bzw.
+  turnRate = gyro.g.z - gyroOffset; //8,75mdps/LSB
   uint16_t m = micros();
   uint16_t dt = m - gyroLastUpdate;
   gyroLastUpdate = m;
@@ -196,17 +259,17 @@ void GyroUpdate()
   rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
 }
 
-// Update Beschleunigungssensor
+//Update Beschleunigungssensor
 void CompassUpdate()
 {
-  compass.read();
-  moveRate = compass.a.x - compassOffset;
-  uint16_t m = micros();
-  uint16_t dt = m - compassLastUpdate;
-  compassLastUpdate = m;
-  int32_t d = (int32_t)moveRate * dt;
-  moveDistance += (int64_t)d * dt >> 14;
-  moveDisplay = moveDistance * 1000 / 9,81;
+  compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw.
+  moveRate = compass.a.x - compassOffset; //bei +/-2g Messbereich 0,61mg/LSB
+//   uint16_t m = micros();
+//   uint16_t dt = m - compassLastUpdate;
+//   compassLastUpdate = m;
+//   int32_t d = (int32_t)moveRate * dt;
+//   moveDistance += (int64_t)d * dt >> 14;
+//   moveDisplay = moveDistance * 9,81 / 1000;
 }
 
 //Update Encoder
@@ -220,20 +283,26 @@ void EncoderUpdate()
 
 /*-----------------------------------------------------------------*/
 
-//Funktion Kollisionserkennung
-void Kollisionserkennung()
+//Funktion Kontaktvermeiden
+void Kontaktvermeiden()
 {
   dir = 'X';
-  Serial1.println("Kollision");
+  Serial1.println("Kontaktvermeiden");
   ledRed(1);
 
   motors.setSpeeds(0, 0);
   delay(500);
-  while(lineValue[0] < 300 && lineValue[2] < 300)
+  while(proxValue[1] == 0 || proxValue[2] == 0)
   {
-    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+    ProxUpdate();
+    motors.setSpeeds(-50, -50);
+    if(lineValue[0] > 200 || lineValue[2] > 200) return;
   }
   delay(500);
+
+  Serial1.println("Vermeiden beendet");
+  motors.setSpeeds(100, 100);
+  lastUpdate = millis();
 }
 
 //Funktion Hindernisumfahrung
@@ -250,41 +319,37 @@ void Hindernisumfahren()
   while(rotationAngle < 45)
   {
     GyroUpdate();
-    motors.setSpeeds(maxSpeed/2, maxSpeed);
+    motors.setSpeeds(50, 100);
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[2] < 300)
+  while(lineValue[2] < 200)
   {
     LineUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed);
+    motors.setSpeeds(100, 100);
   }
   //rechts drehen
   GyroUpdate();
   while(rotationAngle > 0)
   {
     GyroUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed/2);
+    motors.setSpeeds(100, 50);
   }
   //geradeaus fahren
-  motors.setSpeeds(maxSpeed, maxSpeed);
+  motors.setSpeeds(100, 100);
 
   //Gegenverkehr beachten
-  proxSensors.read();
-  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValue[2] = proxSensors.countsFrontWithRightLeds();
+  ProxUpdate();
   if(proxValue[1] < 5 || proxValue[2] < 5)
   {
     //Schritt 2: Hindernis passieren
-    motors.setSpeeds(maxSpeed, maxSpeed);
+    motors.setSpeeds(100, 100);
     delay(1000);
-    proxSensors.read();
-    proxValue[3] = proxSensors.countsRightWithRightLeds();
+    ProxUpdate();
     while(proxValue[3] > 4)
     {
-      motors.setSpeeds(maxSpeed, maxSpeed);
-      proxSensors.read();
-      proxValue[3] = proxSensors.countsRightWithRightLeds();
+      motors.setSpeeds(100, 100);
+      ProxUpdate();
       Spurhalten();
     }
   }
@@ -296,26 +361,26 @@ void Hindernisumfahren()
   while(rotationAngle > -45)
   {
     GyroUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed/2);
+    motors.setSpeeds(100, 50);
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[0] < 300)
+  while(lineValue[0] < 200)
   {
     LineUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed);
+    motors.setSpeeds(100, 100);
   }
   //links drehen
   GyroUpdate();
   while(rotationAngle < 0)
   {
     GyroUpdate();
-    motors.setSpeeds(maxSpeed/2, maxSpeed);
+    motors.setSpeeds(50, 100);
   }
   //geradeaus fahren
-  motors.setSpeeds(maxSpeed, maxSpeed);
-
   Serial1.println("Umfahren beendet");
+  motors.setSpeeds(100, 100);
+  lastUpdate = millis();
 }
 
 //Funktion Abbiegen
@@ -325,7 +390,7 @@ void Abbiegen()
   ledYellow(1);
   Serial1.println("Abbiegen");
 
-  //Kodierung analysieren
+  //Markierung analysieren
   bool leftCode; //links => 1
   bool rightCode; //rechts => 2
   if(lineValue[0] > 1000) leftCode = true;
@@ -349,10 +414,10 @@ void Abbiegen()
     //zur Kreuzungsmitte fahren
     driveRotation[0] = 0;
     driveRotation[1] = 0;
-    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
+    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 200 && lineValue[2] < 200)
     {
       EncoderUpdate();
-      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+      motors.setSpeeds(50, 50);
     }
     //links drehen
     rotationAngle = 0;
@@ -360,13 +425,11 @@ void Abbiegen()
     while(rotationAngle != 90)
     {
       GyroUpdate();
-      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
-      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
-      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(0, maxSpeed/2);
-      else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
+      if(lineValue[0] > 200 && lineValue[0] < 1000) motors.setSpeeds(50, 50);
+      else if(lineValue[1] > 100 && lineValue[1] < 1000) motors.setSpeeds(-25, -25);
+      else if(lineValue[2] > 200 && lineValue[2] < 1000) motors.setSpeeds(0, 50);
+      else motors.setSpeeds(25, 50);
     }
-    //geradeaus fahren
-    motors.setSpeeds(maxSpeed/2, maxSpeed/2);
   }
 
   //rechts Abbiegen
@@ -378,57 +441,62 @@ void Abbiegen()
     while(rotationAngle != -90)
     {
       GyroUpdate();
-      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, 0);
-      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
-      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
-      else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
+      if(lineValue[0] > 200 && lineValue[0] < 1000) motors.setSpeeds(50, 0);
+      else if(lineValue[1] > 100 && lineValue[1] < 1000) motors.setSpeeds(-25, -25);
+      else if(lineValue[2] > 200 && lineValue[2] < 1000) motors.setSpeeds(50, 50);
+      else motors.setSpeeds(50, 25);
     }
-    //geradeaus fahren
-    motors.setSpeeds(maxSpeed/2, maxSpeed/2);
   }
 
   //nicht Abbiegen, geradeaus fahren
-  else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+  else motors.setSpeeds(50, 50);
   delay(500);
 
+  //geradeaus fahren
   Serial1.println("Abbiegen beendet");
+  motors.setSpeeds(100, 100);
+  lastUpdate = millis();
 }
 
 //Funktion Spurhalten
 void Spurhalten()
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > 300 && lineValue[0] < 500)
+  if(lineValue[0] > 200 && lineValue[0] < 1000)
   {
     dir = 'R';
     ledYellow(1);
-    Serial1.println("Spur nach rechts korrigieren");
-    motors.setSpeeds(maxSpeed, maxSpeed/2);
+    //Serial1.println("Spur nach rechts korrigieren");  
+    motors.setSpeeds(100/eventSpeed, 25/eventSpeed);
     delay(100);
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > 300 && lineValue[2] < 500)
+  else if(lineValue[2] > 200 && lineValue[2] < 1000)
   {
     dir = 'L';
     ledYellow(1);
-    Serial1.println("Spur nach links korrigieren");
-    motors.setSpeeds(maxSpeed/2, maxSpeed);
+    //Serial1.println("Spur nach links korrigieren");  
+    motors.setSpeeds(25/eventSpeed, 100/eventSpeed);
     delay(100);
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > 300 && lineValue[1] < 500)
+  else if(lineValue[1] > 100 && lineValue[1] < 1000)
   {
+
     dir = 'B';
     ledRed(1);
-    Serial1.println("Spur verlassen");
-    Serial1.println("1");
-    while(lineValue[0] < 300 && lineValue[2] < 300) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+    Serial1.println("Spurfinden");
+    uint16_t startTime = millis();
+    while(lineValue[0] < 200 && lineValue[2] < 200) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
     {
-      motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+      motors.setSpeeds(-50/eventSpeed, -50/eventSpeed);
+      uint16_t backTime = millis();
+      if((backTime - startTime) > 3000) return;
     }
-    delay(500);
+    delay(100);
+    Serial1.println("Spur gefunden");
   }
 
   //normale Fahrt
@@ -436,19 +504,20 @@ void Spurhalten()
   {
     dir = 'F';
     ledGreen(1);
-    Serial1.println("Spur folgen");
-    motors.setSpeeds(maxSpeed, maxSpeed);
   }
 }
 
 //Funktion Serielle Ausgabe
-void SerialOutput()
+void SerielleAusgabe()
 {
   snprintf_P(report, sizeof(report),
-    (__extension__({static const char __c[] __attribute__((__progmem__)) = ("Abstand: %d %d %d %d   Linie: %d %d %d   Weg: %d %d   Winkel: %d   Zeit: %d"); &__c[0];})),
+    (__extension__({static const char __c[] __attribute__((__progmem__)) = ("Abstand: %d %d %d %d   Linie: %d %d %d"); &__c[0];})),
     proxValue[0], proxValue[1], proxValue[2], proxValue[3],
-    lineValue[0], lineValue[1], lineValue[2],
-    driveRotation[0], driveRotation[1], rotationAngle, CycleTime);
+    lineValue[0], lineValue[1], lineValue[2]);
+  Serial1.println(report);
+  snprintf_P(report, sizeof(report),
+    (__extension__({static const char __c[] __attribute__((__progmem__)) = ("Weg: %d %d   Winkel: %d   Beschleunigung: %d"); &__c[0];})),
+    driveRotation[0], driveRotation[1], rotationAngle, moveRate);
   Serial1.println(report);
 }
 
@@ -456,38 +525,42 @@ void SerialOutput()
 
 void loop()
 {
+  LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
+
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
 
   //Abfragen der Sensordaten
-  TimeUpdate();
   LineUpdate();
   ProxUpdate();
+  EncoderUpdate();
   GyroUpdate();
   CompassUpdate();
-  EncoderUpdate();
+  TimeUpdate();
 
   //Funktionsauswahl
   btData = Serial1.readString();
   //verfügbare Funktionen bei laufenden Manövern
-  if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kollision")
+  if(btData == "Abbiegen" || btData == "Hindernisumfahren"
+  || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
   {
-    maxSpeed = 100;
-    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
-    lineValue[2] > 1000) motors.setSpeeds(0, 0);
+    //Serial1.println("Verstanden");
+    eventSpeed = 2;
+    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 ||
+    lineValue[0] > 1000 || lineValue[2] > 1000) motors.setSpeeds(0, 0);
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else
   {
-    if(moveRate > 1000) Kollisionserkennung();
-    else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
+    eventSpeed = 1;
+    if(((moveRate)>0?(moveRate):-(moveRate)) > 1500 && eventTime > 1000) Kontaktvermeiden();
+    if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
     else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
     else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
     else Spurhalten();
   }
 
-  //Ausgabe über Bluetoothverbindung
-  SerialOutput();
+  LoopTiming();
 }

ArduinoOutput/sketch/Hauptprojekt.ino.cpp

@@ -2,19 +2,19 @@
 #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 21.02.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.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 die Beschleunigungssensoren (LSM303) erkannt. 
+//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. 
 
-#include <Wire.h>
-#include <Zumo32U4.h>
+#include  <Wire.h>
+#include  <Zumo32U4.h>
 
 Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
 Zumo32U4LineSensors       lineSensors;  //Liniensensoren
@@ -24,10 +24,19 @@ Zumo32U4Encoders          encoders;     //Motorencoder
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
+#define   MARKLINE0       200
+#define   MARKLINE1       100
+#define   MARKLINE2       200
+#define   SIGN            1000
+#define   MAXSPEED        400
+#define   FULLSPEED       100
+#define   HALFSPEED       50
+#define   SLOWSPEED       25
+
 int16_t   lineValue[3];                 //Liniensensoren
 uint16_t  lineOffset[3];                //von links (0) nach rechts (2)
 
-uint8_t   proxValue[4];                 //Abstandssensoren v.l.(0)n.r.(3)
+uint8_t   proxValue[4];                 //Abstandssensoren v.l. (0) n.r. (3)
 
 int16_t   encoderCounts[2];             //Anzahl der Umdrehungen
 int16_t   driveRotation[2];             //von links (0) nach rechts (1)
@@ -38,57 +47,59 @@ int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
 
-int32_t   moveDisplay = 0;              //Beschleunigung
-int32_t   moveDistance = 0;   
+//int32_t   moveDisplay = 0;              //Beschleunigung
+//int32_t   moveDistance = 0;   
 int16_t   moveRate;
 int16_t   compassOffset;                
-uint16_t  compassLastUpdate;
+//uint16_t  compassLastUpdate;
 
-uint16_t  LastUpdate = 0;               //Systemzeit
-uint16_t  CycleTime = 0;                //Zykluszeit
-int       maxSpeed = 200;               //Maximale Geschwindigkeit (max. 400)
-char      dir;                          //Fahrtrichtung, Ereignis
+uint16_t  lastUpdate = 0;               //Systemzeit
+uint16_t  eventTime = 0;                //Zeit seit letzter Geschwindigkeitsänderung
 char      report[200];                  //Ausgabe
+int       eventSpeed = 1;               //vermindert die Geschwindigkeit bei Manövern
+char      dir;                          //Fahrtrichtung, Ereignis
 String    btData;                       //Gelesene Daten aus Bluetooth
 
 /*-----------------------------------------------------------------*/
 
 //Setup Liniensensoren
-#line 54 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 63 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineSetup();
-#line 79 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxSetup();
-#line 85 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 94 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroSetup();
-#line 109 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 127 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassSetup();
-#line 130 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 158 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void setup();
-#line 157 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void TimeUpdate();
-#line 165 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 193 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void LoopTiming();
+#line 237 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxUpdate();
-#line 175 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 247 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineUpdate();
-#line 185 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 257 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroUpdate();
-#line 198 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 270 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassUpdate();
-#line 211 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 283 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void EncoderUpdate();
-#line 222 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
-void Kollisionserkennung();
-#line 238 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void Kontaktvermeiden();
+#line 316 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Hindernisumfahren();
-#line 320 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 394 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Abbiegen();
-#line 396 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 469 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurhalten();
-#line 443 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
-void SerialOutput();
-#line 455 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 518 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void SerielleAusgabe();
+#line 533 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void loop();
-#line 54 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 63 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineSetup()                                            
 {
   ledYellow(1);
@@ -98,8 +109,8 @@ void LineSetup()
   uint32_t total[3] = {0, 0, 0};  
   for(uint8_t i = 0; i < 120; i++)                                
   {
-    if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
-    else motors.setSpeeds(-maxSpeed, maxSpeed);
+    if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEED, -FULLSPEED);
+    else motors.setSpeeds(-FULLSPEED, FULLSPEED);
     lineSensors.read(lineOffset);  
     total[0] += lineOffset[0];
     total[1] += lineOffset[1];
@@ -124,7 +135,16 @@ void GyroSetup()
 {                                                                 
   ledYellow(1);
   gyro.init();
-
+  if (!gyro.init())
+  {
+    //Fehler beim Initialisieren des Drehbewegungssensors
+    ledRed(1);
+    while(1)
+    {
+      Serial1.println(F("Fehler Drehbewegungssensors"));
+      delay(100);
+    }
+  }
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
   gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
   gyro.writeReg(L3G::CTRL5, 0b00000000);                          //Hochpassfilter ausgeschaltet
@@ -148,6 +168,16 @@ void CompassSetup()
 {
   ledYellow(1);
   compass.init();
+  if (!compass.init())
+  {
+    //Fehler beim Initialisieren des Beschleunigungssensors
+    ledRed(1);
+    while(1)
+    {
+      Serial.println(F("Fehler Beschleunigungssensors"));
+      delay(100);
+    }
+  }
   compass.enableDefault();  
 
   //Kalibrierung
@@ -159,7 +189,7 @@ void CompassSetup()
   }
   compassOffset = total / 1024;
 
-  compassLastUpdate = micros();  
+  //compassLastUpdate = micros();  
   ledYellow(0);
 }
 
@@ -168,14 +198,14 @@ void CompassSetup()
 void setup() 
 {
   //Initialisierung der Bluetoothverbindung
-  Serial1.begin(9600);                                              
+  Serial1.begin(9600);                           
+  Serial1.setTimeout(10);                   //verkürzt Serial.readString auf 10 ms statt 1s
   if(Serial1.available()) Serial1.println("Verbindung hergestellt");  
 
   //Initialisierung und Kalibrierung der Sensoren
   Serial1.println("Sensorkalibrierung");   
   Wire.begin();                       
   delay(500);
-  LastUpdate = micros();
   ProxSetup();
   LineSetup();                                                
   GyroSetup();
@@ -187,6 +217,8 @@ void setup()
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 /*-----------------------------------------------------------------*/
@@ -194,9 +226,51 @@ void setup()
 //Update Durchlaufzeit
 void TimeUpdate()
 {
-  uint16_t m = micros();
-  CycleTime = m - LastUpdate;
-  LastUpdate = m;
+  uint16_t m = millis();
+  eventTime = m - lastUpdate;
+}
+
+void LoopTiming()
+{
+  const int AL = 100;       // Arraylänge, NUR GERADE Zahlen verwenden!
+  static unsigned long LoopTime[AL];
+  static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;
+ 
+  if (Messung % 2 == 0)     // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
+    {
+     LoopTime[Index] = millis();
+     Messung++;
+     Index++;   
+     return;	              // Funktion sofort beenden, spart etwas Zeit
+    }
+
+  if (Messung % 2 == 1)     // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
+    {
+     LoopTime[Index] = millis();
+     LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1];  	// Loopdauer einen Index niedriger einspeichern wie aktuell
+     Messung++;
+    }	
+	   
+  if (Index >= AL) 	        // Array voll, Daten auswerten
+    { 
+     for (int i = 0; i<AL; i++)
+       {
+        Min = min(Min, LoopTime[i]);
+        Max = max(Max, LoopTime[i]);
+        Avg += LoopTime[i];
+       }
+	
+     Avg = Avg / AL;
+     Serial1.print(F("Minimal       "));Serial1.print(Min);Serial1.println(" ms  ");
+     Serial1.print(F("Durchschnitt  "));Serial1.print(Avg);Serial1.println(" ms  ");
+     Serial1.print(F("Maximal       "));Serial1.print(Max);Serial1.println(" ms  ");
+     SerielleAusgabe();
+     Min = 0xFFFF;
+     Max = 0;
+     Avg = 0;
+     Messung = 0;
+     Index = 0;
+    }
 }
 
 //Update Abstandssensoren
@@ -209,7 +283,7 @@ void ProxUpdate()
   proxValue[3] = proxSensors.countsRightWithRightLeds(); 
 }
 
-//Updaten Liniensensoren
+//Update Liniensensoren
 void LineUpdate()
 {
   uint16_t  lineRaw[3];
@@ -222,8 +296,8 @@ void LineUpdate()
 //Update Drehbewegungssensor
 void GyroUpdate()                                                 
 {
-  gyro.read();
-  turnRate = gyro.g.z - gyroOffset;
+  gyro.read();                              //Rohwert 10285 entspricht 90° bzw.
+  turnRate = gyro.g.z - gyroOffset;         //8,75mdps/LSB
   uint16_t m = micros();
   uint16_t dt = m - gyroLastUpdate;
   gyroLastUpdate = m;
@@ -232,17 +306,17 @@ void GyroUpdate()
   rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
 }
 
-// Update Beschleunigungssensor
+//Update Beschleunigungssensor
 void CompassUpdate()
 {
-  compass.read();
-  moveRate = compass.a.x - compassOffset;
-  uint16_t m = micros();
-  uint16_t dt = m - compassLastUpdate;
-  compassLastUpdate = m;
-  int32_t d = (int32_t)moveRate * dt;
-  moveDistance += (int64_t)d * dt >> 14;
-  moveDisplay = moveDistance * 1000 / 9,81;
+  compass.read();                           //Rohwert 16384 entspricht 1g (9,81m/s²) bzw.
+  moveRate = compass.a.x - compassOffset;   //bei +/-2g Messbereich 0,61mg/LSB
+//   uint16_t m = micros();
+//   uint16_t dt = m - compassLastUpdate;
+//   compassLastUpdate = m;
+//   int32_t d = (int32_t)moveRate * dt;
+//   moveDistance += (int64_t)d * dt >> 14;
+//   moveDisplay = moveDistance * 9,81 / 1000;
 }
 
 //Update Encoder
@@ -256,20 +330,26 @@ void EncoderUpdate()
 
 /*-----------------------------------------------------------------*/
 
-//Funktion Kollisionserkennung
-void Kollisionserkennung()                                            
+//Funktion Kontaktvermeiden
+void Kontaktvermeiden()                                            
 {
   dir = 'X';
-  Serial1.println("Kollision");
+  Serial1.println("Kontaktvermeiden");
   ledRed(1);
 
   motors.setSpeeds(0, 0);
   delay(500);
-  while(lineValue[0] < 300 && lineValue[2] < 300)
+  while(proxValue[1] == 0 || proxValue[2] == 0)
   {
-    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+    ProxUpdate();
+    motors.setSpeeds(-HALFSPEED, -HALFSPEED);
+    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) return;
   }
   delay(500); 
+
+  Serial1.println("Vermeiden beendet");
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 //Funktion Hindernisumfahrung
@@ -286,41 +366,37 @@ void Hindernisumfahren()
   while(rotationAngle < 45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(maxSpeed/2, maxSpeed); 
+    motors.setSpeeds(HALFSPEED, FULLSPEED); 
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[2] < 300)          
+  while(lineValue[2] < MARKLINE2)          
   {
     LineUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed); 
+    motors.setSpeeds(FULLSPEED, FULLSPEED); 
   } 
   //rechts drehen
   GyroUpdate();
   while(rotationAngle > 0)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed/2); 
+    motors.setSpeeds(FULLSPEED, HALFSPEED); 
   }
   //geradeaus fahren
-  motors.setSpeeds(maxSpeed, maxSpeed); 
+  motors.setSpeeds(FULLSPEED, FULLSPEED); 
 
   //Gegenverkehr beachten
-  proxSensors.read();                                                 
-  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
+  ProxUpdate();
   if(proxValue[1] < 5 || proxValue[2] < 5)
   {
     //Schritt 2: Hindernis passieren
-    motors.setSpeeds(maxSpeed, maxSpeed);                    
+    motors.setSpeeds(FULLSPEED, FULLSPEED);                    
     delay(1000);
-    proxSensors.read();
-    proxValue[3] = proxSensors.countsRightWithRightLeds();  
+    ProxUpdate(); 
     while(proxValue[3] > 4)
     {
-      motors.setSpeeds(maxSpeed, maxSpeed);  
-      proxSensors.read();
-      proxValue[3] = proxSensors.countsRightWithRightLeds();  
+      motors.setSpeeds(FULLSPEED, FULLSPEED);  
+      ProxUpdate();
       Spurhalten();
     }
   }
@@ -332,26 +408,26 @@ void Hindernisumfahren()
   while(rotationAngle > -45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(maxSpeed, maxSpeed/2); 
+    motors.setSpeeds(FULLSPEED, HALFSPEED); 
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[0] < 300)          
+  while(lineValue[0] < MARKLINE0)          
   {
     LineUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed); 
+    motors.setSpeeds(FULLSPEED, FULLSPEED); 
   } 
   //links drehen
   GyroUpdate();
   while(rotationAngle < 0)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(maxSpeed/2, maxSpeed); 
+    motors.setSpeeds(HALFSPEED, FULLSPEED);
   }
   //geradeaus fahren
-  motors.setSpeeds(maxSpeed, maxSpeed); 
-
-  Serial1.println("Umfahren beendet");  
+  Serial1.println("Umfahren beendet");
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 //Funktion Abbiegen
@@ -361,12 +437,12 @@ void Abbiegen()
   ledYellow(1); 
   Serial1.println("Abbiegen");  
 
-  //Kodierung analysieren
+  //Markierung analysieren
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if(lineValue[0] > 1000) leftCode = true;
+  if(lineValue[0] > SIGN) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > 1000) rightCode = true;
+  if(lineValue[2] > SIGN) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
@@ -385,10 +461,10 @@ void Abbiegen()
     //zur Kreuzungsmitte fahren
     driveRotation[0] = 0;
     driveRotation[1] = 0;
-    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
+    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
     {
       EncoderUpdate();
-      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+      motors.setSpeeds(HALFSPEED, HALFSPEED);
     }
     //links drehen
     rotationAngle = 0;
@@ -396,13 +472,11 @@ void Abbiegen()
     while(rotationAngle != 90)
     {
       GyroUpdate();
-      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
-      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
-      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(0, maxSpeed/2);
-      else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN) motors.setSpeeds(HALFSPEED, HALFSPEED);
+      else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN) motors.setSpeeds(-SLOWSPEED, -SLOWSPEED);
+      else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN) motors.setSpeeds(0, HALFSPEED);
+      else motors.setSpeeds(SLOWSPEED, HALFSPEED);
     }
-    //geradeaus fahren
-    motors.setSpeeds(maxSpeed/2, maxSpeed/2);
   }
 
   //rechts Abbiegen
@@ -414,57 +488,62 @@ void Abbiegen()
     while(rotationAngle != -90)
     {
       GyroUpdate();
-      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, 0);
-      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
-      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
-      else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN) motors.setSpeeds(HALFSPEED, 0);
+      else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN) motors.setSpeeds(-SLOWSPEED, -SLOWSPEED);
+      else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN) motors.setSpeeds(HALFSPEED, HALFSPEED);
+      else motors.setSpeeds(HALFSPEED, SLOWSPEED);
     }
-    //geradeaus fahren
-    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
   }  
 
   //nicht Abbiegen, geradeaus fahren
-  else motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+  else motors.setSpeeds(HALFSPEED, HALFSPEED); 
   delay(500);
 
-  Serial1.println("Abbiegen beendet");  
+  //geradeaus fahren
+  Serial1.println("Abbiegen beendet");
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 //Funktion Spurhalten
 void Spurhalten()                                                   
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > 300 && lineValue[0] < 500)                      
+  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN)                      
   {
     dir = 'R';
     ledYellow(1); 
-    Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(maxSpeed, maxSpeed/2);
+    //Serial1.println("Spur nach rechts korrigieren");  
+    motors.setSpeeds(FULLSPEED/eventSpeed, SLOWSPEED/eventSpeed);
     delay(100);    
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > 300 && lineValue[2] < 500)                 
+  else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN)                 
   {
     dir = 'L'; 
     ledYellow(1); 
-    Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(maxSpeed/2, maxSpeed);  
+    //Serial1.println("Spur nach links korrigieren");  
+    motors.setSpeeds(SLOWSPEED/eventSpeed, FULLSPEED/eventSpeed);  
     delay(100);  
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > 300 && lineValue[1] < 500)                 
+  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN)                 
   { 
+    
     dir = 'B';  
     ledRed(1); 
-    Serial1.println("Spur verlassen");  
-    Serial1.println("1"); 
-    while(lineValue[0] < 300 && lineValue[2] < 300)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+    Serial1.println("Spurfinden");  
+    uint16_t startTime = millis();
+    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
     {
-      motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+      motors.setSpeeds(-HALFSPEED/eventSpeed, -HALFSPEED/eventSpeed);
+      uint16_t backTime = millis();
+      if((backTime - startTime) > 3000) return;
     }
-    delay(500);    
+    delay(100);    
+    Serial1.println("Spur gefunden");  
   }  
 
   //normale Fahrt
@@ -472,19 +551,20 @@ void Spurhalten()
   {
     dir = 'F'; 
     ledGreen(1); 
-    Serial1.println("Spur folgen");  
-    motors.setSpeeds(maxSpeed, maxSpeed);
   }
 }
 
 //Funktion Serielle Ausgabe
-void SerialOutput()                                                     
+void SerielleAusgabe()                                                     
 {
   snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d   Weg: %d %d   Winkel: %d   Zeit: %d"),
+    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
     proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
-    lineValue[0], lineValue[1], lineValue[2],
-    driveRotation[0], driveRotation[1], rotationAngle, CycleTime);
+    lineValue[0], lineValue[1], lineValue[2]);
+  Serial1.println(report);
+  snprintf_P(report, sizeof(report),
+    PSTR("Weg: %d %d   Winkel: %d   Beschleunigung: %d"),
+    driveRotation[0], driveRotation[1], rotationAngle, moveRate);
   Serial1.println(report);
 }
 
@@ -492,39 +572,43 @@ void SerialOutput()
 
 void loop() 
 {
+  LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
+
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
 
   //Abfragen der Sensordaten
-  TimeUpdate();
   LineUpdate();                                         
   ProxUpdate();
+  EncoderUpdate();
   GyroUpdate();
   CompassUpdate();
-  EncoderUpdate();
+  TimeUpdate();
 
   //Funktionsauswahl
   btData = Serial1.readString();
   //verfügbare Funktionen bei laufenden Manövern
-  if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kollision")                                                  
+  if(btData == "Abbiegen" || btData == "Hindernisumfahren" 
+  || btData == "Kontaktvermeiden"|| btData == "Spurfinden")                                                  
   {
-    maxSpeed = 100;
-    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 || 
-    lineValue[2] > 1000) motors.setSpeeds(0, 0);  
+    //Serial1.println("Verstanden");
+    eventSpeed = 2;
+    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || 
+    lineValue[0] > SIGN || lineValue[2] > SIGN) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
-    if(moveRate > 1000) Kollisionserkennung();         
-    else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);  
+    eventSpeed = 1;
+    if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();         
+    if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);  
     else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
-    else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
+    else if(lineValue[0] > SIGN || lineValue[2] > SIGN) Abbiegen();
     else Spurhalten();
   }
 
-  //Ausgabe über Bluetoothverbindung
-  SerialOutput();                                                         
+  LoopTiming();                                                       
 }
 

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,17 +1,17 @@
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 01.03.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.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 die Beschleunigungssensoren (LSM303) erkannt. 
+//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. 
 
-#include <Wire.h>
-#include <Zumo32U4.h>
+#include  <Wire.h>
+#include  <Zumo32U4.h>
 
 Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
 Zumo32U4LineSensors       lineSensors;  //Liniensensoren
@@ -21,10 +21,19 @@ Zumo32U4Encoders          encoders;     //Motorencoder
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
+#define   MARKLINE0       200
+#define   MARKLINE1       100
+#define   MARKLINE2       200
+#define   SIGN            1000
+#define   MAXSPEED        400
+#define   FULLSPEED       100
+#define   HALFSPEED       50
+#define   SLOWSPEED       25
+
 int16_t   lineValue[3];                 //Liniensensoren
 uint16_t  lineOffset[3];                //von links (0) nach rechts (2)
 
-uint8_t   proxValue[4];                 //Abstandssensoren v.l.(0)n.r.(3)
+uint8_t   proxValue[4];                 //Abstandssensoren v.l. (0) n.r. (3)
 
 int16_t   encoderCounts[2];             //Anzahl der Umdrehungen
 int16_t   driveRotation[2];             //von links (0) nach rechts (1)
@@ -35,17 +44,17 @@ int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
 
-int32_t   moveDisplay = 0;              //Beschleunigung
-int32_t   moveDistance = 0;   
+//int32_t   moveDisplay = 0;              //Beschleunigung
+//int32_t   moveDistance = 0;   
 int16_t   moveRate;
 int16_t   compassOffset;                
-uint16_t  compassLastUpdate;
+//uint16_t  compassLastUpdate;
 
-uint16_t  LastUpdate = 0;               //Systemzeit
-uint16_t  CycleTime = 0;                //Zykluszeit
-int       maxSpeed = 200;               //Maximale Geschwindigkeit (max. 400)
-char      dir;                          //Fahrtrichtung, Ereignis
+uint16_t  lastUpdate = 0;               //Systemzeit
+uint16_t  eventTime = 0;                //Zeit seit letzter Geschwindigkeitsänderung
 char      report[200];                  //Ausgabe
+int       eventSpeed = 1;               //vermindert die Geschwindigkeit bei Manövern
+char      dir;                          //Fahrtrichtung, Ereignis
 String    btData;                       //Gelesene Daten aus Bluetooth
 
 /*-----------------------------------------------------------------*/
@@ -60,8 +69,8 @@ void LineSetup()
   uint32_t total[3] = {0, 0, 0};  
   for(uint8_t i = 0; i < 120; i++)                                
   {
-    if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
-    else motors.setSpeeds(-maxSpeed, maxSpeed);
+    if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEED, -FULLSPEED);
+    else motors.setSpeeds(-FULLSPEED, FULLSPEED);
     lineSensors.read(lineOffset);  
     total[0] += lineOffset[0];
     total[1] += lineOffset[1];
@@ -86,7 +95,16 @@ void GyroSetup()
 {                                                                 
   ledYellow(1);
   gyro.init();
-
+  if (!gyro.init())
+  {
+    //Fehler beim Initialisieren des Drehbewegungssensors
+    ledRed(1);
+    while(1)
+    {
+      Serial1.println(F("Fehler Drehbewegungssensors"));
+      delay(100);
+    }
+  }
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
   gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
   gyro.writeReg(L3G::CTRL5, 0b00000000);                          //Hochpassfilter ausgeschaltet
@@ -110,6 +128,16 @@ void CompassSetup()
 {
   ledYellow(1);
   compass.init();
+  if (!compass.init())
+  {
+    //Fehler beim Initialisieren des Beschleunigungssensors
+    ledRed(1);
+    while(1)
+    {
+      Serial.println(F("Fehler Beschleunigungssensors"));
+      delay(100);
+    }
+  }
   compass.enableDefault();  
 
   //Kalibrierung
@@ -121,7 +149,7 @@ void CompassSetup()
   }
   compassOffset = total / 1024;
 
-  compassLastUpdate = micros();  
+  //compassLastUpdate = micros();  
   ledYellow(0);
 }
 
@@ -130,14 +158,14 @@ void CompassSetup()
 void setup() 
 {
   //Initialisierung der Bluetoothverbindung
-  Serial1.begin(9600);                                              
+  Serial1.begin(9600);                           
+  Serial1.setTimeout(10);                   //verkürzt Serial.readString auf 10 ms statt 1s
   if(Serial1.available()) Serial1.println("Verbindung hergestellt");  
 
   //Initialisierung und Kalibrierung der Sensoren
   Serial1.println("Sensorkalibrierung");   
   Wire.begin();                       
   delay(500);
-  LastUpdate = micros();
   ProxSetup();
   LineSetup();                                                
   GyroSetup();
@@ -149,6 +177,8 @@ void setup()
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 /*-----------------------------------------------------------------*/
@@ -156,9 +186,51 @@ void setup()
 //Update Durchlaufzeit
 void TimeUpdate()
 {
-  uint16_t m = micros();
-  CycleTime = m - LastUpdate;
-  LastUpdate = m;
+  uint16_t m = millis();
+  eventTime = m - lastUpdate;
+}
+
+void LoopTiming()
+{
+  const int AL = 100;       // Arraylänge, NUR GERADE Zahlen verwenden!
+  static unsigned long LoopTime[AL];
+  static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;
+ 
+  if (Messung % 2 == 0)     // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
+    {
+     LoopTime[Index] = millis();
+     Messung++;
+     Index++;   
+     return;	              // Funktion sofort beenden, spart etwas Zeit
+    }
+
+  if (Messung % 2 == 1)     // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
+    {
+     LoopTime[Index] = millis();
+     LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1];  	// Loopdauer einen Index niedriger einspeichern wie aktuell
+     Messung++;
+    }	
+	   
+  if (Index >= AL) 	        // Array voll, Daten auswerten
+    { 
+     for (int i = 0; i<AL; i++)
+       {
+        Min = min(Min, LoopTime[i]);
+        Max = max(Max, LoopTime[i]);
+        Avg += LoopTime[i];
+       }
+	
+     Avg = Avg / AL;
+     Serial1.print(F("Minimal       "));Serial1.print(Min);Serial1.println(" ms  ");
+     Serial1.print(F("Durchschnitt  "));Serial1.print(Avg);Serial1.println(" ms  ");
+     Serial1.print(F("Maximal       "));Serial1.print(Max);Serial1.println(" ms  ");
+     SerielleAusgabe();
+     Min = 0xFFFF;
+     Max = 0;
+     Avg = 0;
+     Messung = 0;
+     Index = 0;
+    }
 }
 
 //Update Abstandssensoren
@@ -171,7 +243,7 @@ void ProxUpdate()
   proxValue[3] = proxSensors.countsRightWithRightLeds(); 
 }
 
-//Updaten Liniensensoren
+//Update Liniensensoren
 void LineUpdate()
 {
   uint16_t  lineRaw[3];
@@ -184,8 +256,8 @@ void LineUpdate()
 //Update Drehbewegungssensor
 void GyroUpdate()                                                 
 {
-  gyro.read();
-  turnRate = gyro.g.z - gyroOffset;
+  gyro.read();                              //Rohwert 10285 entspricht 90° bzw.
+  turnRate = gyro.g.z - gyroOffset;         //8,75mdps/LSB
   uint16_t m = micros();
   uint16_t dt = m - gyroLastUpdate;
   gyroLastUpdate = m;
@@ -194,17 +266,17 @@ void GyroUpdate()
   rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
 }
 
-// Update Beschleunigungssensor
+//Update Beschleunigungssensor
 void CompassUpdate()
 {
-  compass.read();
-  moveRate = compass.a.x - compassOffset;
-  uint16_t m = micros();
-  uint16_t dt = m - compassLastUpdate;
-  compassLastUpdate = m;
-  int32_t d = (int32_t)moveRate * dt;
-  moveDistance += (int64_t)d * dt >> 14;
-  moveDisplay = moveDistance * 1000 / 9,81;
+  compass.read();                           //Rohwert 16384 entspricht 1g (9,81m/s²) bzw.
+  moveRate = compass.a.x - compassOffset;   //bei +/-2g Messbereich 0,61mg/LSB
+//   uint16_t m = micros();
+//   uint16_t dt = m - compassLastUpdate;
+//   compassLastUpdate = m;
+//   int32_t d = (int32_t)moveRate * dt;
+//   moveDistance += (int64_t)d * dt >> 14;
+//   moveDisplay = moveDistance * 9,81 / 1000;
 }
 
 //Update Encoder
@@ -230,11 +302,14 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
-    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
-    if(lineValue[0] > 300 || lineValue[2] > 300) return;
+    motors.setSpeeds(-HALFSPEED, -HALFSPEED);
+    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) return;
   }
   delay(500); 
+
   Serial1.println("Vermeiden beendet");
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 //Funktion Hindernisumfahrung
@@ -251,36 +326,36 @@ void Hindernisumfahren()
   while(rotationAngle < 45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(maxSpeed/2, maxSpeed); 
+    motors.setSpeeds(HALFSPEED, FULLSPEED); 
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[2] < 300)          
+  while(lineValue[2] < MARKLINE2)          
   {
     LineUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed); 
+    motors.setSpeeds(FULLSPEED, FULLSPEED); 
   } 
   //rechts drehen
   GyroUpdate();
   while(rotationAngle > 0)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed/2); 
+    motors.setSpeeds(FULLSPEED, HALFSPEED); 
   }
   //geradeaus fahren
-  motors.setSpeeds(maxSpeed, maxSpeed); 
+  motors.setSpeeds(FULLSPEED, FULLSPEED); 
 
   //Gegenverkehr beachten
   ProxUpdate();
   if(proxValue[1] < 5 || proxValue[2] < 5)
   {
     //Schritt 2: Hindernis passieren
-    motors.setSpeeds(maxSpeed, maxSpeed);                    
+    motors.setSpeeds(FULLSPEED, FULLSPEED);                    
     delay(1000);
     ProxUpdate(); 
     while(proxValue[3] > 4)
     {
-      motors.setSpeeds(maxSpeed, maxSpeed);  
+      motors.setSpeeds(FULLSPEED, FULLSPEED);  
       ProxUpdate();
       Spurhalten();
     }
@@ -293,26 +368,26 @@ void Hindernisumfahren()
   while(rotationAngle > -45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(maxSpeed, maxSpeed/2); 
+    motors.setSpeeds(FULLSPEED, HALFSPEED); 
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[0] < 300)          
+  while(lineValue[0] < MARKLINE0)          
   {
     LineUpdate();
-    motors.setSpeeds(maxSpeed, maxSpeed); 
+    motors.setSpeeds(FULLSPEED, FULLSPEED); 
   } 
   //links drehen
   GyroUpdate();
   while(rotationAngle < 0)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(maxSpeed/2, maxSpeed); 
+    motors.setSpeeds(HALFSPEED, FULLSPEED);
   }
   //geradeaus fahren
-  motors.setSpeeds(maxSpeed, maxSpeed); 
-
-  Serial1.println("Umfahren beendet");  
+  Serial1.println("Umfahren beendet");
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 //Funktion Abbiegen
@@ -325,9 +400,9 @@ void Abbiegen()
   //Markierung analysieren
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if(lineValue[0] > 1000) leftCode = true;
+  if(lineValue[0] > SIGN) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > 1000) rightCode = true;
+  if(lineValue[2] > SIGN) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
@@ -346,10 +421,10 @@ void Abbiegen()
     //zur Kreuzungsmitte fahren
     driveRotation[0] = 0;
     driveRotation[1] = 0;
-    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
+    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
     {
       EncoderUpdate();
-      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+      motors.setSpeeds(HALFSPEED, HALFSPEED);
     }
     //links drehen
     rotationAngle = 0;
@@ -357,13 +432,11 @@ void Abbiegen()
     while(rotationAngle != 90)
     {
       GyroUpdate();
-      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
-      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
-      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(0, maxSpeed/2);
-      else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN) motors.setSpeeds(HALFSPEED, HALFSPEED);
+      else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN) motors.setSpeeds(-SLOWSPEED, -SLOWSPEED);
+      else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN) motors.setSpeeds(0, HALFSPEED);
+      else motors.setSpeeds(SLOWSPEED, HALFSPEED);
     }
-    //geradeaus fahren
-    motors.setSpeeds(maxSpeed/2, maxSpeed/2);
   }
 
   //rechts Abbiegen
@@ -375,57 +448,61 @@ void Abbiegen()
     while(rotationAngle != -90)
     {
       GyroUpdate();
-      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, 0);
-      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
-      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
-      else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN) motors.setSpeeds(HALFSPEED, 0);
+      else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN) motors.setSpeeds(-SLOWSPEED, -SLOWSPEED);
+      else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN) motors.setSpeeds(HALFSPEED, HALFSPEED);
+      else motors.setSpeeds(HALFSPEED, SLOWSPEED);
     }
-    //geradeaus fahren
-    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
   }  
 
   //nicht Abbiegen, geradeaus fahren
-  else motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+  else motors.setSpeeds(HALFSPEED, HALFSPEED); 
   delay(500);
 
-  Serial1.println("Abbiegen beendet");  
+  //geradeaus fahren
+  Serial1.println("Abbiegen beendet");
+  motors.setSpeeds(FULLSPEED, FULLSPEED);
+  lastUpdate = millis();
 }
 
 //Funktion Spurhalten
 void Spurhalten()                                                   
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > 300 && lineValue[0] < 500)                      
+  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN)                      
   {
     dir = 'R';
     ledYellow(1); 
-    Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(maxSpeed, maxSpeed/2);
+    //Serial1.println("Spur nach rechts korrigieren");  
+    motors.setSpeeds(FULLSPEED/eventSpeed, SLOWSPEED/eventSpeed);
     delay(100);    
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > 300 && lineValue[2] < 500)                 
+  else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN)                 
   {
     dir = 'L'; 
     ledYellow(1); 
-    Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(maxSpeed/2, maxSpeed);  
+    //Serial1.println("Spur nach links korrigieren");  
+    motors.setSpeeds(SLOWSPEED/eventSpeed, FULLSPEED/eventSpeed);  
     delay(100);  
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > 300 && lineValue[1] < 500)                 
+  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN)                 
   { 
+    
     dir = 'B';  
     ledRed(1); 
     Serial1.println("Spurfinden");  
-    Serial1.println("1"); 
-    while(lineValue[0] < 300 && lineValue[2] < 300)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
+    uint16_t startTime = millis();
+    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
     {
-      motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+      motors.setSpeeds(-HALFSPEED/eventSpeed, -HALFSPEED/eventSpeed);
+      uint16_t backTime = millis();
+      if((backTime - startTime) > 3000) return;
     }
-    delay(500);    
+    delay(100);    
     Serial1.println("Spur gefunden");  
   }  
 
@@ -434,19 +511,20 @@ void Spurhalten()
   {
     dir = 'F'; 
     ledGreen(1); 
-    Serial1.println("Spur folgen");  
-    motors.setSpeeds(maxSpeed, maxSpeed);
   }
 }
 
 //Funktion Serielle Ausgabe
-void SerialOutput()                                                     
+void SerielleAusgabe()                                                     
 {
   snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d   Weg: %d %d   Winkel: %d   Zeit: %d"),
+    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
     proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
-    lineValue[0], lineValue[1], lineValue[2],
-    driveRotation[0], driveRotation[1], rotationAngle, CycleTime);
+    lineValue[0], lineValue[1], lineValue[2]);
+  Serial1.println(report);
+  snprintf_P(report, sizeof(report),
+    PSTR("Weg: %d %d   Winkel: %d   Beschleunigung: %d"),
+    driveRotation[0], driveRotation[1], rotationAngle, moveRate);
   Serial1.println(report);
 }
 
@@ -454,17 +532,19 @@ void SerialOutput()
 
 void loop() 
 {
+  LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
+
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
 
   //Abfragen der Sensordaten
-  TimeUpdate();
   LineUpdate();                                         
   ProxUpdate();
+  EncoderUpdate();
   GyroUpdate();
   CompassUpdate();
-  EncoderUpdate();
+  TimeUpdate();
 
   //Funktionsauswahl
   btData = Serial1.readString();
@@ -472,21 +552,22 @@ void loop()
   if(btData == "Abbiegen" || btData == "Hindernisumfahren" 
   || btData == "Kontaktvermeiden"|| btData == "Spurfinden")                                                  
   {
-    maxSpeed = 100;
-    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 || 
-    lineValue[2] > 1000) motors.setSpeeds(0, 0);  
+    //Serial1.println("Verstanden");
+    eventSpeed = 2;
+    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || 
+    lineValue[0] > SIGN || lineValue[2] > SIGN) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
-    if(moveRate > 1000) Kontaktvermeiden();         
-    else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);  
+    eventSpeed = 1;
+    if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();         
+    if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);  
     else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
-    else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
+    else if(lineValue[0] > SIGN || lineValue[2] > SIGN) Abbiegen();
     else Spurhalten();
   }
 
-  //Ausgabe über Bluetoothverbindung
-  SerialOutput();                                                         
+  LoopTiming();                                                       
 }