CAN BUS Shield V2 reception Issue

[code]
// include libraries
#include <mcp_can.h>
#include <mcp_can_dfs.h>

//---------------------------
//
// Variable initialisation
//
//---------------------------
#define VAR_TURN 1350
#define NEUTRAL 40
#define ACT_VAR 500
#define MAX_COMMAND 220 //90% of 255
#define MIN_COMMAND 30 //10% of 255
#define MAX_SPEED 2500
#define TIME_CAN 2
#define TIME_PROCESS 50
#define CURRENT_SAMPLE 10
#define MAX_CURRENT 400
#define VAR_CORRECT 1
#define VAR_SLOPE 10
#define SPI_CS_CAN 9
#define CORRECT_LEVER_R -150 // correction of the lever position
#define CORRECT_LEVER_L -80 // correction of the lever position

#define Codid_Epos_R 0x602 // CAN address of the EPOS right
#define Codid_Epos_L 0x603 // CAN address of the EPOS left
#define codid_rec_r 0x582 // CAN address for answer of the EPOS right
#define codid_rec_l 0x583 // CAN address for answer of the EPOS left

int current_r[CURRENT_SAMPLE];
int current_l[CURRENT_SAMPLE];
int speed_r = 0;
int speed_l = 0;
int pot_r = 0;
int pwr_r = 128;
int var_r = 0;
int correct_var_r = 0;
int pot_l = 0;
int pwr_l = 128;
int var_l = 0;
int correct_var_l = 0;

int port_pot_r = A0; //A0 input pin for pot_r
int port_pot_l = A1; //A1 input pin for pot_l
int port_kelly_ana_r = 5; //D5 output pin for kelly ana r
int port_kelly_ana_l = 6; //D6 output pin for kelly ana l
//int port_kelly_rev_r = 4; //D4 output pin for kelly rev r
int port_kelly_rev_l = 7; //D7 output pin for kelly rev l
int port_kelly_pwr = 4; //D12 output pin for kelly pwr
int port_switch_seat = 3; //D3 input pin for seat switch
int port_switch_brake = 2; //D2 input pin for brake switch

bool seat = 0; // state of the seat
bool brake = 0; // state of the break
bool stop_r = 1; // EPOS right is disable
bool stop_l = 1; // EPOS left is disable
bool start_secure = 1; //Security to avoid starting outside the neutral area
bool homing = 0;
bool dir_r = 0;
bool dir_l = 0;
bool message_kelly_r = 0; // status of the reception from kelly controler
bool message_kelly_l = 0; // status of the reception from kelly controler

unsigned char len = 0;
byte buf[8];
unsigned char canframe[8] = {0, 0, 0, 0, 0, 0, 0, 0};
unsigned long canId = 0;

unsigned char data_low_right = 0;
unsigned char data_low_left = 0;
unsigned char status_right = 0;
unsigned char status_left = 0;
unsigned char error_r = 0;
unsigned char error_l = 0;
unsigned int connection_test = 0;

MCP_CAN CAN(SPI_CS_CAN); // Set CS pin

void setup() {
// put your setup code here, to run once:
//Port Initialisation
pinMode(port_pot_r, INPUT);
pinMode(port_pot_l, INPUT);
pinMode(port_switch_seat, INPUT_PULLUP);
pinMode(port_switch_brake, INPUT_PULLUP);
pinMode(port_kelly_ana_r, OUTPUT);
pinMode(port_kelly_rev_l, OUTPUT);
pinMode(port_kelly_pwr, OUTPUT);

digitalWrite(port_kelly_pwr, LOW); //Disable the kellys

// send command to the motors
analogWrite(port_kelly_ana_r, pwr_r);
analogWrite(port_kelly_ana_l, pwr_l);

//init of the CAN
Serial.begin(115200);

while (CAN_OK != CAN.begin(CAN_250KBPS)) // init can bus : baudrate = 125k
{
Serial.println(“CAN BUS Shield init fail”);
Serial.println(" Init CAN BUS Shield again");
delay(100);
}
Serial.println(“CAN BUS Shield init ok!”);

// set mask, set both the mask to 0x3ff
CAN.init_Mask(0, 0, 0x7FF); // Mask 0 for filter 0 & 1 allows all ID from 0 to 7
CAN.init_Mask(1, 1, 0x1FFFFFFE); // mask 1 for filter 2 - 4

// set filter, we can receive id from 0x04 ~ 0x09

CAN.init_Filt(0, 0, 0x582); // there are 6 filter in mcp2515
CAN.init_Filt(1, 0, 0x583); // there are 6 filter in mcp2515
CAN.init_Filt(2, 1, 0x00); // there are 6 filter in mcp2515
CAN.init_Filt(3, 1, 0x00); // there are 6 filter in mcp2515
}

