Arduino encoder with CAN BUS

Hello everyone,
I have an Arduino Mega with a CAN bus shield and an incremental encoder (3600 ppr). I'm getting the data from the encoder and then it's sent through CAN bus protocol to the computer. The code seems to work but when I speed up the encoder (above 700 rpm) the data reading fluctuates a lot. The code just has to return the degree between the encoder zero and a point where the arduino receives an external signal that triggers an interruption, then the difference is calculated and sent via CAN.
Could be that the arduino is not fast enough to do this task? I would like use the encoder at 3000 RPM.

#include <mcp_can.h>
#include <SPI.h>
// LIBRERIA --------> https://github.com/coryjfowler/MCP_CAN_lib
//--------------- PIN de entrada de CS ---------------------------
MCP_CAN CAN0(53);     // Set CS to pin 53
//---------------------------------------------------------------------

//----------------- Canales entrada ENCODER ---------------------------
// Canales entrada ENCODER
const int channelPinA = 18;             // Ch.A (Black) a pin 18  (con interrupcion INT5)
const int channelPinB = 19;             // Ch.B (White) a pin 19  (con interrupcion INT4)
const int channelPinZ = 3;              // Ch.Z (Orange) a pin 20 (con interrupcion INT0)
//---------------------------------------------------------------------

//----------------- Canales entrada bomba -----------------------------
#define pinBomba       20               // Pin de interrupción (INT.3) pulso cuadrado de bomba (4 o 6 pulsos cuadrados)
//---------------------------------------------------------------------

//--------------- Contadores pulsos encoder ---------------------------
volatile signed int posBomba = 0;
volatile signed int ISRCounter = 0;     // Contador de interrupciones
volatile signed int angulo = 0;         // angulo de la bomba
volatile signed int counter = 0;        // Detectar movimiento en encoder
volatile uint16_t  myPosition;          // Posicion enviada por CAN BUS
const int maxSteps = 3600;              // Pulsos por revolución
//---------------------------------------------------------------------

//------------------------- Flags -------------------------------------
bool IsCW;                       // true ----> giro horario     (CW)
                                 // false ---> giro antihorario (CCW)
bool ZERO;                       // true ----> cero detectado
                                 // false ---> no hay cero
bool aux;                        // true ----> se ha pasado por cero 
                                 // false ---> se pone a cero cuando se devuelve el valor del angulo de la bomba
//---------------------------------------------------------------------

//------------------------- Configuración -----------------------------
#define CAN_ID_TX      0x050            // ID de mensaje que se transmite
#define CAN_ID_RX      0x051            // ID de mensaje que se busca recibir
#define OUT_DIVIDER    4                // PIN salida OUT_DIVIDER
#define OUT_TRIGGER    6                // PIN salida OUT_TRIGGER

#define posAbs         0                // Nº byte donde se recibe posAbs (HIGH o LOW/ 1 o 0)
                                        // es el 1º byte del mensaje recibido ----> rxBuf[0]
#define posDivider     1                // 2º byte recibido en rxBuf
#define posBloqueo     2                // 3º byte recibido en rxBuf

// posición en la que se guarda la posición del encoder mensaje transmitido
#define posEncoderH    0                // se guarda mensaje en 1º byte
#define posEncoderL    1                // se guarda mensaje en 2º byte

#define tTXMsg         100              // tiempo de transmisión entre mensajes
#define tSignTask      10               // 
#define CAN0_INT       2                // Set INT to pin 2 (interrupción de CAN SHIELD)
//---------------------------------------------------------------------

// --------------------------- Variables ------------------------------
// Mensaje TRX
byte data[8];                           // se envian arrays de datos de 8 bytes

// Mensaje RX
long unsigned int rxID;                 // ID recibida
unsigned char len;                      // longitud de mensaje (en bytes)    
unsigned char rxBuf[8];                 // array char ----> buffer mensajes recibidos (8 bytes)

// Señal de activación para transmisión continua 
byte  bContinuousTx = 0;

// Valores de tiempo (en ms) auxuliares
unsigned long tant;                     //
unsigned long tantSign;                 //
//---------------------------------------------------------------------



