"Kontaktvermeiden" now working

ArduinoOutput/preproc/ctags_target_for_gcc_minus_e.cpp

@@ -1,7 +1,7 @@
 # 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 # 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 14.05.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 17.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 
 
@@ -33,14 +33,16 @@ Zumo32U4Encoders encoders; //Motorencoder
 LSM303 compass; //Beschleunigungssensor x-Achse
 L3G gyro; //Drehbewegungssensor z-Achse
 
+//Medianfilter geben mittleren Wert einer Reihe mit ungerader Anzahl aus
 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 LineFilter1(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
+MedianFilter LineFilter2(3, 0);
+
 MedianFilter ProxFilter0(5, 0); //erstellen der Filter für Abstandssensoren
-MedianFilter ProxFilter1(5, 0);
+MedianFilter ProxFilter1(5, 0); //Fenstergröße: 5, Basiswerte: 0 0 0 0 0
 MedianFilter ProxFilter2(5, 0);
 MedianFilter ProxFilter3(5, 0);
-# 48 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+# 51 "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)
 
@@ -55,18 +57,18 @@ int16_t turnRate;
 int16_t gyroOffset;
 uint16_t gyroLastUpdate;
 
-//int32_t   moveDisplay = 0;              //Beschleunigung
-//int32_t   moveDistance = 0;   
-int16_t moveRate;
+int16_t compassRate; //Beschleunigung
 int16_t compassOffset;
-//uint16_t  compassLastUpdate;
+int16_t compassLastUpdate;
 
 uint16_t lastUpdate = 0; //Systemzeit
-uint16_t eventTime = 0; //Zeit seit letzter Geschwindigkeitsänderung
-char report[200]; //Ausgabe
+uint16_t eventTime = 0; //Zeitdifferenz
+uint16_t stopUpdate = 0; //Systemzeit
+uint16_t stopTime = 0; //Zeit seit letztem Manöver
 float eventSpeed = 1; //vermindert die Geschwindigkeit bei Manövern
 int btData = 0; //Gelesene Daten aus Bluetooth
-bool alarm = false; //zeigt Manöver abhängig von btData
+bool btBuffer = false; //puffert Daten von btData
+bool stop = false; //Sperrt Funktion Kontaktvermeiden
 
 /*-----------------------------------------------------------------*/
 
@@ -169,7 +171,7 @@ void CompassSetup()
   int32_t total = 0;
   for (uint16_t i = 0; i < 1024; i++)
   {
-    compass.read();
+    compass.readAcc();
     total += compass.a.x;
   }
   compassOffset = total / 1024;
@@ -200,18 +202,26 @@ void setup()
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
+  stopUpdate = millis();
   //Serial1.println("Start");
 }
 
 /*-----------------------------------------------------------------*/
 
-//Update Durchlaufzeit
+//Update Zeitdifferenz für alle Funktionen
 void TimeUpdate()
 {
   uint16_t m = millis();
   eventTime = m - lastUpdate;
 }
 
+//Update Zeit für Kontaktvermeiden
+void StopUpdate()
+{
+  uint16_t m = millis();
+  stopTime = m - stopUpdate;
+}
+
 void LoopTiming()
 {
   const int AL = 100; // Arraylänge, NUR GERADE Zahlen verwenden!
@@ -255,6 +265,21 @@ void LoopTiming()
     }
 }
 
+//Funktion Serielle Ausgabe
+void SerielleAusgabe()
+{
+  char report[200];
+  snprintf_P(report, sizeof(report),
+    (__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]);
+  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, compassRate);
+  Serial1.println(report);
+}
+
 //Update Abstandssensoren
 void ProxUpdate()
 {
@@ -272,10 +297,6 @@ void ProxUpdate()
       proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
       proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
       proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
-      // Serial.println(proxValue[0]);
-      // Serial.println(proxValue[1]);
-      // Serial.println(proxValue[2]);
-      // Serial.println(proxValue[3]);
       done = true;
     }
     else
@@ -312,22 +333,18 @@ void GyroUpdate()
 //Update Beschleunigungssensor
 void CompassUpdate()
 {
-  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;
+  compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw. bei +/-2g Messbereich 0,61mg/LSB
+  int16_t x = compass.a.x - compassOffset;
+  compassRate = x - compassLastUpdate;
+  compassLastUpdate = x;
 }
 
 //Update Encoder
 void EncoderUpdate()
 {
-  encoderCounts[0] = encoders.getCountsLeft();
+  encoderCounts[0] += encoders.getCountsAndResetLeft();
   driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
-  encoderCounts[1] = encoders.getCountsRight();
+  encoderCounts[1] += encoders.getCountsAndResetRight();
   driveRotation[1] = encoderCounts[1] / 910;
 }
 
