Zumo32U4/ArduinoOutput/preproc/ctags_target_for_gcc_minus_e.cpp

# 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
# 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
//Verfassser: Felix Stange, 4056379, MET2016 
//Datum: erstellt am 19.07.2017, zuletzt geändert am 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). 

Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 

dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 

Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 

genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 

Für die Filterung der Messwerte werden Medianfilter genutzt. Der Zumo kommuniziert über ein 

Bluetooth-Modul (HC-05) mit anderen Geräten. Die Kommunikation erfolgt seriell ('SERIAL' für USB, 

'SERIAL1' für Bluetooth). Das LCD kann bei Bluetoothnutzung nicht verwendet werden.*/
# 14 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
# 15 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
# 16 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
# 17 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2

Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
Zumo32U4LineSensors lineSensors; //Liniensensoren
Zumo32U4Motors motors; //Motoren
Zumo32U4ButtonA buttonA; //Taste A
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); //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); //Fenstergröße: 5, Basiswerte: 0 0 0 0 0
MedianFilter ProxFilter2(5, 0);
MedianFilter ProxFilter3(5, 0);
# 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)

uint8_t proxValue[4]; //Abstandssensoren v.l. (0) n.r. (3)

int16_t encoderCounts[2]; //Anzahl der Umdrehungen
int16_t driveRotation[2]; //von links (0) nach rechts (1)

int32_t rotationAngle = 0; //Drehwinkel
int32_t turnAngle = 0;
int16_t turnRate;
int16_t gyroOffset;
uint16_t gyroLastUpdate;

int16_t compassRate; //Beschleunigung
int16_t compassOffset;
int16_t compassLastUpdate;

uint16_t lastUpdate = 0; //Systemzeit
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 btBuffer = false; //puffert Daten von btData
bool stop = false; //Sperrt Funktion Kontaktvermeiden

/*-----------------------------------------------------------------*/

//Setup Bluetoothverbindung
void BlueSetup()
{
  Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
  Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
  if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");
  if(Serial1.available() > 0) Serial1.read(); //Verwerfen der alten Informationen aus dem Puffer
  Serial1.print(0);
}

//Setup Liniensensoren
void LineSetup()
{
  ledYellow(1);
  lineSensors.initThreeSensors(); //Initialisierung von 3 Sensoren (max 5)

  //Kalibrierung
  uint32_t total[3] = {0, 0, 0};
  for(uint8_t i = 0; i < 120; i++)
  {
    if (i > 30 && i <= 90) motors.setSpeeds(104, -100);
    else motors.setSpeeds(-104, 100);
    lineSensors.read(lineOffset);
    total[0] += lineOffset[0];
    total[1] += lineOffset[1];
    total[2] += lineOffset[2];
   lineSensors.calibrate();
  }
  motors.setSpeeds(0, 0);
  lineOffset[0] = total[0] / 120;
  lineOffset[1] = total[1] / 120;
  lineOffset[2] = total[2] / 120;
  ledYellow(0);

  // lineOffset[0] = 240;
  // lineOffset[1] = 120;
  // lineOffset[2] = 220;
}

//Setup Abstandssensoren
void ProxSetup()
{
  proxSensors.initThreeSensors();
}

//Setup Drehbewegungssensor
void GyroSetup()
{
  ledYellow(1);
  gyro.init(); //Initialisierung
  if(!gyro.init()) //Fehlerabfrage
  {
    //Fehler beim Initialisieren des Drehbewegungssensors
    ledRed(1);
    while(1)
    {
      Serial.println("Fehler Drehbewegungssensors");
      delay(5000);
    }
  }
  gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
  gyro.writeReg(L3G::CTRL4, 0b00100000); //2000dps Auflösung
  gyro.writeReg(L3G::CTRL5, 0b00000000); //Hochpassfilter ausgeschaltet

  //Kalibrierung
  int32_t total = 0;
  for(uint16_t i = 0; i < 1024; i++)
  {
    while(!gyro.readReg(L3G::STATUS_REG) & 0x08); //Fehlerabfrage
    gyro.read();
    total += gyro.g.z;
  }
  gyroOffset = total / 1024;

  gyroLastUpdate = micros();
  ledYellow(0);
}

//Setup Beschleunigungssensor
void CompassSetup()
{
  ledYellow(1);
  compass.init(); //Initialisierung
  if(!compass.init()) //Fehlerabfrage
  {
    //Fehler beim Initialisieren des Beschleunigungssensors
    ledRed(1);
    while(1)
    {
      Serial.println("Fehler Beschleunigungssensors");
      delay(5000);
    }
  }
  compass.enableDefault();

  //Kalibrierung
  int32_t total = 0;
  for (uint16_t i = 0; i < 1024; i++)
  {
    compass.readAcc();
    total += compass.a.x;
  }
  compassOffset = total / 1024;

  //compassLastUpdate = micros();  
  ledYellow(0);
}

