#include <PID_v1.h>
/*
 * 
***************************************************************
* Arduino PID Library - Version 1.1.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
***************************************************************
 
 - For an ultra-detailed explanation of why the code is the way it is, please visit: 
   http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
 
 - For function documentation see:  http://playground.arduino.cc/Code/PIDLibrary
 */
 
#include <NIBOburger.h>

float l_max, c_max, r_max;      // Messungen im Feld (100%)
float l, c, r;                  // Messwerte Links, Center, Rechts
double l_minus_r;               // Differenz Messwert Links - Rechts
int16_t pwm_speed = 800;        // Maximale Geschwindigkeit
int16_t pwm_links = pwm_speed;  // Geschwindigkeit linker Motor
int16_t pwm_rechts = pwm_speed; // Geschwindigkeit rechter Motor
char key = ' ';
double Output_links, Output_rechts, Setpoint = 0.0; // Regler-Output im Bereich von 0 bis. 255
double Kp = 2.0 , Kd = 0.5, Ki = 0;
 
// PID_rechts wertet l_minus_r im Bereich von -100 bis 0 aus
// Im Bereich von 0 bis 100 ist der Output_rechts == 0
PID PID_rechts(&l_minus_r, &Output_rechts, &Setpoint, Kp, Ki, Kd, REVERSE);
 
// PID_links wertet l_minus_r im Bereich von 0 .bis 100 aus
// Im Bereich von -100 bis 0 ist der Output_links == 0
PID PID_links(&l_minus_r, &Output_links, &Setpoint, Kp, Ki, Kd, DIRECT);
 
enum
{
  mitte,
  mitte_links,
  mitte_rechts,
  links,
  rechts,
  links_ausserhalb,
  rechts_ausserhalb,
  absturz
}  lage; // Auswertung des Messen über/entlang der Linie
uint8_t lage_old=13;
 
