/*
 * 
 *  2 PID controllers, one for each dc motor w/ encoders.
 *  Each PID as its own set of variables and parameters
 *  Both PID have the same setPoint (same encoder pulses target per sampleTime)
 *  It have a flag for 3 kinds of encoders
 *  No special handling of start period
 */
 
#define outputForPlotter true
#define serialSpeed 115200

// encoder type
#define encoderSJ01 true
#define encoderHC020K false
#define encoderLM393 false

#if encoderSJ01
#define encoderSignal CHANGE
#endif
#if encoderHC020K
#define encoderSignal FALLING
#endif
#if encoderLM393
#define encoderSignal RISING
#endif

// motor one
#define enA 5
#define in1 4
#define in2 7
// motor two
#define enB 6
#define in3 8
#define in4 12

// LeftMotor
#define LeftMotorEn enA
#define LeftMotorForward in1
#define LeftMotorBackward in2

// RightMotor
#define RightMotorEn enB
#define RightMotorForward in4
#define RightMotorBackward in3

// encoder LeftMotor
#define encoderLeftInt 0
#define encoderLeftFunction encoderLeftCounter
#define encoderLeftPin 2 // A pin the interrupt pin
#if encoderSJ01
#define encoderLeftPinB 9 // B pin: the digital pin
#endif
#define encoderLeftSignal encoderSignal

// encoder RightMotor
#define encoderRightInt 1
#define encoderRightFunction encoderRightCounter
#define encoderRightPin 3 // A pin: the interrupt pin
#if encoderSJ01
#define encoderRightPinB 10 // B pin: the digital pin
#endif
#define encoderRightSignal encoderSignal

#define pidSetPoint 5
#define pidSampleTime 25

#define pidLeftMinOutput 60
#define pidLeftMaxOutput 225
#define pidRightMinOutput 60
#define pidRightMaxOutput 255

#include <PID_v1.h>

// the left is the master the setPoint for left its the desired pulses per pidSampleTime
double encoderLeftPulses, encoderLeftPulsesAbs = 0; // the number of the pulses, the last is pidInput
signed long encoderLeftTotalPulses = 0;    // total pulses since last reset (use: calculate error left-right)

byte encoderLeftPinLast;
#if encoderSJ01
boolean encoderLeftDirection; // the rotation direction
#endif

// the right is the slave the setPoint for right its the desired pulses per pidSampleTime
double encoderRightPulses, encoderRightPulsesAbs = 0; // the number of the pulses, the last is pidInput 
signed long encoderRightTotalPulses = 0;    // total pulses since last reset (use: error left-right as pidInput correction)

byte encoderRightPinLast;
#if encoderSJ01
boolean encoderRightDirection; //the rotation direction 
#endif

double pidLeftInput;
double pidLeftOutput; // Power supplied to the motor PWM value.
double pidLeftSetPoint;
double KpLeft=0.6, KiLeft=5, KdLeft=0;

double pidRightInput;
double pidRightOutput; // Power supplied to the motor PWM value.
double pidRightSetPoint;
double KpRight=0.6, KiRight=5, KdRight=0;

PID encoderPIDLeft(&pidLeftInput, &pidLeftOutput, &pidLeftSetPoint, KpLeft, KiLeft, KdLeft, DIRECT); 
PID encoderPIDRight(&pidRightInput, &pidRightOutput, &pidRightSetPoint, KpRight, KiRight, KdRight, DIRECT);

unsigned long moveLoopCounter = 0;
unsigned long loopTimeStart = 0;

void setup()
{  
  Serial.begin(serialSpeed); //Initialize the serial port

  pinMode(LeftMotorEn, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorEn, OUTPUT);
  pinMode(RightMotorForward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);

  pidLeftSetPoint = pidSetPoint; 
  encoderPIDLeft.SetMode(AUTOMATIC); 
  encoderPIDLeft.SetSampleTime(pidSampleTime); 
  encoderPIDLeft.SetOutputLimits(pidLeftMinOutput, pidLeftMaxOutput);

#if encoderSJ01
  encoderLeftDirection = true; //default -> Forward  
  pinMode(encoderLeftPinB, INPUT_PULLUP);  
#endif
  attachInterrupt(encoderLeftInt, encoderLeftFunction, encoderLeftSignal);

  pidRightSetPoint = pidSetPoint; 
  encoderPIDRight.SetMode(AUTOMATIC); 
  encoderPIDRight.SetSampleTime(pidSampleTime); 
  encoderPIDRight.SetOutputLimits(pidRightMinOutput, pidRightMaxOutput);

#if encoderSJ01
  encoderRightDirection = true; // default -> Forward  
  pinMode(encoderRightPinB, INPUT_PULLUP);  
#endif
  attachInterrupt(encoderRightInt, encoderRightFunction, encoderRightSignal);

  loopTimeStart = millis();
}
 