/*-----------------------------------------------------------------*/

void setup()
{
  //Initialisierung der Bluetoothverbindung
  BlueSetup();

  //Initialisierung und Kalibrierung der Sensoren
  //Serial1.println("Sensorkalibrierung");   
  Wire.begin();
  delay(500);
  ProxSetup();
  LineSetup();
  GyroSetup();
  CompassSetup();

  //Zumo bereit zu starten
  //Serial1.println("Zumo bereit, starte mit A");
  ledGreen(1);
  buttonA.waitForButton();
  randomSeed(millis());
  delay(500);
  stopUpdate = millis();
  //Serial1.println("Start");
}

/*-----------------------------------------------------------------*/

//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!
  static unsigned long LoopTime[AL];
  static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;

  if (Messung % 2 == 0) // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
    {
     LoopTime[Index] = millis();
     Messung++;
     Index++;
     return; // Funktion sofort beenden, spart etwas Zeit
    }

  if (Messung % 2 == 1) // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
    {
     LoopTime[Index] = millis();
     LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1]; // Loopdauer einen Index niedriger einspeichern wie aktuell
     Messung++;
    }

  if (Index >= AL) // Array voll, Daten auswerten
    {
     for (int i = 0; i<AL; i++)
       {
        Min = ((Min)<(LoopTime[i])?(Min):(LoopTime[i]));
        Max = ((Max)>(LoopTime[i])?(Max):(LoopTime[i]));
        Avg += LoopTime[i];
       }

     Avg = Avg / AL;
     Serial1.print((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Minimal       "); &__c[0];})))));Serial1.print(Min);Serial1.println(" ms  ");
     Serial1.print((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Durchschnitt  "); &__c[0];})))));Serial1.print(Avg);Serial1.println(" ms  ");
     Serial1.print((reinterpret_cast<const __FlashStringHelper *>((__extension__({static const char __c[] __attribute__((__progmem__)) = ("Maximal       "); &__c[0];})))));Serial1.print(Max);Serial1.println(" ms  ");
     SerielleAusgabe();
     Min = 0xFFFF;
     Max = 0;
     Avg = 0;
     Messung = 0;
     Index = 0;
    }
}

//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()
{
  bool done = false;
  int state = 0x0;
  while(done == false)
  {
    //wiederholt abfragen, ob bereits gesendet wird (Dauer je 1000 us bzw. 1 ms)
    if(pulseIn(20, state, 1000) == 0 && pulseIn(22, state, 1000) == 0 && pulseIn(4, state, 1000) == 0)
    //if(pulseIn(22, state, 1000) > 0)
    {
      //wenn nichts gesendet wird, selbst Messung durchführen
      proxSensors.read();
      proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
      proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
      proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
      proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
      done = true;
    }
    else
    {
      //solange fremde Signale empfangen werden, Fehler ausgeben
      Serial.println("Fremdmessung erkannt");
    }
  }
}

//Update Liniensensoren
void LineUpdate()
{
  uint16_t lineRaw[3];
  lineSensors.read(lineRaw); //lese Messwerte der Liniensensoren aus
  lineValue[0] = LineFilter0.in(lineRaw[0] - lineOffset[0]); //ziehe Offsetwerte von Messwerten ab und gib diese in Filter ein
  lineValue[1] = LineFilter1.in(lineRaw[1] - lineOffset[1]); //erhält neue mittlere Werte der Filter
  lineValue[2] = LineFilter2.in(lineRaw[2] - lineOffset[2]); //"LineFilter.out" um gefilterte Werte auszugeben ohne neue Werte einzugeben
}

//Update Drehbewegungssensor
void GyroUpdate()
{
  gyro.read(); //Rohwert 10285 entspricht 90° bzw.
  turnRate = gyro.g.z - gyroOffset; //8,75mdps/LSB
  uint16_t m = micros();
  uint16_t dt = m - gyroLastUpdate;
  gyroLastUpdate = m;
  int32_t d = (int32_t)turnRate * dt;
  turnAngle += (int64_t)d * 14680064 / 17578125;
  rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
}

//Update Beschleunigungssensor
void CompassUpdate()
{
  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.getCountsAndResetLeft();
  driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
  encoderCounts[1] += encoders.getCountsAndResetRight();
  driveRotation[1] = encoderCounts[1] / 910;
}

/*-----------------------------------------------------------------*/

//Funktion Kontaktvermeiden
void Kontaktvermeiden()
{
  //Serial1.println("Kontaktvermeiden");
  Serial1.print(1);
  ledRed(1);
  motors.setSpeeds(0, 0);
  delay(1000);
  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[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);
}

//Funktion Hindernisumfahrung
void Hindernisumfahren()
{
  //Serial1.println("Hindernisumfahren"); 
  Serial1.print(1);
  ledYellow(1);

  //Schritt 1: Spurwechsel links   
  //links drehen    
  turnAngle = 0;
  rotationAngle = 0;
  GyroUpdate();
  while(rotationAngle < 20)
  {
    GyroUpdate();
    LineUpdate();
    motors.setSpeeds(26, 100);
  }
  GyroUpdate();
  while(rotationAngle < 45)
  {
    GyroUpdate();
    LineUpdate();
    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);
  }

  //rechts drehen
  GyroUpdate();
  while(rotationAngle > 10)
  {
    GyroUpdate();
    LineUpdate();
    if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 0);
    else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 100);
    else motors.setSpeeds(104, 25);
  }

  //geradeaus fahren
  motors.setSpeeds(104, 100);

  //Gegenverkehr beachten
  ProxUpdate();
  //if(proxValue[1] == 6 && proxValue[2] == 6)
  //{
    //Schritt 2: Hindernis passieren
    //Serial1.println("Aufholen"); 
    lastUpdate = millis();
    while(proxValue[3] < 6)
    {
      ProxUpdate();
      LineUpdate();
      Spurhalten();
      TimeUpdate();
      //Serial1.println(eventTime);
      if(eventTime > 3000) break;
    }
    //Serial1.println("Vorbeifahren"); 
    ProxUpdate();
    while(proxValue[3] == 6)
    {
      ProxUpdate();
      LineUpdate();
      Spurhalten();
    }
    //Serial1.println("Abstand gewinnen"); 
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 3000)
    {
      LineUpdate();
      Spurhalten();
      TimeUpdate();
      //Serial1.println(eventTime);
    }
  //}

  //Schritt 3: Spurwechsel rechts     
  //rechts drehen     
  turnAngle = 0;
  rotationAngle = 0;
  GyroUpdate();
  while(rotationAngle > -20)
  {
    GyroUpdate();
    LineUpdate();
    motors.setSpeeds(104, 25);
  }
  GyroUpdate();
  while(rotationAngle > -45)
  {
    GyroUpdate();
    LineUpdate();
    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);
  }

  //links drehen
  GyroUpdate();
  while(rotationAngle < -10)
  {
    GyroUpdate();
    LineUpdate();
    if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
    else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 100);
    else motors.setSpeeds(26, 100);
  }

  //geradeaus fahren
  //Serial1.println("Umfahren beendet");
  motors.setSpeeds(104, 100);
  stop = false;
  stopUpdate = millis();
  Serial1.print(0);
}

