Updates for almost all functions

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 26.03.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.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 
@@ -22,7 +22,7 @@ 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"
+# 38 "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)
 
@@ -46,35 +46,47 @@ int16_t compassOffset;
 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
+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
 
 /*-----------------------------------------------------------------*/
 
+//Setup Bluetoothverbindung
+void BlueSetup()
+{
+  Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
+  Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
+  if(Serial.available()) Serial.println("Bluetoothverbindung hergestellt");
+}
+
 //Setup Liniensensoren
 void LineSetup()
 {
   ledYellow(1);
-  lineSensors.initThreeSensors();
-
-  //Kalibrierung
-  uint32_t total[3] = {0, 0, 0};
-  for(uint8_t i = 0; i < 120; i++)
-  {
-    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];
-    total[2] += lineOffset[2];
+  lineSensors.initThreeSensors(); //Initialisierung von 3 Sensoren (max 5)
+
+  // //Kalibrierung
+  // uint32_t total[3] = {0, 0, 0};  
+  // for(uint8_t i = 0; i < 120; i++)                                
+  // {
+  //   if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+  //   else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
+  //   lineSensors.read(lineOffset);  
+  //   total[0] += lineOffset[0];
+  //   total[1] += lineOffset[1];
+  //   total[2] += lineOffset[2];
     lineSensors.calibrate();
-  }
+  // }
+  // motors.setSpeeds(0, 0);
+  // lineOffset[0] = total[0] / 120;
+  // lineOffset[1] = total[1] / 120;
+  // lineOffset[2] = total[2] / 120;
   ledYellow(0);
-  motors.setSpeeds(0, 0);
-  lineOffset[0] = total[0] / 120;
-  lineOffset[1] = total[1] / 120;
-  lineOffset[2] = total[2] / 120;
+
+  lineOffset[0] = 240;
+  lineOffset[1] = 120;
+  lineOffset[2] = 220;
 }
 
 //Setup Abstandssensoren
@@ -87,15 +99,15 @@ void ProxSetup()
 void GyroSetup()
 {
   ledYellow(1);
-  gyro.init();
-  if (!gyro.init())
+  gyro.init(); //Initialisierung
+  if(!gyro.init()) //Fehlerabfrage
   {
     //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);
+      //Serial1.println("Fehler Drehbewegungssensors");
+      delay(1000);
     }
   }
   gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
@@ -104,7 +116,7 @@ void GyroSetup()
 
   //Kalibrierung
   int32_t total = 0;
-  for (uint16_t i = 0; i < 1024; i++)
+  for(uint16_t i = 0; i < 1024; i++)
   {
     while(!gyro.readReg(L3G::STATUS_REG) & 0x08); //Fehlerabfrage
     gyro.read();
@@ -120,15 +132,15 @@ void GyroSetup()
 void CompassSetup()
 {
   ledYellow(1);
-  compass.init();
-  if (!compass.init())
+  compass.init(); //Initialisierung
+  if(!compass.init()) //Fehlerabfrage
   {
     //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);
+      Serial1.println("Fehler Beschleunigungssensors");
+      delay(1000);
     }
   }
   compass.enableDefault();
@@ -151,12 +163,10 @@ void CompassSetup()
 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");
+  BlueSetup();
 
   //Initialisierung und Kalibrierung der Sensoren
-  Serial1.println("Sensorkalibrierung");
+  //Serial1.println("Sensorkalibrierung");   
   Wire.begin();
   delay(500);
   ProxSetup();
@@ -165,13 +175,12 @@ void setup()
   CompassSetup();
 
   //Zumo bereit zu starten
-  Serial1.println("Zumo bereit, starte mit A");
+  //Serial1.println("Zumo bereit, starte mit A");
   ledGreen(1);
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
-  motors.setSpeeds(100, 100);
-  lastUpdate = millis();
+  //Serial1.println("Start");
 }
 
 /*-----------------------------------------------------------------*/
@@ -264,12 +273,12 @@ 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;
+  // 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
@@ -286,8 +295,8 @@ void EncoderUpdate()
 //Funktion Kontaktvermeiden
 void Kontaktvermeiden()
 {
-  dir = 'X';
-  Serial1.println("Kontaktvermeiden");
+  //Serial1.println("Kontaktvermeiden");
+  Serial1.println(1);
   ledRed(1);
 
   motors.setSpeeds(0, 0);
@@ -295,140 +304,229 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
-    motors.setSpeeds(-50, -50);
-    if(lineValue[0] > 200 || lineValue[2] > 200) return;
+    motors.setSpeeds(-54, -50);
+    if(lineValue[0] > 150 || lineValue[2] > 170) break;
   }
   delay(500);
-
-  Serial1.println("Vermeiden beendet");
-  motors.setSpeeds(100, 100);
-  lastUpdate = millis();
+  //Serial1.println("Vermeiden beendet");
+  Serial1.println(0);
 }
 
 //Funktion Hindernisumfahrung
 void Hindernisumfahren()
 {
-  dir = 'U';
-  Serial1.println("Hindernisumfahren");
+  //Serial1.println("Hindernisumfahren"); 
+  Serial1.println(1);
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
-  //links drehen                        
+  //links drehen    
+  turnAngle = 0;
   rotationAngle = 0;
   GyroUpdate();
   while(rotationAngle < 45)
   {
     GyroUpdate();
-    motors.setSpeeds(50, 100);
+    LineUpdate();
+    motors.setSpeeds(27, 100);
+    if(lineValue[2] > 170 && lineValue[2] < 450) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[2] < 200)
+  while(lineValue[2] < 170)
   {
     LineUpdate();
-    motors.setSpeeds(100, 100);
+    motors.setSpeeds(107, 100);
+    //if(lineValue[0] > MARKLINE0) break;
   }
   //rechts drehen
   GyroUpdate();
-  while(rotationAngle > 0)
+  while(rotationAngle > 10)
   {
     GyroUpdate();
-    motors.setSpeeds(100, 50);
+    LineUpdate();
+    if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
+    else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 100);
+    else motors.setSpeeds(107, 25);
   }
   //geradeaus fahren
-  motors.setSpeeds(100, 100);
+  motors.setSpeeds(107, 100);
 
   //Gegenverkehr beachten
   ProxUpdate();