void setup(){
    Serial.begin(115200);
    
    if(CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");
    CAN0.setMode(MCP_NORMAL);
   
    DDRE = 0xDF;    // pin pin 5 como input
    DDRD = 0x00;    // pines 
    PORTE = 0x00;
    PORTD = 0x00;
    //pinMode(channelPinA, INPUT);
    //pinMode(channelPinZ, INPUT);
    //pinMode(pinBomba,    INPUT);              // poner INPUT_PULLUP
    //digitalWrite(pinBomba, HIGH);             //Pull-up
    
    attachInterrupt(digitalPinToInterrupt(pinBomba),    INT_Bomba,  RISING);
    attachInterrupt(digitalPinToInterrupt(channelPinB), doEncode,   RISING);
    attachInterrupt(digitalPinToInterrupt(channelPinZ), doZero,     RISING);
    
    Serial.println(myPosition);

    bContinuousTx = 0;
}

void loop(){
    if ((millis() - tantSign) >= tSignTask){
      tantSign = millis();
      if (counter != ISRCounter){        // Se hace algoritmo si hay cambio en encoder
          if(ISRCounter >= 0){
            myPosition = ISRCounter;
              if (ISRCounter > maxSteps-1){
                  ISRCounter = 0;
                  myPosition = 0;
              }
          }
          if(ISRCounter < 0){
              myPosition = 3600 - abs(ISRCounter);
              if(ISRCounter < -maxSteps + 1){
                  ISRCounter = 0;
                  myPosition = 0;
              }
          }
          
          
          
          //Serial.print(angulo);
          if(bContinuousTx){
              Serial.print(myPosition);
              Serial.print("\t");
              if(IsCW  &&  ZERO)  Serial.println("\tCW \tZERO");  
              if(IsCW  && !ZERO)  Serial.println("\tCW");
              if(!IsCW &&  ZERO)  Serial.println("\tCCW \tZERO");
              if(!IsCW && !ZERO)  Serial.println("\tCCW");
          }else{
              //Serial.print("Angulo bomba: ");
              //Serial.println(posBomba);
          }
        }
        ZERO = false;
        counter = ISRCounter;
  
  
        /*--------------- TX ------------------------------------*/
        if (bContinuousTx){
            if ((millis() - tant) >= tTXMsg){
                tant = millis();
                
                //Serial.println("SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS");
                data[posEncoderL] = (myPosition & 0x000000FF);                    // posEncoderL = 0
                data[posEncoderH] = (myPosition & 0x0000FF00) >> 8;               // posEncoderH = 1
                CAN0.sendMsgBuf(CAN_ID_TX, 0, 8, data);
                
              }
          } 
  
        if (!digitalRead(CAN0_INT)){
            CAN0.readMsgBuf(&rxID, &len, rxBuf);  
            
            if(rxID == CAN_ID_RX){              
                digitalWrite(OUT_DIVIDER, rxBuf[posDivider]);     // posDivider = 1
                digitalWrite(OUT_TRIGGER, rxBuf[posAbs]);         // posAbs = 0

                if(rxBuf[posBloqueo] == 1){            //posBloqueo = 2
                    detachInterrupt(digitalPinToInterrupt(pinBomba));
                    //detachInterrupt(digitalPinToInterrupt(channelPinZ));
                    bContinuousTx = 1;
                    //Serial.println("posBloqueo = 1");
                }else{
                    attachInterrupt(digitalPinToInterrupt(pinBomba), INT_Bomba, RISING);
                    attachInterrupt(digitalPinToInterrupt(channelPinZ), doZero, RISING);
                    bContinuousTx = 0;
                }
            }
        }
    }        
}

void doEncode(){
    if (digitalRead(channelPinB) == digitalRead(channelPinA)){
        IsCW = true;
        ISRCounter++;
        angulo++;
    }else{
        IsCW = false;
        ISRCounter--;
        angulo--;
    }
}


void doZero(){      // cada paso por zero se reinicia el contador
    ZERO = true;
    aux  = true;
    ISRCounter = 0;
    myPosition = 0;
    angulo = 0;
}

void INT_Bomba(){     
     if (aux == true){
        
        if (ISRCounter < 0){
            posBomba = abs(angulo);
        }else{
            posBomba = abs(angulo);
        }
        
        Serial.print("Angulo bomba: ");
        Serial.println(posBomba);
        data[posEncoderL] = (posBomba & 0x000000FF);       // posEncoderL = 0
        data[posEncoderH] = (posBomba & 0x0000FF00) >> 8;  // posEncoderH = 1
        CAN0.sendMsgBuf(CAN_ID_TX, 0, 8, data);
        aux = false;
    }
}

To me it appears you have run out of bandwidth (not fast enough) with your mega. Have you tried using one of the many timers as a counter, it will accumulate the counts in hardware and reduce your interrupt load dramatically.

consider using a microcontroller with a hardware quadrature decoder such as the Arduino Due

But are not the timers used to count time with the 16MHz clock as base? I mean, how can I use an external signal as trigger of a counter with a timer that is based on the internal clock of arduino?

The Arduino Uno's MCU has 2 independent uncommitted timers, each one can have its own external clock. In the UNO there are two free, one is a 8-bit timer and the other is a 16-bit timer. Arduino Mega has more. Klausj posted this in response to a question, it may help: Counting pulses in the background while the main program is running - #14 by Klausj What you do is use the external input to the timer/counter to do the count accumulation for you, then check it at a given point or by interrupt. This dramatically decreases the interupt overhead.