//Funktion Abbiegen
void Abbiegen()
{
  ledYellow(1);
  //Serial1.println("Abbiegen");  
  Serial1.print(1);

  //Markierung analysieren
  LineUpdate();
  bool leftCode; //links => 1
  bool rightCode; //rechts => 2
  if((lineValue[0] > 500 && lineValue[0] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) leftCode = true;
  else leftCode = false;
  if((lineValue[2] > 500 && lineValue[2] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) rightCode = true;
  else rightCode = false;

  //Zufallszahl erzeugen
  uint8_t randy; //geradeaus => 0
  if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
  else if(leftCode == true && rightCode == false) //randy = 1;
  {
    randy = random(0, 2); //0, 1
    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
  }
  else if(leftCode == false && rightCode == true) //randy = 2;
  {
    randy = random(0, 2); //0, 1
    //if(randy == 0) randy = random(0, 2);                                  //erhöht Wahrscheinlickeit abzubiegen
    if(randy == 1) randy = 2; //!1 => 2
  }

  //links Abbiegen (von der Verbindungsstrecke)
  if(randy == 1 && rightCode == true)
  {
    //Serial1.println("links Abbiegen von der Verbindungsstrecke"); 
    //geradeaus zur Mittellinie fahren
    motors.setSpeeds(104, 100 +10);
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      motors.setSpeeds(104, 100 +10);
    }
    LineUpdate();
    while(lineValue[0] < 150 && lineValue[2] < 120)
    {
      LineUpdate();
      motors.setSpeeds(104, 100 +10);
    }

    //links drehen
    turnAngle = 0;
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle < 90)
    {
      GyroUpdate();
      LineUpdate();
      if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
      else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(52, 100);
      else motors.setSpeeds(26, 100);
    }

    //geradeaus fahren
    motors.setSpeeds(104, 100);
  }

  //links Abbiegen (vom Rundkurs)
  else if(randy == 1 && leftCode == true)
  {
    //Serial1.println("links Abbiegen vom Rundkurs"); 
    //links drehen
    motors.setSpeeds(26, 100);
    turnAngle = 0;
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle < 40)
    {
      GyroUpdate();
      motors.setSpeeds(26, 100);
    }
    GyroUpdate();
    while(rotationAngle < 85)
    {
      GyroUpdate();
      motors.setSpeeds(26, 100);
    }

    //geradeaus fahren
    motors.setSpeeds(104, 100);
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      LineUpdate();
      motors.setSpeeds(104, 100);
      if(lineValue[2] > 120 && lineValue[2] < 500) break;
    }
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      LineUpdate();
      Spurhalten();
    }
  }

  //rechts Abbiegen
  else if(randy == 2 && rightCode == true)
  {
    //Serial1.println("rechts Abbiegen"); 
    //rechts drehen
    motors.setSpeeds(104, 25);
    turnAngle = 0;
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle > -40)
    {
      GyroUpdate();
      LineUpdate();
      motors.setSpeeds(104, 25);
      if(lineValue[0] > 150 && lineValue[0] < 500) break;
    }
    GyroUpdate();
    lastUpdate = millis();
    while(rotationAngle > -85)
    {
      TimeUpdate();
      GyroUpdate();
      LineUpdate();
      if(eventTime > 3000) break;
      if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 0);
      //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
      else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 50);
      else motors.setSpeeds(104, 25);
    }
  }

  //nicht Abbiegen, geradeaus fahren
  else
  {
    //Serial1.println("nicht Abbiegen");
    motors.setSpeeds(104, 100);
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      LineUpdate();
      Spurhalten();
    }
  }
  stop = false;
  stopUpdate = millis();
  //Serial1.println("Abbiegen beendet");
  Serial1.print(0);
}