// Untergrund im Feld abseits der Linie messen
void Untergrund()
{
  lage = absturz;
  NIBOburger.setLeds(1,1,1,1);
 
  // Nach dem Drücken der Taste 1 wird der Untegrund gemessen
  while (key != 'A')
  {
    key = NIBOburger.getKeyChar();
  }
  NIBOburger.setLeds(0,0,0,0);
 
  //Serial.println("Messung im Feld");
  //NIBOburger.waitAnalogUpdate();
  c_max = 100. / SurfaceSensor.getC(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("C = ");
  //Serial.println(c_max,DEC);
 
  l_max = 100. /SurfaceSensor.getL(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("L = ");
  //Serial.println(l_max,DEC);
 
  r_max = 100. /SurfaceSensor.getR(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("R = ");
  //Serial.println(r_max,DEC);
 
  //  Nach dem Drücken der Taste 2 wird die Messung gestartet.
  // Der Roboter muss so über der Linie positioniert werden.
  // Über der Linienmitte leuten die beiden blauen LEDs.
  // Danach werden die Motoeren mit Taste 3 gestartet
  while (key != 'B')
  {
    key = NIBOburger.getKeyChar();
  }
  key = ' ';
  while (key != 'C')
  {
    Messen();
    leds();
    key = NIBOburger.getKeyChar();
  }
}
 
 
// Messen über/entlang der Linie
void Messen()
{
  // Die Messwerte der 3 Sensoren werden direkt in %-Werte umgerechnt.
  float c = c_max * SurfaceSensor.getC(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print("C%;");
  //Serial.print(c,DEC);
 
 
  float l = l_max * SurfaceSensor.getL(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print(";L%;");
  //Serial.print(l,DEC);
 
 
  float r = r_max * SurfaceSensor.getR(0);
  //NIBOburger.waitAnalogUpdate();
  //Serial.print(";R%;");
  //Serial.print(r,DEC);
 
  // l_minus_r ist der Input für die beiden PID-Regler
  // Der Sollwert ist 0.0
  l_minus_r = l - r;
  //Serial.print(";l-r;");
  //Serial.println(l_minus_r,DEC);
 
 
  //***********************************
  // Auswertung der Messwerte
  // Im  Regelbereich
  if (c < 40.)
  {
    // Roboter ist über der Linienmitte im Regelbereich
    if (abs(l_minus_r) < 20.)
    {
      //NIBOburger.setLeds(0,1,1,0);
      lage = mitte;
    }
    // Roboter ist rechts von der Linienmitte im Regelbereich
    else if (abs(l_minus_r) == (l_minus_r))
    {
      //NIBOburger.setLeds(0,0,1,0);
      lage = mitte_rechts;
    }
    // Roboter ist links von der Linienmitte im Regelbereich
    else
    {
      //NIBOburger.setLeds(0,1,0,0);
      lage = mitte_links;
    }
  }
  else // (c >=40)
  {
    // Rechts außerhalb des Regelbereiches unter dem rechten Sensor
    if (r < 80.)
    {
      //NIBOburger.setLeds(0,0,1,1);
      lage = rechts;
    }
    // Links außerhalb des Regelbereiches unter dem linken Sensor
    else if (l < 80.)
    {
      //NIBOburger.setLeds(1,1,0,0);
      lage = links;
    }
    // Im Feld, Roboter hat die Linie verloren
    else if (lage == rechts)
    {
      //NIBOburger.setLeds(0,0,0,1);
      lage = rechts_ausserhalb;
    }
    else if (lage == links)
    {
      //NIBOburger.setLeds(1,0,0,0);
      lage = links_ausserhalb;
    }
  }
}
 
void Fahren()
{
  switch (lage) {
    // Bei lage == ..._ausserhalb Drehung auf der Stelle zur Linie hin
    case links_ausserhalb:
    pwm_links = -pwm_speed/2;
    pwm_rechts = pwm_speed/2;
    break;
 
    // Bei lage == ..._ausserhalb Drehung auf der Stelle zur Linie hin
    case rechts_ausserhalb:
    pwm_rechts = -pwm_speed/2;
    pwm_links = pwm_speed/2;
    break;
 
    // Im PID-Regelbereich
    default:
    // Berechnung
    while (!PID_rechts.Compute());
    while (!PID_links.Compute());
    //Output_links und Output_rechts sind im Bereich von 0 bis 255
    pwm_links = pwm_speed - (Output_links * pwm_speed/255.);
    pwm_rechts = pwm_speed - (Output_rechts * pwm_speed/255.);
    pwm_links = constrain(pwm_links, 0, 1024);
    pwm_rechts = constrain(pwm_rechts, 0, 1024);
    break;
 
  }
  Engine.setPWM(pwm_links,pwm_rechts);
}
 
// LED-Anzeige
void leds()
{
  if (lage_old == lage) return;
  switch (lage)
  {
    case mitte:
    NIBOburger.setLeds(0,1,1,0);
    break;
    case mitte_rechts:
    NIBOburger.setLeds(0,0,1,0);
    break;
    case mitte_links:
    NIBOburger.setLeds(0,1,0,0);
    break;
    case rechts:
    NIBOburger.setLeds(0,0,1,1);
    break;
    case links:
    NIBOburger.setLeds(1,1,0,0);
    break;
    case rechts_ausserhalb:
    NIBOburger.setLeds(0,0,0,1);
    break;
    case links_ausserhalb:
    NIBOburger.setLeds(1,0,0,0);
 
  }
  lage_old == lage;
}
// Setup
void setup() {
  NIBOburger.begin();
  //Serial.begin(9600);
  Engine.begin();
  SurfaceSensor.begin();
  PID_rechts.SetMode(AUTOMATIC);
  PID_rechts.SetSampleTime(30);
 
  PID_links.SetMode(AUTOMATIC);
  PID_links.SetSampleTime(30);
 
  Untergrund();
  Engine.setPWM(pwm_links,pwm_rechts);
}
 
void loop() {
  Messen();
  Fahren();
  leds();
}