CAN BUS Shield V2 reception Issue

[testasecc]

Dear All,

I am a newbie on arduino project with CAN communication so I would like to get your comments on an issue that is blocking me.

This project is a vehicule with 4 motors 2 are for the direction and 2 are for the traction.
The motors are driven by various controllers (maxon controllers for the steering and Kelly for traction system).
All the drivers are communicating with the CAN network at 250kbps and I use an Arduino nano as a master to control all the parameters.
I use the SDO protocol with standard frame to control the maxon controllers and so far everything is working quite well.
The issue is coming from the kelly controllers, they are using broadband extended frame (29bits) so the 2 controllers (CAN ID 16 and 17) send 2 frame every 50ms.
In order to avoid to received not need datas during the SDO communication, I allow the data sent by the traction controller only during a short part of the program (maybe it is not needed but I am not so confident with CAN network) with filters and mask.
And sometimes it seems that I loose some datas and I am blocked in the following program in the last while loop ( while (!message_kelly_r || !message_kelly_l)) waiting for a message reception flag.

If you have some ideas on where are my mistakes, do not hesitate to share them with me!!!!

thanks for your time

[testasecc]

Code: Select all

// 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

}