@@ -344,10 +361,25 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
+    LineUpdate();
+    motors.setSpeeds(-52, -50);
+    if(lineValue[0] > 150 || lineValue[1] > 100 || lineValue[2] > 120) break;
+  }
+  lastUpdate = millis();
+  TimeUpdate();
+  while(eventTime < 1000)
+  {
+    TimeUpdate();
+    LineUpdate();
     motors.setSpeeds(-52, -50);
-    if(lineValue[0] > 150 || lineValue[2] > 120) break;
+    if(lineValue[0] > 150 || lineValue[1] > 100 || lineValue[2] > 120) break;
   }
   motors.setSpeeds(0, 0);
+  compassLastUpdate = 0;
+  compassRate = 0;
+  CompassUpdate();
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Vermeiden beendet");
   Serial1.print(0);
 }
@@ -378,14 +410,15 @@ void Hindernisumfahren()
     motors.setSpeeds(26, 100);
     if(lineValue[2] > 120 && lineValue[2] < 500) break;
   }
+
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[2] < 120)
   {
     LineUpdate();
     motors.setSpeeds(104, 100);
-    //if(lineValue[0] > MARKLINE0) break;
   }
+
   //rechts drehen
   GyroUpdate();
   while(rotationAngle > 10)
@@ -396,12 +429,13 @@ void Hindernisumfahren()
     else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 100);
     else motors.setSpeeds(104, 25);
   }
+
   //geradeaus fahren
   motors.setSpeeds(104, 100);
 
   //Gegenverkehr beachten
   ProxUpdate();
-  //if(proxValue[1] < 5 || proxValue[2] < 5)
+  //if(proxValue[1] == 6 && proxValue[2] == 6)
   //{
     //Schritt 2: Hindernis passieren
     //Serial1.println("Aufholen"); 
@@ -454,14 +488,15 @@ void Hindernisumfahren()
     motors.setSpeeds(104, 25);
     if(lineValue[0] > 150 && lineValue[0] < 500) break;
   }
+
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[0] < 150)
   {
     LineUpdate();
     motors.setSpeeds(104, 100);
-    //if(lineValue[0] > MARKLINE0) break;
   }
+
   //links drehen
   GyroUpdate();
   while(rotationAngle < -10)
@@ -472,10 +507,13 @@ void Hindernisumfahren()
     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(104, 100);
+  stop = false;
+  stopUpdate = millis();
+  Serial1.print(0);
 }
 
 //Funktion Abbiegen
@@ -526,8 +564,9 @@ void Abbiegen()
     while(lineValue[0] < 150 && lineValue[2] < 120)
     {
       LineUpdate();
-      motors.setSpeeds(104, 100);
+      motors.setSpeeds(104, 100 +10);
     }
+
     //links drehen
     turnAngle = 0;
     rotationAngle = 0;
@@ -540,6 +579,7 @@ void Abbiegen()
       else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(52, 100);
       else motors.setSpeeds(26, 100);
     }
+
     //geradeaus fahren
     motors.setSpeeds(104, 100);
   }
@@ -549,26 +589,22 @@ void Abbiegen()
   {
     //Serial1.println("links Abbiegen vom Rundkurs"); 
     //links drehen
+    motors.setSpeeds(26, 100);
     turnAngle = 0;
     rotationAngle = 0;
-    driveRotation[1] = 0;
     GyroUpdate();
     while(rotationAngle < 40)
     {
       GyroUpdate();
-      EncoderUpdate();
       motors.setSpeeds(26, 100);
-      //if(driveRotation[1] > 1) break;
     }
-    driveRotation[1] = 0;
     GyroUpdate();
     while(rotationAngle < 85)
     {
       GyroUpdate();
-      EncoderUpdate();
       motors.setSpeeds(26, 100);
-      //if(driveRotation[1] > 1) break;
     }
+
     //geradeaus fahren
     motors.setSpeeds(104, 100);
     lastUpdate = millis();
@@ -577,6 +613,7 @@ void Abbiegen()
     {
       TimeUpdate();
       LineUpdate();
+      motors.setSpeeds(104, 100);
       if(lineValue[2] > 120 && lineValue[2] < 500) break;
     }
     lastUpdate = millis();
@@ -594,14 +631,13 @@ void Abbiegen()
   {
     //Serial1.println("rechts Abbiegen"); 
     //rechts drehen
+    motors.setSpeeds(104, 25);
     turnAngle = 0;
     rotationAngle = 0;
-    driveRotation[0] = 0;
     GyroUpdate();
     while(rotationAngle > -40)
     {
       GyroUpdate();
-      EncoderUpdate();
       LineUpdate();
       motors.setSpeeds(104, 25);
       if(lineValue[0] > 150 && lineValue[0] < 500) break;
@@ -635,6 +671,8 @@ void Abbiegen()
       Spurhalten();
     }
   }
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Abbiegen beendet");
   Serial1.print(0);
 }
