#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 21.02.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 die Beschleunigungssensoren (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

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  CycleTime = 0;                //Zykluszeit
int       maxSpeed = 200;               //Maximale Geschwindigkeit (max. 400)
char      dir;                          //Fahrtrichtung, Ereignis
char      report[200];                  //Ausgabe
String    btData;                       //Gelesene Daten aus Bluetooth

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

//Setup Liniensensoren
#line 54 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineSetup();
#line 79 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void ProxSetup();
#line 85 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void GyroSetup();
#line 109 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void CompassSetup();
#line 130 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void setup();
#line 157 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void TimeUpdate();
#line 165 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void ProxUpdate();
#line 175 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineUpdate();
#line 185 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void GyroUpdate();
#line 198 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void CompassUpdate();
#line 211 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void EncoderUpdate();
#line 222 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Kollisionserkennung();
#line 238 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Hindernisumfahren();
#line 320 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Abbiegen();
#line 396 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void Spurhalten();
#line 443 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void SerialOutput();
#line 455 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void loop();
#line 54 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
void LineSetup()                                            
{
  ledYellow(1);
  lineSensors.initThreeSensors();     
  
  //Kalibrierung
  uint32_t total[3] = {0, 0, 0};  
  for(uint8_t i = 0; i < 120; i++)                                
  {
    if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
    else motors.setSpeeds(-maxSpeed, maxSpeed);
    lineSensors.read(lineOffset);  
    total[0] += lineOffset[0];
    total[1] += lineOffset[1];
    total[2] += lineOffset[2];
    lineSensors.calibrate();                                      
  }
  ledYellow(0);
  motors.setSpeeds(0, 0);
  lineOffset[0] = total[0] / 120;
  lineOffset[1] = total[1] / 120;
  lineOffset[2] = total[2] / 120;
}

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

//Setup Drehbewegungssensor
void GyroSetup()                                            
{                                                                 
  ledYellow(1);
  gyro.init();

  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();
  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
  Serial1.begin(9600);                                              
  if(Serial1.available()) Serial1.println("Verbindung hergestellt");  

  //Initialisierung und Kalibrierung der Sensoren
  Serial1.println("Sensorkalibrierung");   
  Wire.begin();                       
  delay(500);
  LastUpdate = micros();
  ProxSetup();
  LineSetup();                                                
  GyroSetup();
  CompassSetup();
  
  //Zumo bereit zu starten
  Serial1.println("Zumo bereit, starte mit A");
  ledGreen(1);
  buttonA.waitForButton();
  randomSeed(millis());
  delay(500);
}

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

//Update Durchlaufzeit
void TimeUpdate()
{
  uint16_t m = micros();
  CycleTime = m - LastUpdate;
  LastUpdate = m;
}

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

//Updaten 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();
  turnRate = gyro.g.z - gyroOffset;
  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();
  moveRate = compass.a.x - compassOffset;
  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 * 1000 / 9,81;
}

//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 Kollisionserkennung
void Kollisionserkennung()                                            
{
  dir = 'X';
  Serial1.println("Kollision");
  ledRed(1);

  motors.setSpeeds(0, 0);
  delay(500);
  while(lineValue[0] < 300 && lineValue[2] < 300)
  {
    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
  }
  delay(500); 
}

//Funktion Hindernisumfahrung
void Hindernisumfahren()                                              
{
  dir = 'U';
  Serial1.println("Hindernisumfahren"); 
  ledYellow(1);

  //Schritt 1: Spurwechsel links   
  //links drehen                        
  rotationAngle = 0;                                                      
  GyroUpdate();                                                 
  while(rotationAngle < 45)          
  {
    GyroUpdate();  
    motors.setSpeeds(maxSpeed/2, maxSpeed); 
  }
  //geradeaus über Mittellinie fahren
  LineUpdate();
  while(lineValue[2] < 300)          
  {
    LineUpdate();
    motors.setSpeeds(maxSpeed, maxSpeed); 
  } 
  //rechts drehen
  GyroUpdate();
  while(rotationAngle > 0)                                               
  {                                                                   
    GyroUpdate();
    motors.setSpeeds(maxSpeed, maxSpeed/2); 
  }
  //geradeaus fahren
  motors.setSpeeds(maxSpeed, maxSpeed); 

  //Gegenverkehr beachten
  proxSensors.read();                                                 
  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
  if(proxValue[1] < 5 || proxValue[2] < 5)
  {
    //Schritt 2: Hindernis passieren
    motors.setSpeeds(maxSpeed, maxSpeed);                    
    delay(1000);
    proxSensors.read();
    proxValue[3] = proxSensors.countsRightWithRightLeds();  
    while(proxValue[3] > 4)
    {
      motors.setSpeeds(maxSpeed, maxSpeed);  
      proxSensors.read();
      proxValue[3] = proxSensors.countsRightWithRightLeds();  
      Spurhalten();
    }
  }
  
  //Schritt 3: Spurwechsel rechts     
  //rechts drehen                     
  rotationAngle = 0;                                                      
  GyroUpdate();                                                 
  while(rotationAngle > -45)          
  {
    GyroUpdate();  
    motors.setSpeeds(maxSpeed, maxSpeed/2); 
  }
  //geradeaus über Mittellinie fahren
  LineUpdate();
  while(lineValue[0] < 300)          
  {
    LineUpdate();
    motors.setSpeeds(maxSpeed, maxSpeed); 
  } 
  //links drehen
  GyroUpdate();
  while(rotationAngle < 0)                                               
  {                                                                   
    GyroUpdate();
    motors.setSpeeds(maxSpeed/2, maxSpeed); 
  }
  //geradeaus fahren
  motors.setSpeeds(maxSpeed, maxSpeed); 

  Serial1.println("Umfahren beendet");  
}

