// mobile Roboter hat zwei Motoren, die jeweils ein Rad antreiben
// das Programm soll dafuer sorgen, dass die beiden Motoren/Raeder mit der gleichen Geschwindigkeit fahren
// dafuer habe ich eine PI-Rueckkopplungsschleife geschrieben
// die Basis fuer diesen Code liefert die englische Arudino Seite. Den Code habe ich angepasst

unsigned long lastTime;
double OutputL, OutputR, Setpoint;
double ITerm;
double low_kp, low_ki, high_kp, high_ki, kp, ki;
int SampleTime = 1000; // 1 sec
double errorR, errorL;
double outMin, outMax;
bool inAuto = false;
int SpeedL, SpeedR;

#define MANUAL 0        // steht fuer das Ausschalten der Rueckkopplungsschleife
#define AUTOMATIC 1     // steht fuer das Anschalten der Rueckkopplungsschleife

//Data to Motor Driver
const int LeftDir = 7;
const int RightDir = 10;
const int PWML = 9;
const int PWMR = 8;

//Encoder
const int RightEncoderPinA = 3;
const int RightEncoderPinB = 2;
const int LeftEncoderPinA = 0;
const int LeftEncoderPinB = 1;

//Ticks/Speed
volatile int LeftEncoderPos = 0;
volatile int LeftEncoderDir = 0;
volatile int RightEncoderPos = 0;
volatile int RightEncoderDir = 0;

void setup()
{
  Serial.begin(9600);

  pinMode(LeftDir, OUTPUT);
  pinMode(RightDir, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(0), LeftEncoderEvent, CHANGE);
  attachInterrupt(digitalPinToInterrupt(3), RightEncoderEvent, CHANGE);

  SetSampleTime(1000);
  SetOutputLimits(-255, 255); // grenzt die von den Motoren anzunehmende Geschwindikeit und den Integralwert der PI-Schleife ein
  SetMode(AUTOMATIC); // aktiviert die Rueckkopplungsschleife bzw. den PI-Regler
  Setpoint = 40; //Gibt die zu erreichende Sollgeschwindigkeit fuer beide Motoren an

  low_kp = 1;  // falls der Unterschied zwischen Ist- und Sollgeschwindigkeit klein ist, nimmt die PI-Schleife den Wert low_kp an, ansonsten
  high_kp = 4; // wird der Wert high_kp angenommen
  low_ki = 0.05;
  high_ki = 0.2;
}

void loop()
{
        
  int PosL, PosR;
  static int oldPosL, oldPosR;

  delay(1000);

    PosL = LeftEncoderPos;
    PosR = RightEncoderPos;
    Serial.print("LeftEnoderPos=");
    Serial.println(LeftEncoderPos);
    Serial.print("RightEnoderPos=");
    Serial.println(RightEncoderPos);
    
    LeftEncoderPos = 0;
    RightEncoderPos = 0;
    
    if(PosL != oldPosL){
    Serial.print("Encoder 1=");
    Serial.println(PosL,DEC);
    SpeedL = PosL - oldPosL;
    oldPosL = PosL;
    }

    if(PosR != oldPosR){
    Serial.print("Encoder 2=");
    Serial.println(PosR,DEC);
    SpeedR = PosR - oldPosR;
    oldPosR = PosR;
    }
  

  if (!inAuto) return; //legt fest, ob die PI-Reglung an ist oder aus

  // Beginn der PI-Regelung
  unsigned long now = millis();
  int timeChange = (now - lastTime);

  if (timeChange >= SampleTime)
  {
      
    double average = (SpeedL + SpeedR) / 2; // diese un die nachsten 4 Zeilen entscheiden, welche kp und ki Werte fuer die Rueckkopplung verwendet werden sollen
    double gap = abs(Setpoint - average);

    if (gap < 10) SetTunings(low_kp, low_ki);
    else if (gap > 10) SetTunings(high_kp, high_ki);
    
    errorL = Setpoint - SpeedL;
    errorR = Setpoint - SpeedR;
    Serial.print("Setpoint: ");
    Serial.println(Setpoint);
    Serial.print("SpeedL: ");
    Serial.println(SpeedL);
    Serial.print("SpeedR: ");
    Serial.println(SpeedR);
    
    ITerm = ITerm + ki * (SpeedL - SpeedR);   //Berechnung des Integralwertes der PI-Regelung
    if (ITerm > outMax) ITerm = outMax;       //Begrenzung des Integralwertes auf Maximum "outMax", das wegen das maximalen Signals der PWM von 255 auch auf 255 festgelegt ist
    else if (ITerm < outMin) ITerm = outMin;

    //Left Wheel SPC
     
    OutputL = kp * errorL - ITerm;   // Berechnung des Rueckgabewertes/PWM-Signal/Geschwindigkeit fuer den linken Motor
    if (OutputL > outMax) OutputL = outMax;
    else if (OutputL < outMin) OutputL = outMin;
    
    if (OutputL > 0)  // max PWM duty cycle
     {OutputL = OutputL;
        digitalWrite(LeftDir, HIGH);//driving forward
        Serial.print("OutputL_F:");
        Serial.println(OutputL);
      } 
    else if (OutputL < 0)
      {   
        OutputL = -OutputL;  // make speed a positive quantity
        digitalWrite(LeftDir, LOW);  // backward driving
        Serial.print("OutputL_B:");
        Serial.println(OutputL);  
      }
        
    // Right Wheel SPC
    OutputR = kp * errorR + ITerm;
    if (OutputR > outMax) OutputR = outMax;
    else if (OutputR < outMin) OutputR = outMin;
    
    if (OutputR > 0)  // max PWM duty cycle
    {OutputR = OutputR;
        digitalWrite(RightDir, LOW); // forward DRIVING 
        Serial.print("OutputR_F:");
        Serial.println(OutputR);
    } 
    else if (OutputR < 0)
    {
      OutputR = -OutputR;  // make speed a positive quantity
      digitalWrite(RightDir, HIGH);  // backward driving
      Serial.print("OutPutR_B: ");
      Serial.println(OutputR);
    
    }
       
    lastTime = now;
    }// Ende der PI-Regelung
    
    analogWrite(PWML, OutputL);    //sendet das PWW-Signal an die H-Bruecke bzw. and den linken Motor
    analogWrite(PWMR, OutputR);   //sendet das PWW-Signal an die H-Bruecke bzw. and den rechten Moto
    }
  
  