@@ -651,7 +689,7 @@ void Spurhalten()
   {
     ledYellow(1);
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(104/eventSpeed, 25/eventSpeed);
+    motors.setSpeeds((int)104/eventSpeed, (int)25/eventSpeed);
     while(Time < 100)
     {
       Update = millis();
@@ -659,6 +697,8 @@ void Spurhalten()
       LineUpdate();
       if(lineValue[2] > 120) break;
     }
+    stop = false;
+    stopUpdate = millis();
   }
 
   //rechte Linie erreicht, nach links fahren
@@ -666,7 +706,7 @@ void Spurhalten()
   {
     ledYellow(1);
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(26/eventSpeed, 100/eventSpeed);
+    motors.setSpeeds((int)26/eventSpeed, (int)100/eventSpeed);
     while(Time < 100)
     {
       Update = millis();
@@ -674,13 +714,16 @@ void Spurhalten()
       LineUpdate();
       if(lineValue[0] > 150) break;
     }
+    stop = false;
+    stopUpdate = millis();
   }
 
   //normale Fahrt
   else
   {
     ledGreen(1);
-    motors.setSpeeds(104/eventSpeed, 100/eventSpeed);
+    motors.setSpeeds((int)104/eventSpeed, (int)100/eventSpeed);
+    stop = true;
   }
 }
 
@@ -698,24 +741,12 @@ void Spurfinden()
     motors.setSpeeds(-104, -100);
     if(eventTime > 3000) break;
   }
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Spur gefunden"); 
   Serial1.print(0);
 }
 
-//Funktion Serielle Ausgabe
-void SerielleAusgabe()
-{
-  snprintf_P(report, sizeof(report),
-    (__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]);
-  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);
-}
-
 /*-----------------------------------------------------------------*/
 
 void loop()
@@ -732,27 +763,31 @@ void loop()
   CompassUpdate();
   LineUpdate();
   TimeUpdate();
+  StopUpdate();
 
   //Funktionsauswahl
-  //btData = Serial1.readString();
   if(Serial1.available() > 0) btData = Serial1.read();
-  if(btData == '1') alarm = true;
-  else if(btData == '0') alarm = false;
+  if(btData == '1') btBuffer = true;
+  else if(btData == '0') btBuffer = false;
+
   //verfügbare Funktionen bei laufenden Manövern
-  //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")   
-  if(alarm == true)
+  if(btBuffer == true)
   {
     //Serial1.println("Verstanden");
     eventSpeed = 1.4;
-    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > 500 || lineValue[2] > 500 || lineValue[1] > 100) motors.setSpeeds(0, 0);
+    if(proxValue[0] == 6 || (proxValue[1] == 6 && proxValue[2] == 6)) motors.setSpeeds(0, 0);
+    else if(stop == true && stopTime > 2000 && ((compassRate)>0?(compassRate):-(compassRate)) > 3500) motors.setSpeeds(0, 0);
+    else if(lineValue[0] > 500 || lineValue[2] > 500 || lineValue[1] > 100) motors.setSpeeds(0, 0);
     else Spurhalten();
   }