-  if(proxValue[1] < 5 || proxValue[2] < 5)
-  {
+  //if(proxValue[1] < 5 || proxValue[2] < 5)
+  //{
     //Schritt 2: Hindernis passieren
-    motors.setSpeeds(100, 100);
-    delay(1000);
+    //Serial1.println("Aufholen"); 
+    lastUpdate = millis();
+    while(proxValue[3] < 6 && eventTime < 3000)
+    {
+      TimeUpdate();
+      ProxUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+    //Serial1.println("Überholen"); 
     ProxUpdate();
-    while(proxValue[3] > 4)
+    while(proxValue[3] == 6)
     {
-      motors.setSpeeds(100, 100);
       ProxUpdate();
+      LineUpdate();
       Spurhalten();
     }
-  }
+    //Serial1.println("Abstand gewinnen"); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 3000)
+    {
+      //Serial1.println(eventTime); 
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+  //}
 
   //Schritt 3: Spurwechsel rechts     
-  //rechts drehen                     
+  //rechts drehen     
+  turnAngle = 0;
   rotationAngle = 0;
   GyroUpdate();
   while(rotationAngle > -45)
   {
     GyroUpdate();
-    motors.setSpeeds(100, 50);
+    LineUpdate();
+    motors.setSpeeds(107, 25);
+    if(lineValue[0] > 150 && lineValue[0] < 400) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
-  while(lineValue[0] < 200)
+  while(lineValue[0] < 150)
   {
     LineUpdate();
-    motors.setSpeeds(100, 100);
+    motors.setSpeeds(107, 100);
+    //if(lineValue[0] > MARKLINE0) break;
   }
   //links drehen
   GyroUpdate();
-  while(rotationAngle < 0)
+  while(rotationAngle < -10)
   {
     GyroUpdate();
-    motors.setSpeeds(50, 100);
+    LineUpdate();
+    if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
+    else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
+    else motors.setSpeeds(27, 100);
   }
   //geradeaus fahren
-  Serial1.println("Umfahren beendet");
-  motors.setSpeeds(100, 100);
-  lastUpdate = millis();
+  //Serial1.println("Umfahren beendet");
+  Serial1.println(0);
+  motors.setSpeeds(107, 100);
 }
 
 //Funktion Abbiegen
 void Abbiegen()
 {
-  dir = 'A';
   ledYellow(1);
-  Serial1.println("Abbiegen");
+  //Serial1.println("Abbiegen");  
+  Serial1.println(1);
 
   //Markierung analysieren
+  LineUpdate();
   bool leftCode; //links => 1
   bool rightCode; //rechts => 2
-  if(lineValue[0] > 1000) leftCode = true;
+  if(lineValue[0] > 400) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > 1000) rightCode = true;
+  if(lineValue[2] > 450) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   uint8_t randy; //geradeaus => 0
   if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
-  else if(leftCode == true && rightCode == false) randy = random(0, 2); //0, 1
-  else if(leftCode == false && rightCode == true)
+  else if(leftCode == true && rightCode == false) randy = 1;
+  // else if(leftCode == true && rightCode == false) 
+  // {
+  //   randy = random(0, 2);                                                 //0, 1
+  //   if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+  // }
+  else if(leftCode == false && rightCode == true) randy = 2;
+  // else if(leftCode == false && rightCode == true)
+  // {
+  //   randy = random(3);                                                    //0, (1), 2
+  //   if(randy == 0) randy = random(3);                                     //erhöht Wahrscheinlickeit abzubiegen
+  //   while(randy == 1) randy = random(3);                                  //!1 => 0, 2
+  // }
+
+  //links Abbiegen (von der Verbindungsstrecke)
+  if(randy == 1 && rightCode == true)
   {
-    randy = random(3); //0, (1), 2
-    while(randy == 1) randy = random(3); //!1 => 0, 2
+    //Serial1.println("links Abbiegen von der Verbindungsstrecke"); 
+    //geradeaus zur Mittellinie fahren
+    motors.setSpeeds(107, 100 +10);
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 300)
+    {
+      TimeUpdate();
+      //Serial1.println(eventTime); 
+      motors.setSpeeds(107, 100 +10);
+    }
+    LineUpdate();
+    //Serial1.println("Linie suchen"); 
+    while(lineValue[0] < 150 && lineValue[2] < 170)
+    {
+      LineUpdate();
+      motors.setSpeeds(107, 100);
+    }
+    //links drehen
+    turnAngle = 0;
+    rotationAngle = 0;
+    GyroUpdate();
+    //Serial1.println("Drehen"); 
+    while(rotationAngle < 90)
+    {
+      GyroUpdate();
+      LineUpdate();
+      if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
+      else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
+      else motors.setSpeeds(27, 100);
+    }
+    //geradeaus fahren
+    motors.setSpeeds(107, 100);
   }
 
-  //links Abbiegen
-  if(randy == 1 && leftCode == true)
+  //links Abbiegen (vom Rundkurs)
+  else if(randy == 1 && leftCode == true)
   {
-    //zur Kreuzungsmitte fahren
-    driveRotation[0] = 0;
+    //Serial1.println("links Abbiegen vom Rundkurs"); 
+    //links drehen
+    turnAngle = 0;
+    rotationAngle = 0;
     driveRotation[1] = 0;
-    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 200 && lineValue[2] < 200)
+    GyroUpdate();
+    while(rotationAngle < 40)
     {
+      GyroUpdate();
       EncoderUpdate();
-      motors.setSpeeds(50, 50);
+      motors.setSpeeds(27, 100);
+      if(driveRotation[1] > 1) break;
     }
-    //links drehen
-    rotationAngle = 0;
+    driveRotation[1] = 0;
     GyroUpdate();
-    while(rotationAngle != 90)
+    while(rotationAngle < 85)
     {
       GyroUpdate();
-      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);
+      EncoderUpdate();
+      motors.setSpeeds(27, 100);
+      if(driveRotation[1] > 1) break;
+    }
+    //geradeaus fahren
+    motors.setSpeeds(107, 100);
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      if(lineValue[2] > 170 && lineValue[2] < 450) break;
     }
   }
 
@@ -436,74 +534,95 @@ void Abbiegen()
   else if(randy == 2 && rightCode == true)
   {
     //rechts drehen
+    turnAngle = 0;
     rotationAngle = 0;
+    driveRotation[0] = 0;
+    GyroUpdate();
+    while(rotationAngle > -40)
+    {
+      GyroUpdate();
+      EncoderUpdate();
+      LineUpdate();
+      motors.setSpeeds(107, 25);
+      if(driveRotation[0] > 1 || (lineValue[0] > 150 && lineValue[0] < 400)) break;
+    }
     GyroUpdate();
-    while(rotationAngle != -90)
+    lastUpdate = millis();
+    while(rotationAngle > -85)
     {
+      TimeUpdate();
       GyroUpdate();
-      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);
+      LineUpdate();
+      if(eventTime > 3000) break;
+      if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
+      //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
+      else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 50);
+      else motors.setSpeeds(107, 25);
     }
   }
 
   //nicht Abbiegen, geradeaus fahren
