drive.txt

volatile long left_enc_pos = 0L;
  volatile long right_enc_pos = 0L;
  static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};  //encoder lookup table
  
  int Rencoder_value =0;
  int Lencoder_value=0;
  void R_ENA() {
    // Reading the current state of encoder A and B
      int RA = digitalRead(RIGHT_ENC_PIN_A);
      int RB = digitalRead(RIGHT_ENC_PIN_B);
    // If the state of A changed, it means the encoder has been rotated
    if ((RA == HIGH) != (RB == LOW)) {
      Rencoder_value--;
    } else {
      Rencoder_value++;
    } 
    right_enc_pos += ENC_STATES[(Rencoder_value & 0x0f)];
  }
  void L_ENA() {
      int LA = digitalRead(LEFT_ENC_PIN_A);
      int LB = digitalRead(LEFT_ENC_PIN_B);
    if ((LA == HIGH) != (LB == LOW)) {
      Lencoder_value--;
    } else {
      Lencoder_value++;
    }
  //   static uint8_t enc_last=0;
        
	// enc_last <<=2; //shift previous state two places
	// enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
  
   	left_enc_pos += ENC_STATES[(Lencoder_value & 0x0f)];
  }

  /* Wrap the encoder reading function */
  long readEncoder(int i) {
    if (i == LEFT) return left_enc_pos;
    else return right_enc_pos;
  }

  /* Wrap the encoder reset function */
  void resetEncoder(int i) {
    if (i == LEFT){
      left_enc_pos=0L;
      return;
    } else { 
      right_enc_pos=0L;
      return;
    }
  }

/* Wrap the encoder reset function */
void resetEncoders() {
  resetEncoder(LEFT);
  resetEncoder(RIGHT);
}