+
   //verfügbare Funktionen im Normalfall
   else
   {
     eventSpeed = 1.0;
-    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
-    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    if(stop == true && stopTime > 2000 && ((compassRate)>0?(compassRate):-(compassRate)) > 3500) Kontaktvermeiden();
+    else if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    else if(lineValue[0] > 1600 && lineValue[2] > 1600) motors.setSpeeds(0, 0);
     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();

ArduinoOutput/sketch/Hauptprojekt.ino.cpp

@@ -2,7 +2,7 @@
 #line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 #line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 14.05.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 17.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). 
@@ -26,11 +26,13 @@ Zumo32U4Encoders          encoders;     //Motorencoder
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
+//Medianfilter geben mittleren Wert einer Reihe mit ungerader Anzahl aus
 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 LineFilter1(3, 0);         //Fenstergröße: 3, Basiswerte: 0 0 0
+MedianFilter LineFilter2(3, 0);         
+
 MedianFilter ProxFilter0(5, 0);         //erstellen der Filter für Abstandssensoren
-MedianFilter ProxFilter1(5, 0);
+MedianFilter ProxFilter1(5, 0);         //Fenstergröße: 5, Basiswerte: 0 0 0 0 0
 MedianFilter ProxFilter2(5, 0);
 MedianFilter ProxFilter3(5, 0);
 
@@ -40,6 +42,7 @@ MedianFilter ProxFilter3(5, 0);
 #define   SIGN0           500
 #define   SIGN1           300
 #define   SIGN2           500
+
 #define   MAXSPEED        400
 #define   FULLSPEEDLEFT   104
 #define   HALFSPEEDLEFT   52
@@ -61,64 +64,66 @@ int32_t   turnAngle = 0;
 int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
-
-//int32_t   moveDisplay = 0;              //Beschleunigung
-//int32_t   moveDistance = 0;   
-int16_t   moveRate;
-int16_t   compassOffset;                
-//uint16_t  compassLastUpdate;
+            
+int16_t   compassRate;                  //Beschleunigung
+int16_t   compassOffset;          
+int16_t   compassLastUpdate;      
 
 uint16_t  lastUpdate = 0;               //Systemzeit
-uint16_t  eventTime = 0;                //Zeit seit letzter Geschwindigkeitsänderung
-char      report[200];                  //Ausgabe
+uint16_t  eventTime = 0;                //Zeitdifferenz
+uint16_t  stopUpdate = 0;               //Systemzeit
+uint16_t  stopTime = 0;                 //Zeit seit letztem Manöver
 float     eventSpeed = 1;               //vermindert die Geschwindigkeit bei Manövern
 int       btData = 0;                   //Gelesene Daten aus Bluetooth
-bool      alarm = false;                //zeigt Manöver abhängig von btData
+bool      btBuffer = false;             //puffert Daten von btData
+bool      stop = false;                 //Sperrt Funktion Kontaktvermeiden
 
 /*-----------------------------------------------------------------*/
 
 //Setup Bluetoothverbindung
-#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void BlueSetup();
-#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 91 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineSetup();
-#line 117 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 120 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxSetup();
-#line 123 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 126 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroSetup();
-#line 156 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 159 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassSetup();
-#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 190 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void setup();
-#line 213 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 217 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void TimeUpdate();
-#line 219 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 224 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void StopUpdate();
+#line 230 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LoopTiming();
-#line 263 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 274 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void SerielleAusgabe();
+#line 289 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxUpdate();
-#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 316 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineUpdate();
-#line 304 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 326 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroUpdate();
-#line 317 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 339 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassUpdate();
-#line 330 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 348 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void EncoderUpdate();
-#line 341 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 359 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Kontaktvermeiden();
-#line 360 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 393 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Hindernisumfahren();
-#line 486 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 525 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Abbiegen();
-#line 647 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 686 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurhalten();
-#line 692 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 736 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurfinden();
-#line 710 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
-void SerielleAusgabe();
-#line 725 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 757 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void loop();
-#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void BlueSetup()
 {
   Serial1.begin(9600);                              //Initialisierung mit Datengeschwindigkeit(Baud)
@@ -217,7 +222,7 @@ void CompassSetup()
   int32_t total = 0;                                              
   for (uint16_t i = 0; i < 1024; i++)
   {
-    compass.read();
+    compass.readAcc();
     total += compass.a.x;
   }
   compassOffset = total / 1024;
@@ -248,18 +253,26 @@ void setup()
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
+  stopUpdate = millis();
   //Serial1.println("Start");
 }
 
 /*-----------------------------------------------------------------*/
 
-//Update Durchlaufzeit
+//Update Zeitdifferenz für alle Funktionen
 void TimeUpdate()
 {
   uint16_t m = millis();
   eventTime = m - lastUpdate;
 }
 
+//Update Zeit für Kontaktvermeiden
+void StopUpdate()
+{
+  uint16_t m = millis();
+  stopTime = m - stopUpdate;
+}
+
 void LoopTiming()
 {
   const int AL = 100;       // Arraylänge, NUR GERADE Zahlen verwenden!
@@ -303,6 +316,21 @@ void LoopTiming()
     }
 }
 
+//Funktion Serielle Ausgabe
+void SerielleAusgabe()                                                     
+{
+  char report[200]; 
+  snprintf_P(report, sizeof(report),
+    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
+    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
+    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, compassRate);
+  Serial1.println(report);
+}
+
 //Update Abstandssensoren
 void ProxUpdate()
 {
@@ -320,10 +348,6 @@ void ProxUpdate()
       proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
       proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());  
       proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds()); 
-      // Serial.println(proxValue[0]);
-      // Serial.println(proxValue[1]);
-      // Serial.println(proxValue[2]);
-      // Serial.println(proxValue[3]);
       done = true;
     }
     else