-  else motors.setSpeeds(50, 50);
-  delay(500);
-
-  //geradeaus fahren
-  Serial1.println("Abbiegen beendet");
-  motors.setSpeeds(100, 100);
-  lastUpdate = millis();
+  else
+  {
+    motors.setSpeeds(107, 100);
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+  }
+  //Serial1.println("Abbiegen beendet");
+  Serial1.println(0);
 }
 
 //Funktion Spurhalten
 void Spurhalten()
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > 200 && lineValue[0] < 1000)
+  if(lineValue[0] > 150 && lineValue[0] < 400)
   {
-    dir = 'R';
     ledYellow(1);
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(100/eventSpeed, 25/eventSpeed);
+    motors.setSpeeds(107/eventSpeed, 25/eventSpeed);
     delay(100);
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > 200 && lineValue[2] < 1000)
+  else if(lineValue[2] > 170 && lineValue[2] < 450)
   {
-    dir = 'L';
     ledYellow(1);
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(25/eventSpeed, 100/eventSpeed);
+    motors.setSpeeds(27/eventSpeed, 100/eventSpeed);
     delay(100);
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > 100 && lineValue[1] < 1000)
+  else if(lineValue[1] > 100 && lineValue[1] < 350)
   {
-
-    dir = 'B';
     ledRed(1);
-    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
+    //Serial1.println("Spurfinden"); 
+    Serial1.println(1);
+    lastUpdate = millis();
+    while(lineValue[0] < 150 && lineValue[2] < 170) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
     {
-      motors.setSpeeds(-50/eventSpeed, -50/eventSpeed);
-      uint16_t backTime = millis();
-      if((backTime - startTime) > 3000) return;
+      TimeUpdate();
+      LineUpdate();
+      motors.setSpeeds(-107/eventSpeed, -100/eventSpeed);
+      if(eventTime > 3000) break;
     }
     delay(100);
-    Serial1.println("Spur gefunden");
+    //Serial1.println("Spur gefunden"); 
+    Serial1.println(0);
   }
 
   //normale Fahrt
   else
   {
-    dir = 'F';
     ledGreen(1);
+    motors.setSpeeds(107/eventSpeed, 100/eventSpeed);
   }
 }
 
@@ -525,8 +644,7 @@ void SerielleAusgabe()
 
 void loop()
 {
-  LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
-
+  //LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
@@ -540,27 +658,27 @@ void loop()
   TimeUpdate();
 
   //Funktionsauswahl
-  btData = Serial1.readString();
+  //btData = Serial1.readString();
+  if(Serial1.available() > 0) btData = Serial1.read();
+  if(btData == '1') alarm = true;
+  else if(btData == '0') alarm = false;
   //verfügbare Funktionen bei laufenden Manövern
-  if(btData == "Abbiegen" || btData == "Hindernisumfahren"
-  || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
+  //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")   
+  if(alarm == true)
   {
     //Serial1.println("Verstanden");
     eventSpeed = 2;
-    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 ||
-    lineValue[0] > 1000 || lineValue[2] > 1000) motors.setSpeeds(0, 0);
+    if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > 400 || lineValue[2] > 450) motors.setSpeeds(0, 0);
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else
   {
     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();
+    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
+    if(proxValue[2] == 6) Hindernisumfahren();
+    else if(lineValue[0] > 400 && lineValue[0] < 1000 || lineValue[2] > 450 && lineValue[2] < 1000) Abbiegen();
     else Spurhalten();
   }
-
-  LoopTiming();
+  //LoopTiming();                                                       
 }

ArduinoOutput/sketch/Hauptprojekt.ino.cpp

@@ -2,7 +2,7 @@
 #line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 #line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.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 
@@ -24,14 +24,19 @@ Zumo32U4Encoders          encoders;     //Motorencoder
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
-#define   MARKLINE0       200
+#define   MARKLINE0       150
 #define   MARKLINE1       100
-#define   MARKLINE2       200
-#define   SIGN            1000
+#define   MARKLINE2       170
+#define   SIGN0           400
+#define   SIGN1           350
+#define   SIGN2           450
 #define   MAXSPEED        400
-#define   FULLSPEED       100
-#define   HALFSPEED       50
-#define   SLOWSPEED       25
+#define   FULLSPEEDLEFT   107
+#define   HALFSPEEDLEFT   54
+#define   SLOWSPEEDLEFT   27
+#define   FULLSPEEDRIGHT  100
+#define   HALFSPEEDRIGHT  50
+#define   SLOWSPEEDRIGHT  25
 
 int16_t   lineValue[3];                 //Liniensensoren
 uint16_t  lineOffset[3];                //von links (0) nach rechts (2)
@@ -56,72 +61,86 @@ int16_t   compassOffset;
 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
+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
 
 /*-----------------------------------------------------------------*/
 
-//Setup Liniensensoren
-#line 63 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+//Setup Bluetoothverbindung
+#line 68 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void BlueSetup();
+#line 76 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineSetup();
-#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 105 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxSetup();
-#line 94 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 111 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroSetup();
-#line 127 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 144 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassSetup();
-#line 158 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 175 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void setup();
-#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 201 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void TimeUpdate();
-#line 193 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 207 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LoopTiming();
-#line 237 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 251 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void ProxUpdate();
-#line 247 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 261 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void LineUpdate();
-#line 257 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 271 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void GyroUpdate();
-#line 270 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 284 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void CompassUpdate();
-#line 283 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 297 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void EncoderUpdate();
-#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 308 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Kontaktvermeiden();
-#line 316 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 328 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Hindernisumfahren();
-#line 394 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 438 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Abbiegen();
-#line 469 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 594 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void Spurhalten();
-#line 518 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 642 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void SerielleAusgabe();
-#line 533 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 657 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
 void loop();
-#line 63 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+#line 68 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
+void BlueSetup()
+{
+  Serial1.begin(9600);                      //Initialisierung mit Datengeschwindigkeit(Baud)
+  Serial1.setTimeout(10);                   //verkürzt Serial(1).read auf 10 ms statt 1000 ms
+  if(Serial.available()) Serial.println("Bluetoothverbindung hergestellt");     
+}           
+
+//Setup Liniensensoren
 void LineSetup()                                            
 {
   ledYellow(1);
-  lineSensors.initThreeSensors();     
+  lineSensors.initThreeSensors();           //Initialisierung von 3 Sensoren (max 5)
   
-  //Kalibrierung
-  uint32_t total[3] = {0, 0, 0};  
-  for(uint8_t i = 0; i < 120; i++)                                
-  {
-    if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEED, -FULLSPEED);
-    else motors.setSpeeds(-FULLSPEED, FULLSPEED);
-    lineSensors.read(lineOffset);  
-    total[0] += lineOffset[0];
-    total[1] += lineOffset[1];
-    total[2] += lineOffset[2];
+  // //Kalibrierung
+  // uint32_t total[3] = {0, 0, 0};  
+  // for(uint8_t i = 0; i < 120; i++)                                
+  // {
+  //   if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+  //   else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
+  //   lineSensors.read(lineOffset);  
+  //   total[0] += lineOffset[0];
+  //   total[1] += lineOffset[1];
+  //   total[2] += lineOffset[2];
     lineSensors.calibrate();                                      
-  }
+  // }
+  // motors.setSpeeds(0, 0);
+  // lineOffset[0] = total[0] / 120;
+  // lineOffset[1] = total[1] / 120;
+  // lineOffset[2] = total[2] / 120;
   ledYellow(0);
