Zumo32U4/ArduinoOutput/sketch/Hauptprojekt...

#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 14.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 LineFilter0(3, 0);         //erstellen der Filter für Liniensensoren
MedianFilter LineFilter1(3, 0);         //Medianfilter geben mittleren Wert einer Reihe aus
MedianFilter LineFilter2(3, 0);         //Fenstergröße: 3, Basiswerte: 0 0 0
MedianFilter ProxFilter0(5, 0);         //erstellen der Filter für Abstandssensoren
MedianFilter ProxFilter1(5, 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;

//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
#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void BlueSetup();
#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineSetup();
#line 117 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void ProxSetup();
#line 123 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void GyroSetup();
#line 156 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void CompassSetup();
#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void setup();
#line 213 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void TimeUpdate();
#line 219 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LoopTiming();
#line 263 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void ProxUpdate();
#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineUpdate();
#line 304 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void GyroUpdate();
#line 317 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void CompassUpdate();
#line 330 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void EncoderUpdate();
#line 341 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Kontaktvermeiden();
#line 360 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Hindernisumfahren();
#line 486 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Abbiegen();
#line 647 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Spurhalten();
#line 692 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Spurfinden();
#line 710 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void SerielleAusgabe();
#line 725 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void loop();
#line 78 "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.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()
{
  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()); 
      // Serial.println(proxValue[0]);
      // Serial.println(proxValue[1]);
      // Serial.println(proxValue[2]);
      // Serial.println(proxValue[3]);
      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.
  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 < 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); 
    //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)
    {
      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); 
    //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] < 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); 
    }
    //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()                                                   
{
  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(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
    while(Time < 100)
    {
      Update = millis();
      Time = Update - StartTime;
      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);   
    while(Time < 100)
    {
      Update = millis();
      Time = Update - StartTime;
      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] < 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();                                                     
}