@@ -360,22 +384,18 @@ void GyroUpdate()
 //Update Beschleunigungssensor
 void CompassUpdate()
 {
-  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;
+  compass.read();                           //Rohwert 16384 entspricht 1g (9,81m/s²) bzw. bei +/-2g Messbereich 0,61mg/LSB
+  int16_t x = compass.a.x - compassOffset;
+  compassRate = x - compassLastUpdate;
+  compassLastUpdate = x;
 }
 
 //Update Encoder
 void EncoderUpdate()
 {
-  encoderCounts[0] = encoders.getCountsLeft();
+  encoderCounts[0] += encoders.getCountsAndResetLeft();
   driveRotation[0] = encoderCounts[0] / 910;                        //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
-  encoderCounts[1] = encoders.getCountsRight();
+  encoderCounts[1] += encoders.getCountsAndResetRight();
   driveRotation[1] = encoderCounts[1] / 910;
 }
 
@@ -392,10 +412,25 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
+    LineUpdate();
     motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
-    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
+    if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
+  }
+  lastUpdate = millis();
+  TimeUpdate();
+  while(eventTime < 1000)
+  {
+    TimeUpdate();
+    LineUpdate();
+    motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
+    if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
   }
   motors.setSpeeds(0, 0);
+  compassLastUpdate = 0;
+  compassRate = 0;
+  CompassUpdate();
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Vermeiden beendet");
   Serial1.print(0); 
 }
@@ -426,14 +461,15 @@ void Hindernisumfahren()
     motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
     if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
   }
+
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[2] < MARKLINE2)          
   {
     LineUpdate();
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
-    //if(lineValue[0] > MARKLINE0) break;
   } 
+
   //rechts drehen
   GyroUpdate();
   while(rotationAngle > 10)                                               
@@ -444,12 +480,13 @@ void Hindernisumfahren()
     else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
   }
+
   //geradeaus fahren
   motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
 
   //Gegenverkehr beachten
   ProxUpdate();
-  //if(proxValue[1] < 5 || proxValue[2] < 5)
+  //if(proxValue[1] == 6 && proxValue[2] == 6)
   //{
     //Schritt 2: Hindernis passieren
     //Serial1.println("Aufholen"); 
@@ -502,14 +539,15 @@ void Hindernisumfahren()
     motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
     if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
   }
+
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[0] < MARKLINE0)          
   {
     LineUpdate();
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
-    //if(lineValue[0] > MARKLINE0) break;
   } 
+
   //links drehen
   GyroUpdate();
   while(rotationAngle < -10)                                               
@@ -520,10 +558,13 @@ void Hindernisumfahren()
     else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
   }
+
   //geradeaus fahren
   //Serial1.println("Umfahren beendet");
-  Serial1.print(0); 
   motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
+  stop = false;
+  stopUpdate = millis();
+  Serial1.print(0); 
 }
 
 //Funktion Abbiegen
@@ -574,8 +615,9 @@ void Abbiegen()
     while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
     {
       LineUpdate();
-      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
     }
+
     //links drehen
     turnAngle = 0;  
     rotationAngle = 0;
@@ -588,6 +630,7 @@ void Abbiegen()
       else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT); 
       else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
     }
+
     //geradeaus fahren
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
   }
@@ -597,26 +640,22 @@ void Abbiegen()
   {
     //Serial1.println("links Abbiegen vom Rundkurs"); 
     //links drehen
+    motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
     turnAngle = 0;  
     rotationAngle = 0;
-    driveRotation[1] = 0;
     GyroUpdate();
     while(rotationAngle < 40)
     {
       GyroUpdate();
-      EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      //if(driveRotation[1] > 1) break;
     }
-    driveRotation[1] = 0;
     GyroUpdate();
     while(rotationAngle < 85)
     {
       GyroUpdate();
-      EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      //if(driveRotation[1] > 1) break;
     }
+
     //geradeaus fahren
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     lastUpdate = millis();
@@ -625,6 +664,7 @@ void Abbiegen()
     {
       TimeUpdate();
       LineUpdate();
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
       if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
     }
     lastUpdate = millis();
@@ -642,14 +682,13 @@ void Abbiegen()
   {
     //Serial1.println("rechts Abbiegen"); 
     //rechts drehen
+    motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
     turnAngle = 0;  
     rotationAngle = 0;
-    driveRotation[0] = 0;
     GyroUpdate();
     while(rotationAngle > -40)
     {
       GyroUpdate();
-      EncoderUpdate();
       LineUpdate();
       motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
       if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
@@ -683,6 +722,8 @@ void Abbiegen()
       Spurhalten();
     }
   }
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Abbiegen beendet");
   Serial1.print(0); 
 }