//Funktion Abbiegen
void Abbiegen()
{
  dir = 'A';
  ledYellow(1); 
  Serial1.println("Abbiegen");  

  //Kodierung analysieren
  bool  leftCode;                                                         //links => 1
  bool  rightCode;                                                        //rechts => 2
  if(lineValue[0] > 1000) leftCode = true;
  else leftCode = false;
  if(lineValue[2] > 1000) rightCode = true;
  else rightCode = false;

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

  //links Abbiegen
  if(randy == 1 && leftCode == true) 
  {
    //zur Kreuzungsmitte fahren
    driveRotation[0] = 0;
    driveRotation[1] = 0;
    while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
    {
      EncoderUpdate();
      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
    }
    //links drehen
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle != 90)
    {
      GyroUpdate();
      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(0, maxSpeed/2);
      else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
    }
    //geradeaus fahren
    motors.setSpeeds(maxSpeed/2, maxSpeed/2);
  }

  //rechts Abbiegen
  else if(randy == 2 && rightCode == true)
  {
    //rechts drehen
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle != -90)
    {
      GyroUpdate();
      if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, 0);
      else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
      else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
      else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
    }
    //geradeaus fahren
    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
  }  

  //nicht Abbiegen, geradeaus fahren
  else motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
  delay(500);

  Serial1.println("Abbiegen beendet");  
}

//Funktion Spurhalten
void Spurhalten()                                                   
{
  //linke Linie erreicht, nach rechts fahren
  if(lineValue[0] > 300 && lineValue[0] < 500)                      
  {
    dir = 'R';
    ledYellow(1); 
    Serial1.println("Spur nach rechts korrigieren");  
    motors.setSpeeds(maxSpeed, maxSpeed/2);
    delay(100);    
  }

  //rechte Linie erreicht, nach links fahren
  else if(lineValue[2] > 300 && lineValue[2] < 500)                 
  {
    dir = 'L'; 
    ledYellow(1); 
    Serial1.println("Spur nach links korrigieren");  
    motors.setSpeeds(maxSpeed/2, maxSpeed);  
    delay(100);  
  }

  //Linie überfahren, zurücksetzen
  else if(lineValue[1] > 300 && lineValue[1] < 500)                 
  { 
    dir = 'B';  
    ledRed(1); 
    Serial1.println("Spur verlassen");  
    Serial1.println("1"); 
    while(lineValue[0] < 300 && lineValue[2] < 300)                 //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
    {
      motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
    }
    delay(500);    
  }  

  //normale Fahrt
  else                                                                
  {
    dir = 'F'; 
    ledGreen(1); 
    Serial1.println("Spur folgen");  
    motors.setSpeeds(maxSpeed, maxSpeed);
  }
}

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

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

void loop() 
{
  ledGreen(0);
  ledYellow(0);
  ledRed(0);

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

  //Funktionsauswahl
  btData = Serial1.readString();
  //verfügbare Funktionen bei laufenden Manövern
  if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kollision")                                                  
  {
    maxSpeed = 100;
    if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 || 
    lineValue[2] > 1000) motors.setSpeeds(0, 0);  
    else Spurhalten();
  }
  //verfügbare Funktionen im Normalfall
  else                                                              
  {
    if(moveRate > 1000) Kollisionserkennung();         
    else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);  
    else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
    else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
    else Spurhalten();
  }

  //Ausgabe über Bluetoothverbindung
  SerialOutput();                                                         
}