void LeftEncoderEvent()
{ //detachInterrupt(digitalPinToInterrupt(0));
 if (digitalRead(LeftEncoderPinA) == HIGH)
{ if (digitalRead(LeftEncoderPinB) == LOW)
    {LeftEncoderPos++;}
  else{LeftEncoderPos--;}
  }
  else
  {
    if (digitalRead(LeftEncoderPinB) == LOW)
    { LeftEncoderPos--;
      digitalWrite(LeftEncoderDir, LOW);
    }
    else
      LeftEncoderPos++;
    digitalWrite(LeftEncoderDir, HIGH);
  }
}                                       

void RightEncoderEvent()
{//detachInterrupt(digitalPinToInterrupt(3));
  if (digitalRead(RightEncoderPinA) == HIGH)
  {
   if (digitalRead(RightEncoderPinB) == LOW)
   { RightEncoderPos++;}
   else
   { RightEncoderPos--;}
  }
  else
  {if (digitalRead(RightEncoderPinB) == LOW)
   { RightEncoderPos--;}
    else
    {RightEncoderPos++;
    }
  }
}

void SetTunings(double Kp, double Ki) // erlaubt die Eingabe von kp und ki in sec
{
  double SampleTimeInSec = ((double)SampleTime) / 1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
}

void SetSampleTime(int NewSampleTime) //falls die Sampletime/Periodendauer eines Schleifendurchgangs geandert wird, wird ki daran von selbst angepasst, ohne das ki manuell angepasst werden muss
{
  if (NewSampleTime > 0)
  {
    double ratio = (double)NewSampleTime / (double)SampleTime;
    ki *= ratio;
    SampleTime = (unsigned long)NewSampleTime;
  }
}

void SetOutputLimits(double Min, double Max) // sets the lower and upper limit of the output
{
  if (Min > Max) return;
  outMin = Min;
  outMax = Max;

  if (OutputL > outMax) OutputL = outMax;
  else if (OutputL < outMin) OutputL = outMin;

  if (OutputR > outMax) OutputR = outMax;
  else if (OutputR < outMin) OutputR = outMin;

  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;
}

//Ist die Funktion die den PI-Regler an oder ausstellt
void SetMode(int Mode)
{
  bool newAuto = (Mode == AUTOMATIC);
  if (newAuto && !inAuto)
  {
    Initialize();
  }
  inAuto = newAuto;
}


//verhindert, dass die PI-Schleife zueruck zu dem letzen Ausgangswert der Geschwindigkeit springt, wenn die PI-Schleife wieder angeschaltet wird
void Initialize()
{
  ITerm = (OutputL + OutputR) / 2;
  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;
}