//  TEST TO READ WHEEL ENCODERS


//loop time parameters
unsigned int loop_time = 20;
unsigned long currentMillis; 
unsigned long previousMillis; 

// wheel encoder interrupts
#define encoder_PinA_right 2  //right encoder chanals
#define encoder_PinB_right 4                         
#define encoder_PinA_left 3   //left encoder chanals
#define encoder_PinB_left 5

volatile long pos_right = 0;  // encoder 1
volatile long pos_left = 0;   // encoder 2



void setup () { 
  // encoder pins 
  pinMode (encoder_PinA_right, INPUT_PULLUP);
  pinMode (encoder_PinB_right, INPUT_PULLUP); 
  
  pinMode (encoder_PinA_left, INPUT_PULLUP); 
  pinMode (encoder_PinB_left, INPUT_PULLUP); 
  
  attachInterrupt (digitalPinToInterrupt(encoder_PinA_right), readEncoder_right, RISING);

  attachInterrupt (digitalPinToInterrupt(encoder_PinA_left), readEncoder_left, RISING);
 
  Serial.begin(115200);
}

void loop(){
  
  currentMillis = millis ();
  
  if (currentMillis - previousMillis >= loop_time) { // start timed loop 
    previousMillis = currentMillis; 
    
    Serial.print(pos_right);
    Serial.print(" , ");
    Serial.print(pos_left);
    Serial.print("\n"); 
    
  } // end of timed loop 
  
} // end of main loop 


// *** encoders interrupts **** 

void readEncoder_right () {
  if (digitalRead(encoder_PinB_right) > 0){
    pos_right++;
  }
  else {
    pos_right--;
  }
}

 void readEncoder_left () { //note that it Reverse right encoder in direction
  if (digitalRead(encoder_PinB_left) > 0){
    pos_left--;
  }
  else {
    pos_left++;
  }
}