//Funktion Spurhalten
void Spurhalten()
{
  uint16_t StartTime = millis();
  uint16_t Update = millis();
  uint16_t Time = Update - StartTime;

  //linke Linie erreicht, nach rechts fahren
  if(lineValue[0] > 150 && lineValue[0] < 500)
  {
    ledYellow(1);
    //Serial1.println("Spur nach rechts korrigieren");  
    motors.setSpeeds((int)104/eventSpeed, (int)25/eventSpeed);
    while(Time < 100)
    {
      Update = millis();
      Time = Update - StartTime;
      LineUpdate();
      if(lineValue[2] > 120) break;
    }
    stop = false;
    stopUpdate = millis();
  }

  //rechte Linie erreicht, nach links fahren
  else if(lineValue[2] > 120 && lineValue[2] < 500)
  {
    ledYellow(1);
    //Serial1.println("Spur nach links korrigieren");  
    motors.setSpeeds((int)26/eventSpeed, (int)100/eventSpeed);
    while(Time < 100)
    {
      Update = millis();
      Time = Update - StartTime;
      LineUpdate();
      if(lineValue[0] > 150) break;
    }
    stop = false;
    stopUpdate = millis();
  }

  //normale Fahrt
  else
  {
    ledGreen(1);
    motors.setSpeeds((int)104/eventSpeed, (int)100/eventSpeed);
    stop = true;
  }
}

//Funktion Spurfinden
void Spurfinden()
{
  ledRed(1);
  //Serial1.println("Spurfinden"); 
  Serial1.print(1);
  lastUpdate = millis();
  while(lineValue[0] < 150 && lineValue[2] < 120) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
  {
    TimeUpdate();
    LineUpdate();
    motors.setSpeeds(-104, -100);
    if(eventTime > 3000) break;
  }
  stop = false;
  stopUpdate = millis();
  //Serial1.println("Spur gefunden"); 
  Serial1.print(0);
}

/*-----------------------------------------------------------------*/

void loop()
{
  //LoopTiming();                             //Zykluszeit beträgt max. 20ms im Idle
  ledGreen(0);
  ledYellow(0);
  ledRed(0);

  //Abfragen der Sensordaten                                 
  ProxUpdate();
  EncoderUpdate();
  GyroUpdate();
  CompassUpdate();
  LineUpdate();
  TimeUpdate();
  StopUpdate();

  //Funktionsauswahl
  if(Serial1.available() > 0) btData = Serial1.read();
  if(btData == '1') btBuffer = true;
  else if(btData == '0') btBuffer = false;

  //verfügbare Funktionen bei laufenden Manövern
  if(btBuffer == true)
  {
    //Serial1.println("Verstanden");
    eventSpeed = 1.4;
    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(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();
    else Spurhalten();
  }
  //LoopTiming();                                                     
}