#include <Arduino.h>
#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 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.*/

#include  <Wire.h>
#include  <Zumo32U4.h>
#include  <MedianFilter.h>

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);

#define   MARKLINE0       150
#define   MARKLINE1       100
#define   MARKLINE2       120
#define   SIGN0           500
#define   SIGN1           300
#define   SIGN2           500

#define   MAXSPEED        400
#define   FULLSPEEDLEFT   104
#define   HALFSPEEDLEFT   52
#define   SLOWSPEEDLEFT   26
#define   FULLSPEEDRIGHT  100
#define   HALFSPEEDRIGHT  50
#define   SLOWSPEEDRIGHT  25

int16_t   lineValue[3];                 //Liniensensoren
uint16_t  lineOffset[3];                //von links (0) nach rechts (2)

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

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
#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void BlueSetup();
#line 91 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineSetup();
#line 120 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void ProxSetup();
#line 126 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void GyroSetup();
#line 159 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void CompassSetup();
#line 190 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void setup();
#line 217 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void TimeUpdate();
#line 224 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void StopUpdate();
#line 230 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LoopTiming();
#line 274 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void SerielleAusgabe();
#line 289 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void ProxUpdate();
#line 316 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineUpdate();
#line 326 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void GyroUpdate();
#line 339 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void CompassUpdate();
#line 348 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void EncoderUpdate();
#line 359 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Kontaktvermeiden();
#line 393 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Hindernisumfahren();
#line 525 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Abbiegen();
#line 686 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Spurhalten();
#line 736 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Spurfinden();
#line 757 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void loop();
#line 81 "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(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(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);

  // 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(Min, LoopTime[i]);
        Max = max(Max, LoopTime[i]);
        Avg += LoopTime[i];
       }
	
     Avg = Avg / AL;
     Serial1.print(F("Minimal       "));Serial1.print(Min);Serial1.println(" ms  ");
     Serial1.print(F("Durchschnitt  "));Serial1.print(Avg);Serial1.println(" ms  ");
     Serial1.print(F("Maximal       "));Serial1.print(Max);Serial1.println(" ms  ");
     SerielleAusgabe();
     Min = 0xFFFF;
     Max = 0;
     Avg = 0;
     Messung = 0;
     Index = 0;
    }
}

//Funktion Serielle Ausgabe
void SerielleAusgabe()                                                     
{
  char report[200]; 
  snprintf_P(report, sizeof(report),
    PSTR("Abstand: %d %d %d %d   Linie: %d %d %d"),
    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
    lineValue[0], lineValue[1], lineValue[2]);
  Serial1.println(report);
  snprintf_P(report, sizeof(report),
    PSTR("Weg: %d %d   Winkel: %d   Beschleunigung: %d"),
    driveRotation[0], driveRotation[1], rotationAngle, compassRate);
  Serial1.println(report);
}

//Update Abstandssensoren
void ProxUpdate()
{
  bool  done = false;
  int   state = LOW;
  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(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
    if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
  }
  lastUpdate = millis();
  TimeUpdate();
  while(eventTime < 1000)
  {
    TimeUpdate();
    LineUpdate();
    motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
    if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
  }
  motors.setSpeeds(0, 0);
  compassLastUpdate = 0;
  compassRate = 0;
  CompassUpdate();
  stop = false;
  stopUpdate = millis();
  //Serial1.println("Vermeiden beendet");
  Serial1.print(0); 
}

//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(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
  }
  GyroUpdate();                                                 
  while(rotationAngle < 45)          
  {
    GyroUpdate();  
    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(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
  } 

  //rechts drehen
  GyroUpdate();
  while(rotationAngle > 10)                                               
  {                                                                   
    GyroUpdate();
    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(FULLSPEEDLEFT, FULLSPEEDRIGHT); 

  //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(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
  }
  GyroUpdate();                                                 
  while(rotationAngle > -45)          
  {
    GyroUpdate();  
    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(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
  } 

  //links drehen
  GyroUpdate();
  while(rotationAngle < -10)                                               
  {                                                                   
    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
  //Serial1.println("Umfahren beendet");
  motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
  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] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) leftCode = true;
  else leftCode = false;
  if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && 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(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
    }
    LineUpdate();
    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
    {
      LineUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
    }

    //links drehen
    turnAngle = 0;  
    rotationAngle = 0;
    GyroUpdate();
    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(HALFSPEEDLEFT, FULLSPEEDRIGHT); 
      else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
    }

    //geradeaus fahren
    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
  }

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

    //geradeaus fahren
    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      LineUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
      if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) 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(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
    turnAngle = 0;  
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle > -40)
    {
      GyroUpdate();
      LineUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
      if(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 
  {
    //Serial1.println("nicht Abbiegen");
    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
    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] > MARKLINE0 && lineValue[0] < SIGN0)                      
  {
    ledYellow(1); 
    //Serial1.println("Spur nach rechts korrigieren");  
    motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)SLOWSPEEDRIGHT/eventSpeed);
    while(Time < 100)
    {
      Update = millis();
      Time = Update - StartTime;
      LineUpdate();
      if(lineValue[2] > MARKLINE2) break;
    }   
    stop = false;
    stopUpdate = millis();
  }

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

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

//Funktion Spurfinden
void Spurfinden()             
{ 
  ledRed(1); 
  //Serial1.println("Spurfinden"); 
  Serial1.print(1); 
  lastUpdate = millis();
  while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
  {
    TimeUpdate();
    LineUpdate();
    motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
    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 && abs(compassRate) > 3500) motors.setSpeeds(0, 0); 
    else if(lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
    else Spurhalten();
  }

  //verfügbare Funktionen im Normalfall
  else                                                              
  {
    eventSpeed = 1.0;
    if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) Kontaktvermeiden();           
    else if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
    else if(lineValue[0] > 1600 && lineValue[2] > 1600) motors.setSpeeds(0, 0);
    else if((lineValue[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
    else if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
    else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
    else Spurhalten();
  }
  //LoopTiming();                                                     
}