//Adjust the pin and parameter of vehicle at the below part
#define encoderLeft 2 // we connect the encoder signal from left rear wheel with Pin2
#define encoderRight 3 // we connect the encoder signal from left rear wheel with Pin3
#define ENA 5 //Use Pin5 to produce PWM signal for the fan  
#define IN1 7 // Use Pin7 to control clockwise rotation for the fan
#define IN2 8 // Use Pin7 to control counterclockwise rotation for the fan
float pwmMax = 255; //assume the maximum pwm voltage signal for the motor drive is 255 (L298 is 255, so we use 255)
const int holes = 20; // assume that is 20 holes on the encoder disk for both wheels
const float wheelRadius = 0.03;//m as unit
const float trackWidth = 0.14; //the distance between two rear wheel, m as unit
const float wheelbaseWidth =0.18; //the distance between rear wheels and forward wheels, m as unit
unsigned int pulseCountLeft = 0;
unsigned int pulseCountRight = 0;
unsigned long TimeLeft = 0;
unsigned long TimeRight = 0;

//adjust the constant for PID control with the below code
float kp = 0.9;
float ki = 0.25;
float kd = 0.15;
long current = 0;
long prevT = 0;
float ederivative = 0;
float eprev = 0;
float eintegral = 0;

void setup() {
  Serial.begin(9600); //start serial comunication
  pinMode(encoderLeft, INPUT);
  pinMode(encoderRight, INPUT);
  attachInterrupt(digitalPinToInterrupt(encoderLeft),rpmCounterLeft,RISING);
  attachInterrupt(digitalPinToInterrupt(encoderRight),rpmCounterRight, RISING);
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
}

void rpmCounterLeft(){
  pulseCountLeft++; //Count the time of inrement detected
  TimeLeft = millis(); //update the last time that interrupt occured
} 

void rpmCounterRight(){
  pulseCountRight++; //Count the time of inrement detected
  TimeRight = millis(); //update the last time that interrupt occured
}

float actualVelocity = 0;
float desiredVelocity = 10;//desired velocity for the vehicle in m/s as unit

void loop() {
  //In this case, the fan always rotate in a same direction.So, IN1 always set as high whilst IN2 always set as low.
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  // calculate the angular velocity of the wheel every second
  if(millis()-TimeLeft>=1000){
    float PeriodLeft = holes/(pulseCountLeft/float(millis()-TimeLeft));
    float rpmLeft = 3.14/(1000*PeriodLeft);
    Serial.print("Angular speed of left tire:");
    Serial.println(rpmLeft);
    Serial.print("RPM");
    TimeLeft = millis(); //reset the time
    pulseCountLeft = 0; //reset the pulse count
  }
  if(millis()-TimeRight>=1000){
    float PeriodRight = holes/(pulseCountRight/float(millis()-TimeRight));
    float rpmRight = 3.14/(1000*PeriodRight);
    Serial.print("Angular speed of right tire:");
    Serial.println(rpmRight);
    Serial.print("RPM");
    TimeRight = millis(); //reset the time
    pulseCountRight = 0; //reset the pulse count
  }

  //below code calculate the velocity of the vehicle
  if(rpmRight=rpmLeft){
    actualVelocity = rpmLeft*2*3.14/60*wheelRadius;
  }
  else if(rpmRight>rpmLeft){
    float instantCentre = trackWidth*rpmLeft/(rpmRight-rpmLeft);//distance of instant centre to the inner rear wheel
    float D = sqrt(pow(instantCentre + (trackWidth / 2), 2) + pow(wheelbaseWidth / 2, 2));//distance from instant centre to the centroid of vehicle
    actualVelocity = D*((wheelRadius*rpmLeft*2*3.14/60)/instantCentre);
    Serial.print("Velocity of the vehicle:");
    Serial.println(actualVelocity);
    Serial.print("m/s");
  }
  else{
    float instantCentre = trackWidth*rpmRight/(rpmLeft-rpmRight);//distance of instant centre to the inner rear wheel
    float D = sqrt(pow(instantCentre + (trackWidth / 2), 2) + pow(wheelbaseWidth / 2, 2));//distance from instant centre to the centroid of vehicle
    actualVelocity = D*((wheelRadius*rpmRight*2*3.14/60)/instantCentre);
    Serial.print("Velocity of the vehicle:");
    Serial.println(actualVelocity);
    Serial.print("m/s");
  }

   // calclate the time difference
  current = millis();
  float deltaT = (float(current-prevT))/(1.0e3);
  prevT = current;
  //calculate the error of the desired velocity with the actual velocity
  float e = desiredVelocity - actualVelocity;
  //calculate the integration of the error
  eintegral = eintegral + (e*deltaT);
  //calculate the derivative
  ederivative = (e-eprev)/(deltaT);
  //calculate the control signal using the formula of PID controller
  float u = kp*e + kd*ederivative + ki*eintegral;
  //not allowing the pwm signal exceed the maximum pwm value for the motor driver
  float pwm = fabs(u);
  if(pwm> pwmMax){
    pwm = pwmMax;
  }
  //write the pwm signal for the motor driver
  analogWrite(ENA, pwm);
  //store the previous error of the PID controller
  eprev = e;

  }