#include <Servo.h>
#include <SoftwareSerial.h>

//Anpassen:
const int minWert = 180; // untere Grenze für die Regelung der Motorausgänge
const int maxWert = 220; // obere Grenze für die Regelung der Motorausgänge
const int k = 20; //Proportionalitätsfaktor für die Regelung
const unsigned int test = 0; //Debug-Modus. Für test = 1 wird eine Ausgabe über die HardwareSerial erzeugt

//Pins:
int M1In1 = 5;
int M1In2 = 6;
int M2In1 = 11;
int M2In2 = 12;

int M1EncA = 2;
int M1EncB = 8;
int M2EncA = 3;
int M2EncB = 7;

//Motor 1
unsigned long encoderPos1 = 0; //Umdrehungszähler des Rades an Motor1 mit 1 Umdrehung = 2370,72 Werte von encoderPos1 (Übersetzung = 98,78, Messungen pro HallSensordrehung = 24 -> 24 *98,78 = 2370,72)
unsigned long messzeit1Alt = 0; //Zeitpunkt der letzten Messung durch den Hallsensor in mikrosekunden
float messgeschwindigkeit1; //gemessen Geschwindigkeit durch den Hallsensor
int hall1BAlt; //Wert von Hall1B bei der letzen Messung
boolean ersteMessung1; //Ist Messung an Motor1 die erste Messung
float motorAusgang1 = 200; //Standarausgabe an InputPin von Motor1
boolean fahrtRichtung1 = false; //Fahrtrichtung true = vorwaerts, false = rückwärts

//Motor 2
unsigned long encoderPos2 = 0;//Umdrehungszähler des Rades an Motor2 mit 1 Umdrehung = 2370,72 Werte von encoderPos2 (Übersetzung = 98,78, Messungen pro HallSensordrehung = 24 -> 24 *98,78 = 2370,72)
unsigned long messzeit2Alt = 0; //Zeitpunkt der letzten Messung durch den Hallsensor in mikrosekunden
float messgeschwindigkeit2; //gemessen Geschwindigkeit durch den Hallsensor
int hall2BAlt; //Wert von Hall2B bei der letzen Messung
boolean ersteMessung2; //Ist Messung an Motor2 die erste Messung
float motorAusgang2 = 200; //Standarausgabe an InputPin von Motor2
boolean fahrtRichtung2 = true; //Fahrtrichtung true = vorwaerts, false = rückwärts

int a = 0; //Zähler für die Ausgabe
float umdrehungen = 0; //Sollwert für die Radbewegung
boolean fahren = false; //Soll der Roboter fahren? true = ja, false = nein
Servo myservo;
SoftwareSerial mySerial(A0, A1); //Wifly-Modul an A0 und A1

void setup()
{
  pinMode(M1In1, OUTPUT); //In1 Motor 1
  pinMode(M1In2, OUTPUT); //In2 Motor 1
  pinMode(M2In1, OUTPUT); //In1 Motor 2
  pinMode(M2In2, OUTPUT); //In2 Motor 2

  pinMode(M1EncA, INPUT); //Motor 1 Encoder A
  pinMode(M1EncB, INPUT); //Motor 1 Encoder B
  pinMode(M2EncA, INPUT); //Motor 2 Encoder A
  pinMode(M2EncB, INPUT); //Motor 2 Encoder B

  attachInterrupt(digitalPinToInterrupt(M1EncA), Hall1Change, CHANGE); //Interrupt durch den Encoder an Motor 1
  attachInterrupt(digitalPinToInterrupt(M2EncA), Hall2Change, CHANGE); //Interrupt durch den Encoder an Motor 2

  myservo.attach(9); //Servo initialisieren

  Serial.begin(19200); //serielle Schnittstelle Initialisieren (Hardware)
  mySerial.begin(19200); //serielle Schnittstelle Initialisieren (Software / Wifly)
  Serial.println("Start");

  messgeschwindigkeit1 = 0;
  messgeschwindigkeit2 = 0;

}

void loop()
{
  if (test == 1) //Debugausgaben erzeugen
  {
    if (a >= 10000) //nur alle 10000 loop-Durchgänge eine Ausgabe erzeugen
    {
      Serial.print(messgeschwindigkeit1);
      Serial.print('\t');
      Serial.print(encoderPos1);
      Serial.print('\t');
      Serial.print(encoderPos1 / 2370.72);
      Serial.print('\t');
      Serial.print(motorAusgang1);
      Serial.print('\t');
      Serial.print(messgeschwindigkeit2);
      Serial.print('\t');
      Serial.print(encoderPos2);
      Serial.print('\t');
      Serial.print(encoderPos2 / 2370.72);
      Serial.print('\t');
      Serial.print(motorAusgang2);
      Serial.print('\n');

      a = 0;
    }
    a++;
  }

  if ((encoderPos1 > umdrehungen * 2370.72) || (encoderPos2 > umdrehungen * 2370.72)) // Wenn der Roboter die geforderte Anzahl an Runden gefahren ist
  {
    encoderPos1 = 0; //Umdrehungszähler zurücksetzen
    encoderPos2 = 0; //Umdrehungszähler zurücksetzen
    fahren = false; //stoppen
    mySerial.write('f'); //An Processing Signal für die Beendigung senden
  }
  Eingabe(); //Überprüfen ob ein Signal von Processing kam
  motorSteuerung(); //Räder steuern
}