-  motors.setSpeeds(0, 0);
-  lineOffset[0] = total[0] / 120;
-  lineOffset[1] = total[1] / 120;
-  lineOffset[2] = total[2] / 120;
+
+  lineOffset[0] = 240;
+  lineOffset[1] = 120;
+  lineOffset[2] = 220;
 }
 
 //Setup Abstandssensoren
@@ -134,15 +153,15 @@ void ProxSetup()
 void GyroSetup()                                            
 {                                                                 
   ledYellow(1);
-  gyro.init();
-  if (!gyro.init())
+  gyro.init();                                                    //Initialisierung
+  if(!gyro.init())                                                //Fehlerabfrage
   {
     //Fehler beim Initialisieren des Drehbewegungssensors
     ledRed(1);
     while(1)
     {
-      Serial1.println(F("Fehler Drehbewegungssensors"));
-      delay(100);
+      //Serial1.println("Fehler Drehbewegungssensors");
+      delay(1000);
     }
   }
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
@@ -151,7 +170,7 @@ void GyroSetup()
 
   //Kalibrierung
   int32_t total = 0;                                              
-  for (uint16_t i = 0; i < 1024; i++)
+  for(uint16_t i = 0; i < 1024; i++)
   {
     while(!gyro.readReg(L3G::STATUS_REG) & 0x08);                 //Fehlerabfrage
     gyro.read();
@@ -167,15 +186,15 @@ void GyroSetup()
 void CompassSetup()                                            
 {
   ledYellow(1);
-  compass.init();
-  if (!compass.init())
+  compass.init();                                                 //Initialisierung
+  if(!compass.init())                                             //Fehlerabfrage
   {
     //Fehler beim Initialisieren des Beschleunigungssensors
     ledRed(1);
     while(1)
     {
-      Serial.println(F("Fehler Beschleunigungssensors"));
-      delay(100);
+      Serial1.println("Fehler Beschleunigungssensors");
+      delay(1000);
     }
   }
   compass.enableDefault();  
@@ -198,12 +217,10 @@ void CompassSetup()
 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");  
+  BlueSetup();
 
   //Initialisierung und Kalibrierung der Sensoren
-  Serial1.println("Sensorkalibrierung");   
+  //Serial1.println("Sensorkalibrierung");   
   Wire.begin();                       
   delay(500);
   ProxSetup();
@@ -212,13 +229,12 @@ void setup()
   CompassSetup();
   
   //Zumo bereit zu starten
-  Serial1.println("Zumo bereit, starte mit A");
+  //Serial1.println("Zumo bereit, starte mit A");
   ledGreen(1);
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  //Serial1.println("Start");
 }
 
 /*-----------------------------------------------------------------*/
@@ -311,12 +327,12 @@ 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;
+  // 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
@@ -333,8 +349,8 @@ void EncoderUpdate()
 //Funktion Kontaktvermeiden
 void Kontaktvermeiden()                                            
 {
-  dir = 'X';
-  Serial1.println("Kontaktvermeiden");
+  //Serial1.println("Kontaktvermeiden");
+  Serial1.println(1); 
   ledRed(1);
 
   motors.setSpeeds(0, 0);
@@ -342,140 +358,229 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
-    motors.setSpeeds(-HALFSPEED, -HALFSPEED);
-    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) return;
+    motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
+    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
   }
   delay(500); 
-
-  Serial1.println("Vermeiden beendet");
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  //Serial1.println("Vermeiden beendet");
+  Serial1.println(0); 
 }
 
 //Funktion Hindernisumfahrung
 void Hindernisumfahren()                                              
 {
-  dir = 'U';
-  Serial1.println("Hindernisumfahren"); 
+  //Serial1.println("Hindernisumfahren"); 
+  Serial1.println(1); 
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
-  //links drehen                        
+  //links drehen    
+  turnAngle = 0;                    
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
   while(rotationAngle < 45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(HALFSPEED, FULLSPEED); 
+    LineUpdate();
+    motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+    if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[2] < MARKLINE2)          
   {
     LineUpdate();
-    motors.setSpeeds(FULLSPEED, FULLSPEED); 
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    //if(lineValue[0] > MARKLINE0) break;
   } 
   //rechts drehen
   GyroUpdate();
-  while(rotationAngle > 0)                                               
+  while(rotationAngle > 10)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(FULLSPEED, HALFSPEED); 
+    LineUpdate();
+    if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0); 
+    else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
   }
   //geradeaus fahren
-  motors.setSpeeds(FULLSPEED, FULLSPEED); 
+  motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
 
   //Gegenverkehr beachten
   ProxUpdate();
-  if(proxValue[1] < 5 || proxValue[2] < 5)
-  {
+  //if(proxValue[1] < 5 || proxValue[2] < 5)
+  //{
     //Schritt 2: Hindernis passieren
-    motors.setSpeeds(FULLSPEED, FULLSPEED);                    
-    delay(1000);
+    //Serial1.println("Aufholen"); 
+    lastUpdate = millis();
+    while(proxValue[3] < 6 && eventTime < 3000)
+    {
+      TimeUpdate();
+      ProxUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+    //Serial1.println("Überholen"); 
     ProxUpdate(); 
-    while(proxValue[3] > 4)
+    while(proxValue[3] == 6)
     {
-      motors.setSpeeds(FULLSPEED, FULLSPEED);  
       ProxUpdate();
+      LineUpdate();
       Spurhalten();
     }
-  }
+    //Serial1.println("Abstand gewinnen"); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 3000)
+    {
+      //Serial1.println(eventTime); 
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+  //}
   
   //Schritt 3: Spurwechsel rechts     
-  //rechts drehen                     
+  //rechts drehen     
+  turnAngle = 0;                
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
   while(rotationAngle > -45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(FULLSPEED, HALFSPEED); 
+    LineUpdate();
+    motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
+    if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[0] < MARKLINE0)          
   {
     LineUpdate();
-    motors.setSpeeds(FULLSPEED, FULLSPEED); 
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    //if(lineValue[0] > MARKLINE0) break;
   } 
   //links drehen
   GyroUpdate();
-  while(rotationAngle < 0)                                               
+  while(rotationAngle < -10)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(HALFSPEED, FULLSPEED);
+    LineUpdate();
+    if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT); 
+    else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
   }
   //geradeaus fahren