@@ -699,7 +740,7 @@ void Spurhalten()
   {
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
+    motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)SLOWSPEEDRIGHT/eventSpeed);
     while(Time < 100)
     {
       Update = millis();
@@ -707,6 +748,8 @@ void Spurhalten()
       LineUpdate();
       if(lineValue[2] > MARKLINE2) break;
     }   
+    stop = false;
+    stopUpdate = millis();
   }
 
   //rechte Linie erreicht, nach links fahren
@@ -714,7 +757,7 @@ void Spurhalten()
   {
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);   
+    motors.setSpeeds((int)SLOWSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);   
     while(Time < 100)
     {
       Update = millis();
@@ -722,13 +765,16 @@ void Spurhalten()
       LineUpdate();
       if(lineValue[0] > MARKLINE0) break;
     }   
+    stop = false;
+    stopUpdate = millis();
   }
 
   //normale Fahrt
   else                                                                
   {
     ledGreen(1); 
-    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
+    motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);
+    stop = true;
   }
 }
 
@@ -746,24 +792,12 @@ void Spurfinden()
     motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
     if(eventTime > 3000) break;
   } 
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Spur gefunden"); 
   Serial1.print(0);  
 }  
 
-//Funktion Serielle Ausgabe
-void SerielleAusgabe()                                                     
-{
-  snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
-    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
-    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);
-}
-
 /*-----------------------------------------------------------------*/
 
 void loop() 
@@ -780,27 +814,31 @@ void loop()
   CompassUpdate();
   LineUpdate();      
   TimeUpdate();
+  StopUpdate();
 
   //Funktionsauswahl
-  //btData = Serial1.readString();
   if(Serial1.available() > 0) btData = Serial1.read();
-  if(btData == '1') alarm = true;
-  else if(btData == '0') alarm = false;
+  if(btData == '1') btBuffer = true;
+  else if(btData == '0') btBuffer = false;
+
   //verfügbare Funktionen bei laufenden Manövern
-  //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")   
-  if(alarm == true)                                    
+  if(btBuffer == true)                                    
   {
     //Serial1.println("Verstanden");
     eventSpeed = 1.4;
-    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
+    if(proxValue[0] == 6 || (proxValue[1] == 6 && proxValue[2] == 6)) motors.setSpeeds(0, 0); 
+    else if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) motors.setSpeeds(0, 0); 
+    else if(lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
+
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
     eventSpeed = 1.0;
-    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
-    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) Kontaktvermeiden();           
+    else if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    else if(lineValue[0] > 1600 && lineValue[2] > 1600) motors.setSpeeds(0, 0);
     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();

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,5 +1,5 @@
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 14.05.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 17.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). 
@@ -23,11 +23,13 @@ Zumo32U4Encoders          encoders;     //Motorencoder
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
+//Medianfilter geben mittleren Wert einer Reihe mit ungerader Anzahl aus
 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 LineFilter1(3, 0);         //Fenstergröße: 3, Basiswerte: 0 0 0
+MedianFilter LineFilter2(3, 0);         
+
 MedianFilter ProxFilter0(5, 0);         //erstellen der Filter für Abstandssensoren
-MedianFilter ProxFilter1(5, 0);
+MedianFilter ProxFilter1(5, 0);         //Fenstergröße: 5, Basiswerte: 0 0 0 0 0
 MedianFilter ProxFilter2(5, 0);
 MedianFilter ProxFilter3(5, 0);
 
@@ -37,6 +39,7 @@ MedianFilter ProxFilter3(5, 0);
 #define   SIGN0           500
 #define   SIGN1           300
 #define   SIGN2           500
+
 #define   MAXSPEED        400
 #define   FULLSPEEDLEFT   104
 #define   HALFSPEEDLEFT   52
@@ -58,19 +61,19 @@ int32_t   turnAngle = 0;
 int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
-
-//int32_t   moveDisplay = 0;              //Beschleunigung
-//int32_t   moveDistance = 0;   
-int16_t   moveRate;
-int16_t   compassOffset;                
-//uint16_t  compassLastUpdate;
+            
+int16_t   compassRate;                  //Beschleunigung
+int16_t   compassOffset;          
+int16_t   compassLastUpdate;      
 
 uint16_t  lastUpdate = 0;               //Systemzeit
-uint16_t  eventTime = 0;                //Zeit seit letzter Geschwindigkeitsänderung
-char      report[200];                  //Ausgabe
+uint16_t  eventTime = 0;                //Zeitdifferenz
+uint16_t  stopUpdate = 0;               //Systemzeit
+uint16_t  stopTime = 0;                 //Zeit seit letztem Manöver
 float     eventSpeed = 1;               //vermindert die Geschwindigkeit bei Manövern
 int       btData = 0;                   //Gelesene Daten aus Bluetooth
