[FIXED]Due Interrupt Bug

#define VELOCITY_SAMPLE_TIME 10 //sample time (ms) between velocity checks
#define ENCODER_COUNTS 250

#include "Encoder.h"

unsigned long VelocityTimeStamp=0;

Encoder encoderFL(27, 29);
Encoder encoderFR(45, 43);
Encoder encoderRL(31, 33);
Encoder encoderRR(41, 39);

void setup()
{
  Serial.begin(9600);
  
  encoderFL.initilise();
  encoderFR.initilise();
  encoderRL.initilise();
  encoderRR.initilise();

  //Attach Interupts to both pins for each encoder(for full resolution)
  attachInterrupt(encoderFL.pin_a,doEncoderFL,CHANGE);
  attachInterrupt(encoderFL.pin_b,doEncoderFL,CHANGE);
  attachInterrupt(encoderFR.pin_a,doEncoderFR,CHANGE);
  attachInterrupt(encoderFR.pin_b,doEncoderFR,CHANGE);
  attachInterrupt(encoderRL.pin_a,doEncoderRL,CHANGE);
  attachInterrupt(encoderRL.pin_b,doEncoderRL,CHANGE);
  attachInterrupt(encoderRR.pin_a,doEncoderRR,CHANGE);
  attachInterrupt(encoderRR.pin_b,doEncoderRR,CHANGE);

}

void loop()
{
   updateVelocity();
   //ENCODER DEBUG
   Serial.print(encoderFL.getPosition(),DEC);
   Serial.print(" ");
   Serial.print(encoderFR.getPosition(),DEC);
   Serial.print(" ");
   Serial.print(encoderRL.getPosition(),DEC);
   Serial.print(" ");
   Serial.print(encoderRR.getPosition(),DEC);
   Serial.print(" ---- ");
   Serial.print(encoderFL.getVelocity(),DEC);
   Serial.print(" ");
   Serial.print(encoderFR.getVelocity(),DEC);
   Serial.print(" ");
   Serial.print(encoderRL.getVelocity(),DEC);
   Serial.print(" ");
   Serial.print(encoderRR.getVelocity(),DEC);
   Serial.println();

}

//Wrapper Functions to call correct encoder object method on interupt
void doEncoderFL(){
  encoderFL.doCount(); 
}
void doEncoderFR(){
  encoderFR.doCount(); 
}
void doEncoderRL(){
  encoderRL.doCount(); 
}
void doEncoderRR(){
  encoderRR.doCount(); 
}


//Function to calculate velocity of encoders once every VELOCITY_SAMPLE_TIME
void updateVelocity(){
  if(VelocityTimeStamp+VELOCITY_SAMPLE_TIME<millis()){
    encoderFL.calculateVelocity();
    encoderFR.calculateVelocity();
    encoderRL.calculateVelocity();
    encoderRR.calculateVelocity();
    VelocityTimeStamp=millis();
  }
}

#ifndef __ENCODER_H__
#define __ENCODER_H__

#include "Arduino.h"

//This class is designed to manage the encoders 
//Four instances of this class are created: one for each encoder
//The methods in this class are called by interupts (attached in the setup function of the main program)
//There is a wrapper function called 'doEncoder' in the main function designed to call the correct encoder method
//this function is required as interupts cannot call object methods directly (unless static there is no pointer)

class Encoder {
public:

  int pin_a;

  int pin_b;

  // constructor : sets pins as inputs and turns on pullup resistors

  Encoder(int A, int B){
    // Set pin a and b to be input
    Evelocity=0;
    Eposition=0;
    EpositionCache=0;
    pin_a=A;
    pin_b=B;

  };

  void initilise(){
    pinMode(pin_a, INPUT); 
    pinMode(pin_b, INPUT); 

    // Turn on pullup resistors
    digitalWrite(pin_a, HIGH);    
    digitalWrite(pin_b, HIGH);   
  }
  
  void doCount(){
    //Serial.println("Doing Count");
    State = (digitalRead(pin_a)<<1)|digitalRead(pin_b);
    switch(State)
    {
      case 0:
        if(PreviousState==1)Eposition++;
        else Eposition--;
      break;
      case 1:
        if(PreviousState==3)Eposition++;
        else Eposition--;
      break;
      case 2:
        if(PreviousState==0)Eposition++;
        else Eposition--;
      break;
      case 3:
        if(PreviousState==2)Eposition++;
        else Eposition--;
      break;
    }
    PreviousState=State;
  }

  // returns current position
  long int getPosition () { 
    return Eposition; 
  };

  float getVelocity () { 
    return Evelocity; 
  };

  void calculateVelocity(){
    Evelocity = (float(Eposition-EpositionCache)/float(millis()-TimeStamp))*1000/(ENCODER_COUNTS*4);
    TimeStamp = millis();
    EpositionCache=Eposition;
  }

  // set the position value
  void setPosition ( const long int p) { 
    Eposition = p; 
  };

private:

  float Evelocity;

  unsigned long TimeStamp;

  long int EpositionCache;

  volatile long int Eposition;
  
  byte State;
  
  byte PreviousState;
};

#endif // __ENCODER_H__