-  Serial1.println("Umfahren beendet");
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  //Serial1.println("Umfahren beendet");
+  Serial1.println(0); 
+  motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
 }
 
 //Funktion Abbiegen
 void Abbiegen()
 {
-  dir = 'A';
   ledYellow(1); 
-  Serial1.println("Abbiegen");  
+  //Serial1.println("Abbiegen");  
+  Serial1.println(1); 
 
   //Markierung analysieren
+  LineUpdate(); 
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if(lineValue[0] > SIGN) leftCode = true;
+  if(lineValue[0] > SIGN0) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > SIGN) rightCode = true;
+  if(lineValue[2] > SIGN2) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   uint8_t randy;                                                          //geradeaus => 0
   if(leftCode == true && rightCode == true) randy = random(1, 3);         //1, 2
-  else if(leftCode == true && rightCode == false) randy = random(0, 2);   //0, 1
-  else if(leftCode == false && rightCode == true)
+  else if(leftCode == true && rightCode == false) randy = 1;
+  // else if(leftCode == true && rightCode == false) 
+  // {
+  //   randy = random(0, 2);                                                 //0, 1
+  //   if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+  // }
+  else if(leftCode == false && rightCode == true) randy = 2;
+  // else if(leftCode == false && rightCode == true)
+  // {
+  //   randy = random(3);                                                    //0, (1), 2
+  //   if(randy == 0) randy = random(3);                                     //erhöht Wahrscheinlickeit abzubiegen
+  //   while(randy == 1) randy = random(3);                                  //!1 => 0, 2
+  // }
+
+  //links Abbiegen (von der Verbindungsstrecke)
+  if(randy == 1 && rightCode == true) 
   {
-    randy = random(3);                                                    //0, (1), 2
-    while(randy == 1) randy = random(3);                                  //!1 => 0, 2
+    //Serial1.println("links Abbiegen von der Verbindungsstrecke"); 
+    //geradeaus zur Mittellinie fahren
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 300)
+    {
+      TimeUpdate();
+      //Serial1.println(eventTime); 
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
+    }
+    LineUpdate();
+    //Serial1.println("Linie suchen"); 
+    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
+    {
+      LineUpdate();
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    }
+    //links drehen
+    turnAngle = 0;  
+    rotationAngle = 0;
+    GyroUpdate();
+    //Serial1.println("Drehen"); 
+    while(rotationAngle < 90)
+    {
+      GyroUpdate();
+      LineUpdate();
+      if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT); 
+      else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+      else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
+    }
+    //geradeaus fahren
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
   }
 
-  //links Abbiegen
-  if(randy == 1 && leftCode == true) 
+  //links Abbiegen (vom Rundkurs)
+  else if(randy == 1 && leftCode == true) 
   {
-    //zur Kreuzungsmitte fahren
-    driveRotation[0] = 0;
+    //Serial1.println("links Abbiegen vom Rundkurs"); 
+    //links drehen
+    turnAngle = 0;  
+    rotationAngle = 0;
     driveRotation[1] = 0;
-    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
+    GyroUpdate();
+    while(rotationAngle < 40)
     {
+      GyroUpdate();
       EncoderUpdate();
-      motors.setSpeeds(HALFSPEED, HALFSPEED);
+      motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+      if(driveRotation[1] > 1) break;
     }
-    //links drehen
-    rotationAngle = 0;
+    driveRotation[1] = 0;
     GyroUpdate();
-    while(rotationAngle != 90)
+    while(rotationAngle < 85)
     {
       GyroUpdate();
-      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);
+      EncoderUpdate();
+      motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+      if(driveRotation[1] > 1) break;
+    }
+    //geradeaus fahren
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
     }
   }
 
@@ -483,74 +588,95 @@ void Abbiegen()
   else if(randy == 2 && rightCode == true)
   {
     //rechts drehen
+    turnAngle = 0;  
     rotationAngle = 0;
+    driveRotation[0] = 0;
     GyroUpdate();
-    while(rotationAngle != -90)
+    while(rotationAngle > -40)
     {
       GyroUpdate();
-      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);
+      EncoderUpdate();
+      LineUpdate();
+      motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
+      if(driveRotation[0] > 1 || (lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)) break;
+    }
+    GyroUpdate();
+    lastUpdate = millis();
+    while(rotationAngle > -85)
+    {
+      TimeUpdate();
+      GyroUpdate();
+      LineUpdate();
+      if(eventTime > 3000) break;
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0);
+      //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
+      else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, HALFSPEEDRIGHT);
+      else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
     }
   }  
 
   //nicht Abbiegen, geradeaus fahren
-  else motors.setSpeeds(HALFSPEED, HALFSPEED); 
-  delay(500);
-
-  //geradeaus fahren
-  Serial1.println("Abbiegen beendet");
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  else 
+  {
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+  }
+  //Serial1.println("Abbiegen beendet");
+  Serial1.println(0); 
 }
 
 //Funktion Spurhalten
 void Spurhalten()                                                   
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN)                      
+  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)                      
   {
-    dir = 'R';
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(FULLSPEED/eventSpeed, SLOWSPEED/eventSpeed);
+    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
     delay(100);    
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN)                 
+  else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2)                 
   {
-    dir = 'L'; 
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(SLOWSPEED/eventSpeed, FULLSPEED/eventSpeed);  
+    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);  
     delay(100);  
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN)                 
+  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1)                 
   { 
-    
-    dir = 'B';  
     ledRed(1); 
-    Serial1.println("Spurfinden");  
-    uint16_t startTime = millis();
+    //Serial1.println("Spurfinden"); 
+    Serial1.println(1); 
+    lastUpdate = millis();
     while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
     {
-      motors.setSpeeds(-HALFSPEED/eventSpeed, -HALFSPEED/eventSpeed);
-      uint16_t backTime = millis();
-      if((backTime - startTime) > 3000) return;
+      TimeUpdate();
+      LineUpdate();
+      motors.setSpeeds(-FULLSPEEDLEFT/eventSpeed, -FULLSPEEDRIGHT/eventSpeed);
+      if(eventTime > 3000) break;
     }
     delay(100);    
-    Serial1.println("Spur gefunden");  
+    //Serial1.println("Spur gefunden"); 
+    Serial1.println(0);  
   }  
 
   //normale Fahrt
   else                                                                
   {
-    dir = 'F'; 
     ledGreen(1); 
+    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
   }
 }
 