-bool      alarm = false;                //zeigt Manöver abhängig von btData
+bool      btBuffer = false;             //puffert Daten von btData
+bool      stop = false;                 //Sperrt Funktion Kontaktvermeiden
 
 /*-----------------------------------------------------------------*/
 
@@ -173,7 +176,7 @@ void CompassSetup()
   int32_t total = 0;                                              
   for (uint16_t i = 0; i < 1024; i++)
   {
-    compass.read();
+    compass.readAcc();
     total += compass.a.x;
   }
   compassOffset = total / 1024;
@@ -204,18 +207,26 @@ void setup()
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
+  stopUpdate = millis();
   //Serial1.println("Start");
 }
 
 /*-----------------------------------------------------------------*/
 
-//Update Durchlaufzeit
+//Update Zeitdifferenz für alle Funktionen
 void TimeUpdate()
 {
   uint16_t m = millis();
   eventTime = m - lastUpdate;
 }
 
+//Update Zeit für Kontaktvermeiden
+void StopUpdate()
+{
+  uint16_t m = millis();
+  stopTime = m - stopUpdate;
+}
+
 void LoopTiming()
 {
   const int AL = 100;       // Arraylänge, NUR GERADE Zahlen verwenden!
@@ -259,6 +270,21 @@ void LoopTiming()
     }
 }
 
+//Funktion Serielle Ausgabe
+void SerielleAusgabe()                                                     
+{
+  char report[200]; 
+  snprintf_P(report, sizeof(report),
+    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
+    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
+    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, compassRate);
+  Serial1.println(report);
+}
+
 //Update Abstandssensoren
 void ProxUpdate()
 {
@@ -276,10 +302,6 @@ void ProxUpdate()
       proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
       proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());  
       proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds()); 
-      // Serial.println(proxValue[0]);
-      // Serial.println(proxValue[1]);
-      // Serial.println(proxValue[2]);
-      // Serial.println(proxValue[3]);
       done = true;
     }
     else
@@ -316,22 +338,18 @@ void GyroUpdate()
 //Update Beschleunigungssensor
 void CompassUpdate()
 {
-  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;
+  compass.read();                           //Rohwert 16384 entspricht 1g (9,81m/s²) bzw. bei +/-2g Messbereich 0,61mg/LSB
+  int16_t x = compass.a.x - compassOffset;
+  compassRate = x - compassLastUpdate;
+  compassLastUpdate = x;
 }
 
 //Update Encoder
 void EncoderUpdate()
 {
-  encoderCounts[0] = encoders.getCountsLeft();
+  encoderCounts[0] += encoders.getCountsAndResetLeft();
   driveRotation[0] = encoderCounts[0] / 910;                        //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
-  encoderCounts[1] = encoders.getCountsRight();
+  encoderCounts[1] += encoders.getCountsAndResetRight();
   driveRotation[1] = encoderCounts[1] / 910;
 }
 
@@ -348,10 +366,25 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
+    LineUpdate();
     motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
-    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
+    if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
+  }
+  lastUpdate = millis();
+  TimeUpdate();
+  while(eventTime < 1000)
+  {
+    TimeUpdate();
+    LineUpdate();
+    motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
+    if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
   }
   motors.setSpeeds(0, 0);
+  compassLastUpdate = 0;
+  compassRate = 0;
+  CompassUpdate();
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Vermeiden beendet");
   Serial1.print(0); 
 }
@@ -382,14 +415,15 @@ void Hindernisumfahren()
     motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
     if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
   }
+
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[2] < MARKLINE2)          
   {
     LineUpdate();
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
-    //if(lineValue[0] > MARKLINE0) break;
   } 
+
   //rechts drehen
   GyroUpdate();
   while(rotationAngle > 10)                                               
@@ -400,12 +434,13 @@ void Hindernisumfahren()
     else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
   }
+
   //geradeaus fahren
   motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
 
   //Gegenverkehr beachten
   ProxUpdate();
-  //if(proxValue[1] < 5 || proxValue[2] < 5)
+  //if(proxValue[1] == 6 && proxValue[2] == 6)
   //{
     //Schritt 2: Hindernis passieren
     //Serial1.println("Aufholen"); 
@@ -458,14 +493,15 @@ void Hindernisumfahren()
     motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
     if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
   }
+
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[0] < MARKLINE0)          
   {
     LineUpdate();
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
-    //if(lineValue[0] > MARKLINE0) break;
   } 
+
   //links drehen
   GyroUpdate();
   while(rotationAngle < -10)                                               
@@ -476,10 +512,13 @@ void Hindernisumfahren()
     else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
   }
+
   //geradeaus fahren
   //Serial1.println("Umfahren beendet");
-  Serial1.print(0); 
   motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