void loop()
{   
  
  boolean pidResult;
  unsigned long loopTime = 0;
 
  moveForward(pidLeftOutput, pidRightOutput); // Motor Forward
  
  encoderLeftPulsesAbs = abs(encoderLeftPulses);
  pidLeftInput = encoderLeftPulsesAbs;
  pidResult=encoderPIDLeft.Compute(); // PID conversion is complete and returns 1?
  if(pidResult)
  {
    encoderLeftPulses = 0; // Count clear, wait for the next count
  }

  encoderRightPulsesAbs = abs(encoderRightPulses);
  pidRightInput = encoderRightPulsesAbs;
  pidResult=encoderPIDRight.Compute(); // PID conversion is complete and returns 1?
  if(pidResult)
  {
    encoderRightPulses = 0; // Count clear, wait for the next count
  }

  loopTime = millis() - loopTimeStart;  
  loopTimeStart = millis();
  
  if(pidResult)
  {
    #if outputForPlotter
    //Serial.print(loopTime); Serial.print("\t");
    Serial.print(abs(pidLeftInput)); Serial.print("\t");
    Serial.print(abs(pidRightInput)); Serial.print("\t");
    Serial.print((float)pidLeftOutput/10.0); Serial.print("\t"); // divided for scale display
    Serial.print((float)pidRightOutput/10.0); Serial.print("\t "); // divided for scale display
    Serial.print(pidRightSetPoint); Serial.print("\t");
    Serial.print((((double)abs(encoderLeftTotalPulses) - (double)abs(encoderRightTotalPulses)))/10); Serial.print("\t"); // divided for scale display
    Serial.println(""); 
    #endif
  }

  moveLoopCounter++;
}
 
void encoderLeftCounter()
{
#if encoderSJ01
  int Lstate = digitalRead(encoderLeftPin);
  if((encoderLeftPinLast == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(encoderLeftPinB);
    if(val == LOW && encoderLeftDirection)
    {
      encoderLeftDirection = false; //Reverse
    }
    else if(val == HIGH && !encoderLeftDirection)
    {
      encoderLeftDirection = true;  //Forward
    }
  }
  encoderLeftPinLast = Lstate;
 
  if(!encoderLeftDirection) {
    encoderLeftPulses++;
    encoderLeftTotalPulses++;
  }
  else {
    encoderLeftPulses--;
    encoderLeftTotalPulses--;
  }
#else
  encoderLeftPulses++;
  encoderLeftTotalPulses ++;
#endif
}
 
void encoderRightCounter()
{
#if encoderSJ01
  int Lstate = digitalRead(encoderRightPin);
  if((encoderRightPinLast == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(encoderRightPinB);
    if(val == LOW && encoderRightDirection)
    {
      encoderRightDirection = false; // Reverse
    }
    else if(val == HIGH && !encoderRightDirection)
    {
      encoderRightDirection = true;  // Forward
    }
  }
  encoderRightPinLast = Lstate;
 
  if(!encoderRightDirection) {
    encoderRightPulses++;
    encoderRightTotalPulses ++;  
  } else {
    encoderRightPulses--;
    encoderRightTotalPulses --;  
  }
#else
  encoderRightPulses++;
  encoderRightTotalPulses ++;  
#endif
}

void moveForward( int pmwSpeedLeft, int pmwSpeedRight ){
  // set motors direction
  setMotorLeftForward();
  setMotorRightForward();
  // set motors speed
  analogWrite(LeftMotorEn, pmwSpeedLeft);
  analogWrite(RightMotorEn, pmwSpeedRight);
}

void moveBackward( int pmwSpeedLeft, int pmwSpeedRight ) {
  // set motors direction
  setMotorLeftBackward();
  setMotorRightBackward();  
  // set motors speed
  analogWrite(LeftMotorEn, pmwSpeedLeft);
  analogWrite(RightMotorEn, pmwSpeedRight);
  }

void setMotorLeftForward() {
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
}

void setMotorLeftBackward() {
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(LeftMotorBackward, HIGH);
}

void setMotorRightForward() {
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(RightMotorBackward, LOW);
}

void setMotorRightBackward() {
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(RightMotorBackward, HIGH);
}