@@ -572,8 +698,7 @@ void SerielleAusgabe()
 
 void loop() 
 {
-  LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
-
+  //LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
@@ -587,28 +712,28 @@ void loop()
   TimeUpdate();
 
   //Funktionsauswahl
-  btData = Serial1.readString();
+  //btData = Serial1.readString();
+  if(Serial1.available() > 0) btData = Serial1.read();
+  if(btData == '1') alarm = true;
+  else if(btData == '0') alarm = false;
   //verfügbare Funktionen bei laufenden Manövern
-  if(btData == "Abbiegen" || btData == "Hindernisumfahren" 
-  || btData == "Kontaktvermeiden"|| btData == "Spurfinden")                                                  
+  //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")   
+  if(alarm == true)                                    
   {
     //Serial1.println("Verstanden");
     eventSpeed = 2;
-    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || 
-    lineValue[0] > SIGN || lineValue[2] > SIGN) motors.setSpeeds(0, 0);  
+    if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > SIGN0 || lineValue[2] > SIGN2) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
     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] > SIGN || lineValue[2] > SIGN) Abbiegen();
+    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
+    if(proxValue[2] == 6) Hindernisumfahren();
+    else if(lineValue[0] > SIGN0 && lineValue[0] < 1000 || lineValue[2] > SIGN2 && lineValue[2] < 1000) Abbiegen();
     else Spurhalten();
   }
-
-  LoopTiming();                                                       
+  //LoopTiming();                                                       
 }
 

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,5 +1,5 @@
 //Verfassser: Felix Stange, 4056379, MET2016 
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.2018 
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.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 
@@ -21,14 +21,19 @@ Zumo32U4Encoders          encoders;     //Motorencoder
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
-#define   MARKLINE0       200
+#define   MARKLINE0       150
 #define   MARKLINE1       100
-#define   MARKLINE2       200
-#define   SIGN            1000
+#define   MARKLINE2       170
+#define   SIGN0           400
+#define   SIGN1           350
+#define   SIGN2           450
 #define   MAXSPEED        400
-#define   FULLSPEED       100
-#define   HALFSPEED       50
-#define   SLOWSPEED       25
+#define   FULLSPEEDLEFT   107
+#define   HALFSPEEDLEFT   54
+#define   SLOWSPEEDLEFT   27
+#define   FULLSPEEDRIGHT  100
+#define   HALFSPEEDRIGHT  50
+#define   SLOWSPEEDRIGHT  25
 
 int16_t   lineValue[3];                 //Liniensensoren
 uint16_t  lineOffset[3];                //von links (0) nach rechts (2)
@@ -53,35 +58,47 @@ int16_t   compassOffset;
 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
+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
 
 /*-----------------------------------------------------------------*/
 
+//Setup Bluetoothverbindung
+void BlueSetup()
+{
+  Serial1.begin(9600);                      //Initialisierung mit Datengeschwindigkeit(Baud)
+  Serial1.setTimeout(10);                   //verkürzt Serial(1).read auf 10 ms statt 1000 ms
+  if(Serial.available()) Serial.println("Bluetoothverbindung hergestellt");     
+}           
+
 //Setup Liniensensoren
 void LineSetup()                                            
 {
   ledYellow(1);
-  lineSensors.initThreeSensors();     
+  lineSensors.initThreeSensors();           //Initialisierung von 3 Sensoren (max 5)
   
-  //Kalibrierung
-  uint32_t total[3] = {0, 0, 0};  
-  for(uint8_t i = 0; i < 120; i++)                                
-  {
-    if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEED, -FULLSPEED);
-    else motors.setSpeeds(-FULLSPEED, FULLSPEED);
-    lineSensors.read(lineOffset);  
-    total[0] += lineOffset[0];
-    total[1] += lineOffset[1];
-    total[2] += lineOffset[2];
+  // //Kalibrierung
+  // uint32_t total[3] = {0, 0, 0};  
+  // for(uint8_t i = 0; i < 120; i++)                                
+  // {
+  //   if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
+  //   else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
+  //   lineSensors.read(lineOffset);  
+  //   total[0] += lineOffset[0];
+  //   total[1] += lineOffset[1];
+  //   total[2] += lineOffset[2];
     lineSensors.calibrate();                                      
-  }
+  // }
+  // motors.setSpeeds(0, 0);
+  // lineOffset[0] = total[0] / 120;
+  // lineOffset[1] = total[1] / 120;
+  // lineOffset[2] = total[2] / 120;
   ledYellow(0);
-  motors.setSpeeds(0, 0);
-  lineOffset[0] = total[0] / 120;
-  lineOffset[1] = total[1] / 120;
-  lineOffset[2] = total[2] / 120;
+
+  lineOffset[0] = 240;
+  lineOffset[1] = 120;
+  lineOffset[2] = 220;
 }
 
 //Setup Abstandssensoren