void loop() {
// put your main code here, to run repeatedly:
//--------------------------
//
// Checking Epos
//
//--------------------------

// check the status of the EPOS2 right (read statusword at 0x6041 subindex 0x00)
canframe[0] = 0x40;
canframe[1] = 0x41;
canframe[2] = 0x60;
canframe[3] = 0x00;
canframe[4] = 0x00;
canframe[5] = 0x00;
canframe[6] = 0x00;
canframe[7] = 0x00;
CAN.sendMsgBuf(Codid_Epos_R, 0, 4, canframe);
do {
delay(TIME_CAN);
connection_test++;
if (connection_test > 10) { //resend command every 10 cycles
delay(1000);
CAN.sendMsgBuf(Codid_Epos_R, 0, 4, canframe);
connection_test = 0;
Serial.println(“EPOS RIGHT NOT CONECTED … \r”);
}
} while (CAN_MSGAVAIL != CAN.checkReceive());
connection_test = 0;
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf

data_low_right = buf[4];
if (data_low_right == 0 ) {
Serial.println(“EPOS RIGHT NOT CONECTED … \r”);
}

// check the status of the EPOS2 left (read statusword at 0x6041 subindex 0x00)
CAN.sendMsgBuf(Codid_Epos_L, 0, 4, canframe);
do {
delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf

data_low_left = buf[4];
if (data_low_left == 0 ) {
Serial.println(“EPOS LEFT NOT CONECTED … \r”);
}

canframe[0] = 0x2F;
canframe[1] = 0x60;
canframe[2] = 0x60;
canframe[3] = 0x00;
canframe[4] = 0xFF;
canframe[5] = 0x00;
canframe[6] = 0x00;
canframe[7] = 0x00;
CAN.sendMsgBuf(Codid_Epos_R, 0, 5, canframe);
do {
  delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

CAN.sendMsgBuf(Codid_Epos_L, 0, 5, canframe);
do {
  delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

canframe[0] = 0x40;
canframe[1] = 0x01;
canframe[2] = 0x10;
canframe[3] = 0x00;
canframe[4] = 0x00;
canframe[5] = 0x00;
canframe[6] = 0x00;
canframe[7] = 0x00;
CAN.sendMsgBuf(Codid_Epos_R, 0, 4, canframe);
do {
  delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

error_r = buf[4];

CAN.sendMsgBuf(Codid_Epos_L, 0, 4, canframe);
do {
  delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

error_l = buf[4];

    digitalWrite(port_kelly_pwr, HIGH);                        //enable the kellys

    //reset status of kellys message
    message_kelly_r = 0;
    message_kelly_l = 0;

    //read speed and current of the motor on kellys controllers
    CAN.init_Filt(2, 1, 0x0CF11E11);                    // set filter for right kelly
    delay (50);

    while (!message_kelly_r || !message_kelly_l) {
      do {
        delay(TIME_CAN);
        connection_test++;
        if (connection_test > 1000) {
          // digitalWrite(port_kelly_pwr, LOW);
          connection_test = 0;
          Serial.println("Message Bloqued");
        }
      } while (CAN_MSGAVAIL != CAN.checkReceive());
      CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf
      connection_test = 0;

      canId = CAN.getCanId();

      switch (canId) {
        case (0x0CF11E10):
          speed_r = buf[0] + 256 * buf[1];
          current_r[0] = buf[2] + 256 * buf[3];
          message_kelly_r = 1;
          Serial.println("message R OK... \r");
          break;
        case (0x0CF11E11):
          speed_l = buf[0] + 256 * buf[1];
          current_l[0] = buf[2] + 256 * buf[3];
          message_kelly_l = 1;
          Serial.println("message L OK ... \r");
          break;
      }
    }
    // disable message from kellys

    CAN.init_Filt(2, 1, 0x0000000);                    // there are 6 filter in mcp2515

}
[/code]