void Eingabe()
{
  if (mySerial.available())    // Wenn Daten empfangen wurden und zum Lesen bereitstehen
  {
    int val = mySerial.read();  // lese das erste Zeichen in der Warteschlange und speichere es in der Variable 'val' zwischen
    switch (val)
    {
      case 'R':     // schwamm hoch
        for (int i = 20; i < 160; i++) //Servo drehen
        {
          myservo.write(i);
          delay(20);
        }
        break;
      case 'H': //schwamm runter
        for (int i = 160; i > 20; i--) //Servo drehen
        {
          myservo.write(i);
          delay(20);
        }
        break;
      case 'v': //vorwaerts fahren
        fahren = true;
        fahrtRichtung1 = true;
        fahrtRichtung2 = true;
        motorAusgang1 = 200;
        motorAusgang2 = 200;
        umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen
        break;
      case 'b': //rueckwaerts fahren
        fahren = true;
        fahrtRichtung1 = false;
        fahrtRichtung2 = false;
        motorAusgang1 = 200;
        motorAusgang2 = 200;
        umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen
        break;
      case 'r': //rechts drehen
        fahren = true;
        fahrtRichtung1 = true;
        fahrtRichtung2 = false;
        motorAusgang1 = 200;
        motorAusgang2 = 200;
        umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen
        break;
      case 'l': //links drehen
        fahren = true;
        fahrtRichtung1 = false;
        fahrtRichtung2 = true;
        motorAusgang1 = 200;
        motorAusgang2 = 200;
        umdrehungen = mySerial.parseFloat(); //zu fahrende Umdrehungen einlesen
        break;
      case 's': //stoppen
        fahren = false;
        umdrehungen = 0;
        break;
      default:
        break;
    }
    //mySerial.write(val);
  }
}
void Hall1Change()
{
  boolean illegal = false; //Falls der Encoder prellt oder ähnliches kommt ein Signal, es solla ber keine Geschwindigkeit berechnet werden, da das Signal kein gültiges war

  //Encoderwerte einlesen
  int hall1A = digitalRead(M1EncA);
  int hall1B = digitalRead(M1EncB);

  if (ersteMessung1 == true) //Erste Messung ist immer illegal
  {
    illegal = true;
    ersteMessung1 = false;
  }
  if (hall1BAlt == hall1B) //Wenn sich HallA änder aber HallB nicht ist die Messung illegal
  {
    illegal = true;
  }

  unsigned long currTime1 = micros(); //aktueller Zeitpunkt
  if (illegal == false)
  {
    messgeschwindigkeit1 = 1000000 / float((currTime1 - messzeit1Alt) * 24 * 98.78); //Geschwindigkeit in Umdrehungen pro Sekunde
    if (hall1A == 1)
    {
      if (hall1B == 1) //Falls Hall1A = Hall1B, dann dreht der Motor rückwärts
      {
        messgeschwindigkeit1 *= -1;
      }
    }
    else
    {
      if (hall1B == 0) //Falls Hall1A = Hall1B, dann dreht der Motor rückwärts
      {
        messgeschwindigkeit1 *= -1;
      }
    }
    encoderPos1 += 1; //Umdrehungszähler erhöhen
  }

  //Vergleichsvariablen neu beschreiben
  hall1BAlt = hall1B;
  messzeit1Alt = currTime1;
}