@@ -94,15 +111,15 @@ void ProxSetup()
 void GyroSetup()                                            
 {                                                                 
   ledYellow(1);
-  gyro.init();
-  if (!gyro.init())
+  gyro.init();                                                    //Initialisierung
+  if(!gyro.init())                                                //Fehlerabfrage
   {
     //Fehler beim Initialisieren des Drehbewegungssensors
     ledRed(1);
     while(1)
     {
-      Serial1.println(F("Fehler Drehbewegungssensors"));
-      delay(100);
+      //Serial1.println("Fehler Drehbewegungssensors");
+      delay(1000);
     }
   }
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
@@ -111,7 +128,7 @@ void GyroSetup()
 
   //Kalibrierung
   int32_t total = 0;                                              
-  for (uint16_t i = 0; i < 1024; i++)
+  for(uint16_t i = 0; i < 1024; i++)
   {
     while(!gyro.readReg(L3G::STATUS_REG) & 0x08);                 //Fehlerabfrage
     gyro.read();
@@ -127,15 +144,15 @@ void GyroSetup()
 void CompassSetup()                                            
 {
   ledYellow(1);
-  compass.init();
-  if (!compass.init())
+  compass.init();                                                 //Initialisierung
+  if(!compass.init())                                             //Fehlerabfrage
   {
     //Fehler beim Initialisieren des Beschleunigungssensors
     ledRed(1);
     while(1)
     {
-      Serial.println(F("Fehler Beschleunigungssensors"));
-      delay(100);
+      Serial1.println("Fehler Beschleunigungssensors");
+      delay(1000);
     }
   }
   compass.enableDefault();  
@@ -158,12 +175,10 @@ void CompassSetup()
 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");  
+  BlueSetup();
 
   //Initialisierung und Kalibrierung der Sensoren
-  Serial1.println("Sensorkalibrierung");   
+  //Serial1.println("Sensorkalibrierung");   
   Wire.begin();                       
   delay(500);
   ProxSetup();
@@ -172,13 +187,12 @@ void setup()
   CompassSetup();
   
   //Zumo bereit zu starten
-  Serial1.println("Zumo bereit, starte mit A");
+  //Serial1.println("Zumo bereit, starte mit A");
   ledGreen(1);
   buttonA.waitForButton();
   randomSeed(millis());
   delay(500);
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  //Serial1.println("Start");
 }
 
 /*-----------------------------------------------------------------*/
@@ -271,12 +285,12 @@ 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;
+  // 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
@@ -293,8 +307,8 @@ void EncoderUpdate()
 //Funktion Kontaktvermeiden
 void Kontaktvermeiden()                                            
 {
-  dir = 'X';
-  Serial1.println("Kontaktvermeiden");
+  //Serial1.println("Kontaktvermeiden");
+  Serial1.println(1); 
   ledRed(1);
 
   motors.setSpeeds(0, 0);
@@ -302,140 +316,229 @@ void Kontaktvermeiden()
   while(proxValue[1] == 0 || proxValue[2] == 0)
   {
     ProxUpdate();
-    motors.setSpeeds(-HALFSPEED, -HALFSPEED);
-    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) return;
+    motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
+    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
   }
   delay(500); 
-
-  Serial1.println("Vermeiden beendet");
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  //Serial1.println("Vermeiden beendet");
+  Serial1.println(0); 
 }
 
 //Funktion Hindernisumfahrung
 void Hindernisumfahren()                                              
 {
-  dir = 'U';
-  Serial1.println("Hindernisumfahren"); 
+  //Serial1.println("Hindernisumfahren"); 
+  Serial1.println(1); 
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
-  //links drehen                        
+  //links drehen    
+  turnAngle = 0;                    
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
   while(rotationAngle < 45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(HALFSPEED, FULLSPEED); 
+    LineUpdate();
+    motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+    if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[2] < MARKLINE2)          
   {
     LineUpdate();
-    motors.setSpeeds(FULLSPEED, FULLSPEED); 
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    //if(lineValue[0] > MARKLINE0) break;
   } 
   //rechts drehen
   GyroUpdate();
-  while(rotationAngle > 0)                                               
+  while(rotationAngle > 10)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(FULLSPEED, HALFSPEED); 
+    LineUpdate();
+    if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0); 
+    else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
   }
   //geradeaus fahren
-  motors.setSpeeds(FULLSPEED, FULLSPEED); 
+  motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
 
   //Gegenverkehr beachten
   ProxUpdate();
-  if(proxValue[1] < 5 || proxValue[2] < 5)
-  {
+  //if(proxValue[1] < 5 || proxValue[2] < 5)
+  //{
     //Schritt 2: Hindernis passieren
-    motors.setSpeeds(FULLSPEED, FULLSPEED);                    
-    delay(1000);
+    //Serial1.println("Aufholen"); 
+    lastUpdate = millis();
+    while(proxValue[3] < 6 && eventTime < 3000)
+    {
+      TimeUpdate();
+      ProxUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+    //Serial1.println("Überholen"); 
     ProxUpdate(); 
-    while(proxValue[3] > 4)
+    while(proxValue[3] == 6)
     {
-      motors.setSpeeds(FULLSPEED, FULLSPEED);  
       ProxUpdate();
+      LineUpdate();
       Spurhalten();
     }
-  }
+    //Serial1.println("Abstand gewinnen"); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 3000)
+    {
+      //Serial1.println(eventTime); 
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+  //}
   
   //Schritt 3: Spurwechsel rechts     
-  //rechts drehen                     
+  //rechts drehen     
+  turnAngle = 0;                
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
   while(rotationAngle > -45)          
   {
     GyroUpdate();  
-    motors.setSpeeds(FULLSPEED, HALFSPEED); 
+    LineUpdate();
+    motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
+    if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
   }
   //geradeaus über Mittellinie fahren
   LineUpdate();
   while(lineValue[0] < MARKLINE0)          
   {
     LineUpdate();
-    motors.setSpeeds(FULLSPEED, FULLSPEED); 
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    //if(lineValue[0] > MARKLINE0) break;
   } 
   //links drehen
   GyroUpdate();
-  while(rotationAngle < 0)                                               
+  while(rotationAngle < -10)                                               
   {                                                                   
     GyroUpdate();
-    motors.setSpeeds(HALFSPEED, FULLSPEED);
+    LineUpdate();
+    if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT); 
+    else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
   }
   //geradeaus fahren
-  Serial1.println("Umfahren beendet");
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  //Serial1.println("Umfahren beendet");
+  Serial1.println(0); 
+  motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
 }
 
 //Funktion Abbiegen
 void Abbiegen()
 {
-  dir = 'A';
   ledYellow(1); 
-  Serial1.println("Abbiegen");  
+  //Serial1.println("Abbiegen");  
+  Serial1.println(1); 
 
   //Markierung analysieren
+  LineUpdate(); 
   bool  leftCode;                                                         //links => 1
   bool  rightCode;                                                        //rechts => 2
-  if(lineValue[0] > SIGN) leftCode = true;
+  if(lineValue[0] > SIGN0) leftCode = true;
   else leftCode = false;
-  if(lineValue[2] > SIGN) rightCode = true;
+  if(lineValue[2] > SIGN2) rightCode = true;
   else rightCode = false;
 
   //Zufallszahl erzeugen
   uint8_t randy;                                                          //geradeaus => 0
   if(leftCode == true && rightCode == true) randy = random(1, 3);         //1, 2
-  else if(leftCode == true && rightCode == false) randy = random(0, 2);   //0, 1
-  else if(leftCode == false && rightCode == true)
+  else if(leftCode == true && rightCode == false) randy = 1;
+  // else if(leftCode == true && rightCode == false) 
+  // {
+  //   randy = random(0, 2);                                                 //0, 1
+  //   if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
+  // }
+  else if(leftCode == false && rightCode == true) randy = 2;
+  // else if(leftCode == false && rightCode == true)
+  // {
+  //   randy = random(3);                                                    //0, (1), 2
+  //   if(randy == 0) randy = random(3);                                     //erhöht Wahrscheinlickeit abzubiegen
+  //   while(randy == 1) randy = random(3);                                  //!1 => 0, 2
+  // }
+
+  //links Abbiegen (von der Verbindungsstrecke)
+  if(randy == 1 && rightCode == true) 
   {
-    randy = random(3);                                                    //0, (1), 2
-    while(randy == 1) randy = random(3);                                  //!1 => 0, 2
+    //Serial1.println("links Abbiegen von der Verbindungsstrecke"); 
+    //geradeaus zur Mittellinie fahren
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 300)
+    {
+      TimeUpdate();
+      //Serial1.println(eventTime); 
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
+    }
+    LineUpdate();
+    //Serial1.println("Linie suchen"); 
+    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
+    {
+      LineUpdate();
+      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    }
+    //links drehen
+    turnAngle = 0;  
+    rotationAngle = 0;
+    GyroUpdate();
+    //Serial1.println("Drehen"); 
+    while(rotationAngle < 90)
+    {
+      GyroUpdate();
+      LineUpdate();
+      if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT); 
+      else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+      else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
+    }
+    //geradeaus fahren
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
   }
 
