//Verfassser: Felix Stange, 4056379, MET2016 
//Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018 
//Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe 
//der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen). 
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
//dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt. 
//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth). 
//Das LCD kann bei Bluetoothnutzung nicht verwendet werden. 

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

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

#define   MARKLINE0       150
#define   MARKLINE1       100
#define   MARKLINE2       120
#define   SIGN0           500
#define   SIGN1           300
#define   SIGN2           500
#define   MAXSPEED        400
#define   FULLSPEEDLEFT   106
#define   HALFSPEEDLEFT   53
#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)

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;

//int32_t   moveDisplay = 0;              //Beschleunigung
//int32_t   moveDistance = 0;   
int16_t   moveRate;
int16_t   compassOffset;                
//uint16_t  compassLastUpdate;

uint16_t  lastUpdate = 0;               //Systemzeit
uint16_t  eventTime = 0;                //Zeit seit letzter Geschwindigkeitsänderung
char      report[200];                  //Ausgabe
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(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");   
  if(Serial1.available() > 0) Serial1.read();       //Verwerfen der alten Informationen aus dem Puffer
}           

//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.read();
    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);
  //Serial1.println("Start");
}

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

//Update Durchlaufzeit
void TimeUpdate()
{
  uint16_t m = millis();
  eventTime = m - lastUpdate;
}

void LoopTiming()
{
  const int AL = 100;       // Arraylänge, NUR GERADE Zahlen verwenden!
  static unsigned long LoopTime[AL];
  static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;
 
  if (Messung % 2 == 0)     // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
    {
     LoopTime[Index] = millis();
     Messung++;
     Index++;   
     return;	              // Funktion sofort beenden, spart etwas Zeit
    }

  if (Messung % 2 == 1)     // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
    {
     LoopTime[Index] = millis();
     LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1];  	// Loopdauer einen Index niedriger einspeichern wie aktuell
     Messung++;
    }	
	   
  if (Index >= AL) 	        // Array voll, Daten auswerten
    { 
     for (int i = 0; i<AL; i++)
       {
        Min = min(Min, LoopTime[i]);
        Max = max(Max, LoopTime[i]);
        Avg += LoopTime[i];
       }
	
     Avg = Avg / AL;
     Serial1.print(F("Minimal       "));Serial1.print(Min);Serial1.println(" ms  ");
     Serial1.print(F("Durchschnitt  "));Serial1.print(Avg);Serial1.println(" ms  ");
     Serial1.print(F("Maximal       "));Serial1.print(Max);Serial1.println(" ms  ");
     SerielleAusgabe();
     Min = 0xFFFF;
     Max = 0;
     Avg = 0;
     Messung = 0;
     Index = 0;
    }
}

//Update Abstandssensoren
void ProxUpdate()
{
  proxSensors.read();
  proxValue[0] = proxSensors.countsLeftWithLeftLeds();
  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
  proxValue[3] = proxSensors.countsRightWithRightLeds(); 
}

//Update Liniensensoren
void LineUpdate()
{
  uint16_t  lineRaw[3];
  lineSensors.read(lineRaw);
  lineValue[0] = lineRaw[0] - lineOffset[0];
  lineValue[1] = lineRaw[1] - lineOffset[1];
  lineValue[2] = lineRaw[2] - lineOffset[2];
}

//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.
  moveRate = compass.a.x - compassOffset;   //bei +/-2g Messbereich 0,61mg/LSB
  // uint16_t m = micros();
  // uint16_t dt = m - compassLastUpdate;
  // compassLastUpdate = m;
  // int32_t d = (int32_t)moveRate * dt;
  // moveDistance += (int64_t)d * dt >> 14;
  // moveDisplay = moveDistance * 9,81 / 1000;
}

//Update Encoder
void EncoderUpdate()
{
  encoderCounts[0] = encoders.getCountsLeft();
  driveRotation[0] = encoderCounts[0] / 910;                        //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
  encoderCounts[1] = encoders.getCountsRight();
  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();
    motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
    if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
  }
  motors.setSpeeds(0, 0);
  //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 < 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); 
    //if(lineValue[0] > MARKLINE0) break;
  } 
  //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] < 5 || proxValue[2] < 5)
  //{
    //Schritt 2: Hindernis passieren
    //Serial1.println("Aufholen"); 
    lastUpdate = millis();
    while(proxValue[3] < 6 && eventTime < 3000)
    {
      TimeUpdate();
      ProxUpdate();
      LineUpdate();
      Spurhalten();
    }
    //Serial1.println("Überholen"); 
    ProxUpdate(); 
    while(proxValue[3] == 6)
    {
      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     
  turnAngle = 0;                
  rotationAngle = 0;                                                      
  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); 
    //if(lineValue[0] > MARKLINE0) break;
  } 
  //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");
  Serial1.print(0); 
  motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
}

//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] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) leftCode = true;
  else leftCode = false;
  if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) 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 < 300)
    {
      TimeUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10); 
    }
    LineUpdate();
    while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
    {
      LineUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
    }
    //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
    turnAngle = 0;  
    rotationAngle = 0;
    driveRotation[1] = 0;
    GyroUpdate();
    while(rotationAngle < 40)
    {
      GyroUpdate();
      EncoderUpdate();
      motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
      //if(driveRotation[1] > 1) break;
    }
    driveRotation[1] = 0;
    GyroUpdate();
    while(rotationAngle < 85)
    {
      GyroUpdate();
      EncoderUpdate();
      motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT); 
      //if(driveRotation[1] > 1) break;
    }
    //geradeaus fahren
    motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT); 
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 1000)
    {
      TimeUpdate();
      LineUpdate();
      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
    turnAngle = 0;  
    rotationAngle = 0;
    driveRotation[0] = 0;
    GyroUpdate();
    while(rotationAngle > -40)
    {
      GyroUpdate();
      EncoderUpdate();
      LineUpdate();
      motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT); 
      if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
    }
    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();
    }
  }
  //Serial1.println("Abbiegen beendet");
  Serial1.print(0); 
}

//Funktion Spurhalten
void Spurhalten()                                                   
{
  //linke Linie erreicht, nach rechts fahren
  if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)                      
  {
    ledYellow(1); 
    //Serial1.println("Spur nach rechts korrigieren");  
    motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
    lastUpdate = millis();
    TimeUpdate();
    while(eventTime < 100)
    {
      TimeUpdate();
      LineUpdate();
      if(lineValue[2] > MARKLINE2) break;
    }   
  }

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

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

//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;
  } 
  //Serial1.println("Spur gefunden"); 
  Serial1.print(0);  
}  

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

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

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

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

  //Funktionsauswahl
  //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(alarm == true)                                    
  {
    //Serial1.println("Verstanden");
    eventSpeed = 1.4;
    if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);  
    else Spurhalten();
  }
  //verfügbare Funktionen im Normalfall
  else                                                              
  {
    eventSpeed = 1.0;
    //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();           
    if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
    else if((lineValue[0] > SIGN0 && lineValue[0] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
    else if((lineValue[2] > SIGN2 && lineValue[2] < 1400) && (lineValue[1] > SIGN1 && lineValue[1] < 700)) Abbiegen();
    else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
    else Spurhalten();
  }
  //LoopTiming();                                                     
}