+  stop = false;
+  stopUpdate = millis();
+  Serial1.print(0); 
 }
 
 //Funktion Abbiegen
@@ -530,8 +569,9 @@ void Abbiegen()
     while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
     {
       LineUpdate();
-      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
     }
+
     //links drehen
     turnAngle = 0;  
     rotationAngle = 0;
@@ -544,6 +584,7 @@ void Abbiegen()
       else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT); 
       else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
     }
+
     //geradeaus fahren
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
   }
@@ -553,26 +594,22 @@ void Abbiegen()
   {
     //Serial1.println("links Abbiegen vom Rundkurs"); 
     //links drehen
+    motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
     turnAngle = 0;  
     rotationAngle = 0;
-    driveRotation[1] = 0;
     GyroUpdate();
     while(rotationAngle < 40)
     {
       GyroUpdate();
-      EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      //if(driveRotation[1] > 1) break;
     }
-    driveRotation[1] = 0;
     GyroUpdate();
     while(rotationAngle < 85)
     {
       GyroUpdate();
-      EncoderUpdate();
       motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
-      //if(driveRotation[1] > 1) break;
     }
+
     //geradeaus fahren
     motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
     lastUpdate = millis();
@@ -581,6 +618,7 @@ void Abbiegen()
     {
       TimeUpdate();
       LineUpdate();
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
       if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
     }
     lastUpdate = millis();
@@ -598,14 +636,13 @@ void Abbiegen()
   {
     //Serial1.println("rechts Abbiegen"); 
     //rechts drehen
+    motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
     turnAngle = 0;  
     rotationAngle = 0;
-    driveRotation[0] = 0;
     GyroUpdate();
     while(rotationAngle > -40)
     {
       GyroUpdate();
-      EncoderUpdate();
       LineUpdate();
       motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
       if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
@@ -639,6 +676,8 @@ void Abbiegen()
       Spurhalten();
     }
   }
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Abbiegen beendet");
   Serial1.print(0); 
 }
@@ -655,7 +694,7 @@ void Spurhalten()
   {
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
+    motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)SLOWSPEEDRIGHT/eventSpeed);
     while(Time < 100)
     {
       Update = millis();
@@ -663,6 +702,8 @@ void Spurhalten()
       LineUpdate();
       if(lineValue[2] > MARKLINE2) break;
     }   
+    stop = false;
+    stopUpdate = millis();
   }
 
   //rechte Linie erreicht, nach links fahren
@@ -670,7 +711,7 @@ void Spurhalten()
   {
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);   
+    motors.setSpeeds((int)SLOWSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);   
     while(Time < 100)
     {
       Update = millis();
@@ -678,13 +719,16 @@ void Spurhalten()
       LineUpdate();
       if(lineValue[0] > MARKLINE0) break;
     }   
+    stop = false;
+    stopUpdate = millis();
   }
 
   //normale Fahrt
   else                                                                
   {
     ledGreen(1); 
-    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
+    motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);
+    stop = true;
   }
 }
 
@@ -702,24 +746,12 @@ void Spurfinden()
     motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
     if(eventTime > 3000) break;
   } 
+  stop = false;
+  stopUpdate = millis();
   //Serial1.println("Spur gefunden"); 
   Serial1.print(0);  
 }  
 
-//Funktion Serielle Ausgabe
-void SerielleAusgabe()                                                     
-{
-  snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
-    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
-    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);
-}
-
 /*-----------------------------------------------------------------*/
 
 void loop() 
@@ -736,27 +768,31 @@ void loop()
   CompassUpdate();
   LineUpdate();      
   TimeUpdate();
+  StopUpdate();
 
   //Funktionsauswahl
-  //btData = Serial1.readString();
   if(Serial1.available() > 0) btData = Serial1.read();
-  if(btData == '1') alarm = true;
-  else if(btData == '0') alarm = false;
+  if(btData == '1') btBuffer = true;
+  else if(btData == '0') btBuffer = false;
+
   //verfügbare Funktionen bei laufenden Manövern
-  //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")   
-  if(alarm == true)                                    
+  if(btBuffer == true)                                    
   {
     //Serial1.println("Verstanden");
     eventSpeed = 1.4;
-    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
+    if(proxValue[0] == 6 || (proxValue[1] == 6 && proxValue[2] == 6)) motors.setSpeeds(0, 0); 
+    else if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) motors.setSpeeds(0, 0); 
+    else if(lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
+
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
     eventSpeed = 1.0;
-    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
-    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) Kontaktvermeiden();           
+    else if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
+    else if(lineValue[0] > 1600 && lineValue[2] > 1600) motors.setSpeeds(0, 0);
     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();