-  //links Abbiegen
-  if(randy == 1 && leftCode == true) 
+  //links Abbiegen (vom Rundkurs)
+  else if(randy == 1 && leftCode == true) 
   {
-    //zur Kreuzungsmitte fahren
-    driveRotation[0] = 0;
+    //Serial1.println("links Abbiegen vom Rundkurs"); 
+    //links drehen
+    turnAngle = 0;  
+    rotationAngle = 0;
     driveRotation[1] = 0;
-    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
+    GyroUpdate();
+    while(rotationAngle < 40)
     {
+      GyroUpdate();
       EncoderUpdate();
-      motors.setSpeeds(HALFSPEED, HALFSPEED);
+      motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+      if(driveRotation[1] > 1) break;
     }
-    //links drehen
-    rotationAngle = 0;
+    driveRotation[1] = 0;
     GyroUpdate();
-    while(rotationAngle != 90)
+    while(rotationAngle < 85)
     {
       GyroUpdate();
-      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);
+      EncoderUpdate();
+      motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
+      if(driveRotation[1] > 1) break;
+    }
+    //geradeaus fahren
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
     }
   }
 
@@ -443,74 +546,95 @@ void Abbiegen()
   else if(randy == 2 && rightCode == true)
   {
     //rechts drehen
+    turnAngle = 0;  
     rotationAngle = 0;
+    driveRotation[0] = 0;
     GyroUpdate();
-    while(rotationAngle != -90)
+    while(rotationAngle > -40)
     {
       GyroUpdate();
-      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);
+      EncoderUpdate();
+      LineUpdate();
+      motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
+      if(driveRotation[0] > 1 || (lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)) break;
+    }
+    GyroUpdate();
+    lastUpdate = millis();
+    while(rotationAngle > -85)
+    {
+      TimeUpdate();
+      GyroUpdate();
+      LineUpdate();
+      if(eventTime > 3000) break;
+      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0);
+      //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
+      else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, HALFSPEEDRIGHT);
+      else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
     }
   }  
 
   //nicht Abbiegen, geradeaus fahren
-  else motors.setSpeeds(HALFSPEED, HALFSPEED); 
-  delay(500);
-
-  //geradeaus fahren
-  Serial1.println("Abbiegen beendet");
-  motors.setSpeeds(FULLSPEED, FULLSPEED);
-  lastUpdate = millis();
+  else 
+  {
+    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
+    lastUpdate = millis();
+    TimeUpdate();
+    while(eventTime < 1000)
+    {
+      TimeUpdate();
+      LineUpdate();
+      Spurhalten();
+    }
+  }
+  //Serial1.println("Abbiegen beendet");
+  Serial1.println(0); 
 }
 
 //Funktion Spurhalten
 void Spurhalten()                                                   
 {
   //linke Linie erreicht, nach rechts fahren
-  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN)                      
+  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)                      
   {
-    dir = 'R';
     ledYellow(1); 
     //Serial1.println("Spur nach rechts korrigieren");  
-    motors.setSpeeds(FULLSPEED/eventSpeed, SLOWSPEED/eventSpeed);
+    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
     delay(100);    
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN)                 
+  else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2)                 
   {
-    dir = 'L'; 
     ledYellow(1); 
     //Serial1.println("Spur nach links korrigieren");  
-    motors.setSpeeds(SLOWSPEED/eventSpeed, FULLSPEED/eventSpeed);  
+    motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);  
     delay(100);  
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN)                 
+  else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1)                 
   { 
-    
-    dir = 'B';  
     ledRed(1); 
-    Serial1.println("Spurfinden");  
-    uint16_t startTime = millis();
+    //Serial1.println("Spurfinden"); 
+    Serial1.println(1); 
+    lastUpdate = millis();
     while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
     {
-      motors.setSpeeds(-HALFSPEED/eventSpeed, -HALFSPEED/eventSpeed);
-      uint16_t backTime = millis();
-      if((backTime - startTime) > 3000) return;
+      TimeUpdate();
+      LineUpdate();
+      motors.setSpeeds(-FULLSPEEDLEFT/eventSpeed, -FULLSPEEDRIGHT/eventSpeed);
+      if(eventTime > 3000) break;
     }
     delay(100);    
-    Serial1.println("Spur gefunden");  
+    //Serial1.println("Spur gefunden"); 
+    Serial1.println(0);  
   }  
 
   //normale Fahrt
   else                                                                
   {
-    dir = 'F'; 
     ledGreen(1); 
+    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
   }
 }
 
@@ -532,8 +656,7 @@ void SerielleAusgabe()
 
 void loop() 
 {
-  LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
-
+  //LoopTiming();                             //Zykluszeit beträgt max. 30ms im Idle
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
@@ -547,27 +670,27 @@ void loop()
   TimeUpdate();
 
   //Funktionsauswahl
-  btData = Serial1.readString();
+  //btData = Serial1.readString();
+  if(Serial1.available() > 0) btData = Serial1.read();
+  if(btData == '1') alarm = true;
+  else if(btData == '0') alarm = false;
   //verfügbare Funktionen bei laufenden Manövern
-  if(btData == "Abbiegen" || btData == "Hindernisumfahren" 
-  || btData == "Kontaktvermeiden"|| btData == "Spurfinden")                                                  
+  //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")   
+  if(alarm == true)                                    
   {
     //Serial1.println("Verstanden");
     eventSpeed = 2;
-    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || 
-    lineValue[0] > SIGN || lineValue[2] > SIGN) motors.setSpeeds(0, 0);  
+    if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > SIGN0 || lineValue[2] > SIGN2) motors.setSpeeds(0, 0);  
     else Spurhalten();
   }
   //verfügbare Funktionen im Normalfall
   else                                                              
   {
     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] > SIGN || lineValue[2] > SIGN) Abbiegen();
+    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
+    if(proxValue[2] == 6) Hindernisumfahren();
+    else if(lineValue[0] > SIGN0 && lineValue[0] < 1000 || lineValue[2] > SIGN2 && lineValue[2] < 1000) Abbiegen();
     else Spurhalten();
   }
-
-  LoopTiming();                                                       
+  //LoopTiming();                                                       
 }