void Hall2Change()
{
  boolean illegal = false; //Falls der Encoder prellt oder ähnliches kommt ein Signal, es solla ber keine Geschwindigkeit berechnet werden, da das Signal kein gültiges war

  //Encoderwerte einlesen
  int hall2A = digitalRead(M2EncA);
  int hall2B = digitalRead(M2EncB);

  if (ersteMessung2 == true) //Erste Messung ist immer illegal
  {
    illegal = true;
    ersteMessung2 = false;
  }
  if (hall2BAlt == hall2B) //Wenn sich HallA änder aber HallB nicht ist die Messung illegal
  {
    illegal = true;
  }

  unsigned long currTime2 = micros(); //aktueller Zeitpunkt
  if (illegal == false)
  {
    messgeschwindigkeit2 = 1000000 / float((currTime2 - messzeit2Alt) * 24 * 98.78); //Geschwindigkeit in Umdrehungen pro Sekunde
    if (hall2A == 0)
    {
      if (hall2B == 1) //Falls Hall1A != Hall1B, dann dreht der Motor rückwärts, da die Motoren auf einer Scheibe angeordnet sind und damit der Roboter vorwärts fährt, muss immer ein Motor vorwärts, und einer Rückwärts drehen
      {
        messgeschwindigkeit2 *= -1;
      }
    }
    else
    {
      if (hall2B == 0) //Falls Hall1A != Hall1B, dann dreht der Motor rückwärts, da die Motoren auf einer Scheibe angeordnet sind und damit der Roboter vorwärts fährt, muss immer ein Motor vorwärts, und einer Rückwärts drehen
      {
        messgeschwindigkeit2 *= -1;
      }
    }
    encoderPos2 += 1; //Umdrehungszähler erhöhen
  }

  //Vergleichsvariablen neu beschreiben
  hall2BAlt = hall2B;
  messzeit2Alt = currTime2;
}


int regelung()
{
  float difGeschwindigkeit = abs(abs(messgeschwindigkeit1) - abs(messgeschwindigkeit2)); //Differenz der Messgeschwindigkeiten
  float x = k * difGeschwindigkeit; //x ist der Wert um den der MotorAusgang1 geändert wird, abhängig von der Differenz der Messgeschwindigkeiten

  if (abs(messgeschwindigkeit1) > abs(messgeschwindigkeit2)) //Wenn Motor1 schneller als Motor2 ist wird Motor1 verllangsamt, sonst beschleunigt
    x *= -1;
  else if (abs(messgeschwindigkeit1) == abs(messgeschwindigkeit2)) //oder keine änderung vorgenommen
    return 1;

  motorAusgang1 += x; //Änderung der Kraft an Motor1

  int differenz = motorAusgang1 - motorAusgang2; //Differenz der MotorAusgänge

  //Verschieben der MotorAusgäm+nge in den Bereich zwischen minWert und maxWert:
  if (differenz < 0) //motorAusgang1 < motorAusgang2
  {
    if (motorAusgang1 < minWert)
    {
      motorAusgang1 = minWert;
      motorAusgang2 = minWert + abs(differenz);
    }
    else if (motorAusgang2 > maxWert)
    {
      motorAusgang2 = maxWert;
      motorAusgang1 = maxWert - abs(differenz);
    }
  }
  else if (differenz > 0) //motorAusgang1 > motorAusgang2
  {
    if (motorAusgang2 < minWert)
    {
      motorAusgang2 = minWert;
      motorAusgang1 = minWert + abs(differenz);
    }
    else if (motorAusgang1 > maxWert)
    {
      motorAusgang1 = maxWert;
      motorAusgang2 = maxWert - abs(differenz);
    }
  }

  //Falls dadurch motorAusgang1 oder motorAusgang2 > 255 oder <0 geworden ist, verschieben in den Bereich zwischen 0 und 255:
  constrain(motorAusgang1, 0, 255);
  constrain(motorAusgang2, 0, 255);
  if (differenz < 255)
  {
    if (motorAusgang1 == 255)
      motorAusgang2 -= abs(differenz);
    else if (motorAusgang1 == 0)
      motorAusgang2 += abs(differenz);
    if (motorAusgang2 == 255)
      motorAusgang1 -= abs(differenz);
    else if (motorAusgang2 == 0)
      motorAusgang1 += abs(differenz);
  }
  return 1;
}

void motorSteuerung()
{
  if (fahren == true) //Wenn er fahren soll
  {
    regelung(); //Motoren nachregeln
    if (fahrtRichtung1 == true) //Motor1 vorwärts
    {
      analogWrite(M1In1, int(motorAusgang1));
      digitalWrite(M1In2, LOW);
    }
    else //Motor1 rückwärts
    {
      digitalWrite(M1In1, LOW);
      analogWrite(M1In2, int(motorAusgang1));
    }

    if (fahrtRichtung2 == true) //Motor2 vorwärts
    {
      analogWrite(M2In1, int(motorAusgang2));
      digitalWrite(M2In2, LOW);
    }
    else //Motor2 rückwärts
    {
      digitalWrite(M2In1, LOW);
      analogWrite(M2In2, int(motorAusgang2));
    }
  }
  else //stoppen (fahren = false)
  {
    digitalWrite(M1In1, LOW);
    digitalWrite(M1In2, LOW);
    digitalWrite(M2In1, LOW);
    digitalWrite(M2In2, LOW);
  }
}