Return to Answer

// CAN Receive Example
//

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
uint8_t receivedFirstFrame;

char msgString[128];                        // Array to store serial string
uint32_t TimeMicro;
int16_t AccX, AccY, AccZ, GyX, GyY, GyZ;

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(10);   // Set CS to pin 10


void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    Serial.println("MCP2515 Library Receive Example...");
    receivedFirstFrame = 0;
}

void loop()
{
    if(!digitalRead(CAN0_INT))
    {
        // Received a message
        if (CAN0.readMsgBuf(&rxId, &len, rxBuf) == CAN_OK)
        {
            // Note: the above line is not like in the examples, but looking at the code the
            // function may return if no message was in the queue, so I think this is safer
        
            // Let's check if it is one of the known messages
            switch (rxId)
            {
            case 0x100:
                { // Received first frame
                    // Decode the content and store that the first frame was received
                    decodeFirstFrame(rxBuf);
                    receivedFirstFrame = 1;
                }
                break;
            case 0x101:
                if (receivedFirstFrame)
                { // Received second frame after a first frame
                    // Decode the content
                    decodeSecondFrame(rxBuf);
                    
                    printDataToSerial();
                    
                    receivedFirstFrame = 0;
                }
                break;
            }
        }
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================

void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL); // Change to normal mode to allow messages to be transmitted
    
    pinMode(CAN0_INT, INPUT); // Configuring pin for /INT input
}

void decodeFirstFrame(uint8_t *buffer)
{
    TimeMicro = byte2Double(TimeMicro, 0, buffer);
    AccX = byte2Int(AccX, 4, buffer);
    AccY = byte2Int(AccY, 6, buffer);
}

void decodeSecondFrame(uint8_t *buffer)
{
    AccZ = byte2Int(AccZ, 0, buffer);
    GyX = byte2Int(GyX, 2, buffer);
    GyY = byte2Int(GyY, 4, buffer);
    GyZ = byte2Int(GyZ, 6, buffer);
}

void printDataToSerial()
{
    Serial.print(TimeMicro);
    Serial.print("\t");
    Serial.print(AccX);
    Serial.print("\t");
    Serial.print(AccY);
    Serial.print("\t");
    Serial.print(AccZ);
    Serial.print("\t");
    Serial.print(GyX);
    Serial.print("\t");
    Serial.print(GyY);
    Serial.print("\t");
    Serial.println(GyZ);
}

uint32_t byte2Double(uint32_t output, int pos, uint8_t *data)
{
    for (int i = 0; i < 4; i++)
    {
      output += ((uint32_t)data[i + pos]) << (i * 8);
    }
    return output;
}

int16_t byte2Int(int16_t output, int pos, uint8_t *data)
{
    for (int i = 0; i < 2; i++)
    {
      output += ((int16_t)data[i + pos]) << (i * 8);
    }
    return output;
}
/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Receive Example
//

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
uint8_t receivedFirstFrame;

char msgString[128];                        // Array to store serial string
uint32_t TimeMicro;
int16_t AccX, AccY, AccZ, GyX, GyY, GyZ;

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(10);   // Set CS to pin 10


void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    Serial.println("MCP2515 Library Receive Example...");
    receivedFirstFrame = 0;
}

void loop()
{
    if(!digitalRead(CAN0_INT))
    {
        // Received a message
        if (CAN0.readMsgBuf(&rxId, &len, rxBuf) == CAN_OK)
        {
            // Note: the above line is not like in the examples, but looking at the code the
            // function may return if no message was in the queue, so I think this is safer
        
            // Let's check if it is one of the known messages
            switch (rxId)
            {
            case 0x100:
                { // Received first frame
                    // Decode the content and store that the first frame was received
                    decodeFirstFrame(rxBuf);
                    receivedFirstFrame = 1;
                }
                break;
            case 0x101:
                if (receivedFirstFrame)
                { // Received second frame after a first frame
                    // Decode the content
                    decodeSecondFrame(rxBuf);
                    
                    printDataToSerial();
                    
                    receivedFirstFrame = 0;
                }
                break;
            }
        }
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================

void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL); // Change to normal mode to allow messages to be transmitted
    
    pinMode(CAN0_INT, INPUT); // Configuring pin for /INT input
}

void decodeFirstFrame(uint8_t *buffer)
{
    TimeMicro = byte2Double(0, buffer);
    AccX = byte2Int(4, buffer);
    AccY = byte2Int(6, buffer);
}

void decodeSecondFrame(uint8_t *buffer)
{
    AccZ = byte2Int(0, buffer);
    GyX = byte2Int(2, buffer);
    GyY = byte2Int(4, buffer);
    GyZ = byte2Int(6, buffer);
}

void printDataToSerial()
{
    Serial.print(TimeMicro);
    Serial.print("\t");
    Serial.print(AccX);
    Serial.print("\t");
    Serial.print(AccY);
    Serial.print("\t");
    Serial.print(AccZ);
    Serial.print("\t");
    Serial.print(GyX);
    Serial.print("\t");
    Serial.print(GyY);
    Serial.print("\t");
    Serial.println(GyZ);
}

uint32_t byte2Double(int pos, uint8_t *data)
{
    uint32_t output = 0;
    for (int i = 0; i < 4; i++)
    {
      output += ((uint32_t)data[i + pos]) << (i * 8);
    }
    return output;
}

int16_t byte2Int(int pos, uint8_t *data)
{
    int16_t output = 0;
    for (int i = 0; i < 2; i++)
    {
      output += ((int16_t)data[i + pos]) << (i * 8);
    }
    return output;
}
/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Send Example
//

#include <mcp_can.h>
#include <SPI.h>
#include <Wire.h>

const int MPU_addr = 0x68; // I2C address of the MPU-6050

uint32_t lastTimeSent;
const uint32_t intervalSendMs = 100; // Send period in milliseconds

int16_t AccX, AccY, AccZ, Tmp, GyX, GyY, GyZ; 
int8_t data1[8], data2[8];
int Switch = 7;

#define CAN0_INT 2    // Set INT to pin 2
MCP_CAN CAN0(10);     // Set CS to pin 10

void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    pinMode(Switch, OUTPUT);
    digitalWrite(Switch, HIGH);

    initializeMPU6050();
}

void loop()
{
    uint32_t currMillis = millis();
    
    if ((currMillis - lastTimeSent) >= intervalSendMs)
    {
        lastTimeSent += intervalSendMs;
        
        readFromMPU6050();

        convertDataToPackets();

        // Send the two frames; if the first sending is successful send also the second, otherwise abort
        // send data frame 1:  ID = 0x100, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
        // send data frame 2:  ID = 0x101, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
        if (CAN0.sendMsgBuf(0x100, 0, 8, data1) == CAN_OK)
            CAN0.sendMsgBuf(0x101, 0, 8, data2);
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================
void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
}

void initializeMPU6050()
{
    Wire.begin();
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x6B);  // PWR_MGMT_1 register
    Wire.write(0);     // set to zero (wakes up the MPU-6050)
    Wire.endTransmission(true);
}

void readFromMPU6050()
{
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
    AccX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    AccY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AccZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() << 8 | Wire.read();
    GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

void convertDataToPackets()
{
    long2Byte(currMillis, 0, data1);
    int2Byte(AccX, 4, data1);
    int2Byte(AccY, 6, data1);

    int2Byte(AccZ, 0, data2);
    int2Byte(GyX, 2, data2);
    int2Byte(GyY, 4, data2);
    int2Byte(GyZ, 6, data2);
}

void int2Byte(int16_t input, int pos, uint8_t *data)
{
    for (int i = 0; i < 2; i++)
    {
        data[i + pos] = ((input >> (i * 8)) & 0xff);
    }
}

void long2Byte(uint32_t input, int pos, uint8_t *data)
{
    for (int i = 0; i < 4; i++)
    {
        data[i + pos] = ((input >> (i * 8)) & 0xff);
    }
}

/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Receive Example
//

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
uint8_t receivedFirstFrame;

char msgString[128];                        // Array to store serial string
uint32_t TimeMicro;
int16_t AccX, AccY, AccZ, GyX, GyY, GyZ;

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(10);   // Set CS to pin 10


void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    Serial.println("MCP2515 Library Receive Example...");
    receivedFirstFrame = 0;
}

void loop()
{
    if(!digitalRead(CAN0_INT))
    {
        // Received a message
        if (CAN0.readMsgBuf(&rxId, &len, rxBuf) == CAN_OK)
        {
            // Note: the above line is not like in the examples, but looking at the code the
            // function may return if no message was in the queue, so I think this is safer
        
            // Let's check if it is one of the known messages
            switch (rxId)
            {
            case 0x100:
                { // Received first frame
                    // Decode the content and store that the first frame was received
                    decodeFirstFrame(rxBuf);
                    receivedFirstFrame = 1;
                }
                break;
            case 0x101:
                if (receivedFirstFrame)
                { // Received second frame after a first frame
                    // Decode the content
                    decodeSecondFrame(rxBuf);
                    
                    printDataToSerial();
                    
                    receivedFirstFrame = 0;
                }
                break;
            }
        }
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================

void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
    
    pinMode(CAN0_INT, INPUT);                            // Configuring pin for /INT input
}

void decodeFirstFrame(uint8_t *buffer)
{
    TimeMicro = byte2Double(TimeMicro, 0, buffer);
    AccX = byte2Int(AccX, 4, buffer);
    AccY = byte2Int(AccY, 6, buffer);
}

void decodeSecondFrame(uint8_t *buffer)
{
    AccZ = byte2Int(AccZ, 0, buffer);
    GyX = byte2Int(GyX, 2, buffer);
    GyY = byte2Int(GyY, 4, buffer);
    GyZ = byte2Int(GyZ, 6, buffer);
}

void printDataToSerial()
{
    Serial.print(TimeMicro);
    Serial.print("\t");
    Serial.print(AccX);
    Serial.print("\t");
    Serial.print(AccY);
    Serial.print("\t");
    Serial.print(AccZ);
    Serial.print("\t");
    Serial.print(GyX);
    Serial.print("\t");
    Serial.print(GyY);
    Serial.print("\t");
    Serial.println(GyZ);
}

uint32_t byte2Double(uint32_t output, int pos, uint8_t data[4]){
      for (int i = 0; i < 4; i++)
    {
      output += ((uint32_t)rxBuf1[i + pos]) << (i * 8);
    }
    return output;
}

int16_t byte2Int(int16_t output, int pos, uint8_t data[2]){
      for (int i = 0; i < 2; i++)
    {
      output += ((int16_t)rxBuf1[i + pos]) << (i * 8);
    }
    return output;
}
/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Send Example
//

#include <mcp_can.h>
#include <SPI.h>
#include <Wire.h>

const int MPU_addr = 0x68; // I2C address of the MPU-6050

uint32_t lastTimeSent;
uint32_t currMillis;
const uint32_t intervalSendMs = 100; // Send period in milliseconds

int16_t AccX, AccY, AccZ, Tmp, GyX, GyY, GyZ; 
uint8_t data1[8], data2[8];
int Switch = 7;

#define CAN0_INT 2    // Set INT to pin 2
MCP_CAN CAN0(10);     // Set CS to pin 10

void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    pinMode(Switch, OUTPUT);
    digitalWrite(Switch, HIGH);

    initializeMPU6050();
}

void loop()
{
    currMillis = millis();
    
    if ((currMillis - lastTimeSent) >= intervalSendMs)
    {
        lastTimeSent += intervalSendMs;
        
        readFromMPU6050();

        convertDataToPackets();

        // Send the two frames; if the first sending is successful send also the second, otherwise abort
        // send data frame 1:  ID = 0x100, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
        // send data frame 2:  ID = 0x101, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
        if (CAN0.sendMsgBuf(0x100, 0, 8, data1) == CAN_OK)
            CAN0.sendMsgBuf(0x101, 0, 8, data2);
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================
void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
}

void initializeMPU6050()
{
    Wire.begin();
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x6B);  // PWR_MGMT_1 register
    Wire.write(0);     // set to zero (wakes up the MPU-6050)
    Wire.endTransmission(true);
}

void readFromMPU6050()
{
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
    AccX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    AccY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AccZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() << 8 | Wire.read();
    GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

void convertDataToPackets()
{
    long2Byte(currMillis, 0, data1);
    int2Byte(AccX, 4, data1);
    int2Byte(AccY, 6, data1);

    int2Byte(AccZ, 0, data2);
    int2Byte(GyX, 2, data2);
    int2Byte(GyY, 4, data2);
    int2Byte(GyZ, 6, data2);
}

void int2Byte(int16_t input, int pos, uint8_t *data)
{
    for (int i = 0; i < 2; i++)
    {
        data[i + pos] = ((input >> (i * 8)) & 0xff);
    }
}

void long2Byte(uint32_t input, int pos, uint8_t *data)
{
    for (int i = 0; i < 4; i++)
    {
        data[i + pos] = ((input >> (i * 8)) & 0xff);
    }
}

/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Receive Example
//

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
uint8_t receivedFirstFrame;

char msgString[128];                        // Array to store serial string
uint32_t TimeMicro;
int16_t AccX, AccY, AccZ, GyX, GyY, GyZ;

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(10);   // Set CS to pin 10


void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    Serial.println("MCP2515 Library Receive Example...");
    receivedFirstFrame = 0;
}

void loop()
{
    if(!digitalRead(CAN0_INT))
    {
        // Received a message
        if (CAN0.readMsgBuf(&rxId, &len, rxBuf) == CAN_OK)
        {
            // Note: the above line is not like in the examples, but looking at the code the
            // function may return if no message was in the queue, so I think this is safer
        
            // Let's check if it is one of the known messages
            switch (rxId)
            {
            case 0x100:
                { // Received first frame
                    // Decode the content and store that the first frame was received
                    decodeFirstFrame(rxBuf);
                    receivedFirstFrame = 1;
                }
                break;
            case 0x101:
                if (receivedFirstFrame)
                { // Received second frame after a first frame
                    // Decode the content
                    decodeSecondFrame(rxBuf);
                    
                    printDataToSerial();
                    
                    receivedFirstFrame = 0;
                }
                break;
            }
        }
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================

void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL); // Change to normal mode to allow messages to be transmitted
    
    pinMode(CAN0_INT, INPUT); // Configuring pin for /INT input
}

void decodeFirstFrame(uint8_t *buffer)
{
    TimeMicro = byte2Double(TimeMicro, 0, buffer);
    AccX = byte2Int(AccX, 4, buffer);
    AccY = byte2Int(AccY, 6, buffer);
}

void decodeSecondFrame(uint8_t *buffer)
{
    AccZ = byte2Int(AccZ, 0, buffer);
    GyX = byte2Int(GyX, 2, buffer);
    GyY = byte2Int(GyY, 4, buffer);
    GyZ = byte2Int(GyZ, 6, buffer);
}

void printDataToSerial()
{
    Serial.print(TimeMicro);
    Serial.print("\t");
    Serial.print(AccX);
    Serial.print("\t");
    Serial.print(AccY);
    Serial.print("\t");
    Serial.print(AccZ);
    Serial.print("\t");
    Serial.print(GyX);
    Serial.print("\t");
    Serial.print(GyY);
    Serial.print("\t");
    Serial.println(GyZ);
}

uint32_t byte2Double(uint32_t output, int pos, uint8_t *data) 
{
    for (int i = 0; i < 4; i++)
    {
      output += ((uint32_t)data[i + pos]) << (i * 8);
    }
    return output;
}

int16_t byte2Int(int16_t output, int pos, uint8_t *data) 
{
    for (int i = 0; i < 2; i++)
    {
      output += ((int16_t)data[i + pos]) << (i * 8);
    }
    return output;
}
/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Receive Example
//

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
uint8_t receivedFirstFrame;

char msgString[128];                        // Array to store serial string
uint32_t TimeMicro;
int16_t AccX, AccY, AccZ, GyX, GyY, GyZ;

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(10);   // Set CS to pin 10


void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    Serial.println("MCP2515 Library Receive Example...");
    receivedFirstFrame = 0;
}

void loop()
{
    if(!digitalRead(CAN0_INT))
    {
        // Received a message
        if (CAN0.readMsgBuf(&rxId, &len, rxBuf) == CAN_OK)
        {
            // Note: the above line is not like in the examples, but looking at the code the
            // function may return if no message was in the queue, so I think this is safer
        
            // Let's check if it is one of the known messages
            switch (rxId)
            {
            case 0x100:
                { // Received first frame
                    // Decode the content and store that the first frame was received
                    decodeFirstFrame(rxBuf);
                    receivedFirstFrame = 1;
                }
                break;
            case 0x101:
                if (receivedFirstFrame)
                { // Received second frame after a first frame
                    // Decode the content
                    decodeSecondFrame(rxBuf);
                    
                    printDataToSerial();
                    
                    receivedFirstFrame = 0;
                }
                break;
            }
        }
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================

void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
    
    pinMode(CAN0_INT, INPUT);                            // Configuring pin for /INT input
}

void decodeFirstFrame(uint8_t *rxBuff)
{
    TimeMicro = byte2Double(TimeMicro, 0, rxBuf);
    AccX = byte2Int(AccX, 4, rxBuf);
    AccY = byte2Int(AccY, 6, rxBuf);
}

void decodeSecondFrame(uint8_t *rxBuff)
{
    AccZ = byte2Int(AccZ, 0, rxBuf);
    GyX = byte2Int(GyX, 2, rxBuf);
    GyY = byte2Int(GyY, 4, rxBuf);
    GyZ = byte2Int(GyZ, 6, rxBuf);
}

void printDataToSerial()
{
    Serial.print(TimeMicro);
    Serial.print("\t");
    Serial.print(AccX);
    Serial.print("\t");
    Serial.print(AccY);
    Serial.print("\t");
    Serial.print(AccZ);
    Serial.print("\t");
    Serial.print(GyX);
    Serial.print("\t");
    Serial.print(GyY);
    Serial.print("\t");
    Serial.println(GyZ);
}

uint32_t byte2Double(uint32_t output, int pos, uint8_t data[4]){
      for (int i = 0; i < 4; i++)
    {
      output += ((uint32_t)rxBuf1[i + pos]) << (i * 8);
    }
    return output;
}

int16_t byte2Int(int16_t output, int pos, uint8_t data[2]){
      for (int i = 0; i < 2; i++)
    {
      output += ((int16_t)rxBuf1[i + pos]) << (i * 8);
    }
    return output;
}
/*********************************************************************************************************
  END FILE
*********************************************************************************************************/

// CAN Receive Example
//

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
uint8_t receivedFirstFrame;

char msgString[128];                        // Array to store serial string
uint32_t TimeMicro;
int16_t AccX, AccY, AccZ, GyX, GyY, GyZ;

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(10);   // Set CS to pin 10


void setup()
{
    Serial.begin(115200);

    initializeMCP2515();

    Serial.println("MCP2515 Library Receive Example...");
    receivedFirstFrame = 0;
}

void loop()
{
    if(!digitalRead(CAN0_INT))
    {
        // Received a message
        if (CAN0.readMsgBuf(&rxId, &len, rxBuf) == CAN_OK)
        {
            // Note: the above line is not like in the examples, but looking at the code the
            // function may return if no message was in the queue, so I think this is safer
        
            // Let's check if it is one of the known messages
            switch (rxId)
            {
            case 0x100:
                { // Received first frame
                    // Decode the content and store that the first frame was received
                    decodeFirstFrame(rxBuf);
                    receivedFirstFrame = 1;
                }
                break;
            case 0x101:
                if (receivedFirstFrame)
                { // Received second frame after a first frame
                    // Decode the content
                    decodeSecondFrame(rxBuf);
                    
                    printDataToSerial();
                    
                    receivedFirstFrame = 0;
                }
                break;
            }
        }
    }
}

//===================================================================================
//                                  FUNCTIONS                                             
//===================================================================================

void initializeMCP2515()
{
    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
    if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
    
    pinMode(CAN0_INT, INPUT);                            // Configuring pin for /INT input
}

void decodeFirstFrame(uint8_t *buffer)
{
    TimeMicro = byte2Double(TimeMicro, 0, buffer);
    AccX = byte2Int(AccX, 4, buffer);
    AccY = byte2Int(AccY, 6, buffer);
}

void decodeSecondFrame(uint8_t *buffer)
{
    AccZ = byte2Int(AccZ, 0, buffer);
    GyX = byte2Int(GyX, 2, buffer);
    GyY = byte2Int(GyY, 4, buffer);
    GyZ = byte2Int(GyZ, 6, buffer);
}

void printDataToSerial()
{
    Serial.print(TimeMicro);
    Serial.print("\t");
    Serial.print(AccX);
    Serial.print("\t");
    Serial.print(AccY);
    Serial.print("\t");
    Serial.print(AccZ);
    Serial.print("\t");
    Serial.print(GyX);
    Serial.print("\t");
    Serial.print(GyY);
    Serial.print("\t");
    Serial.println(GyZ);
}

uint32_t byte2Double(uint32_t output, int pos, uint8_t data[4]){
      for (int i = 0; i < 4; i++)
    {
      output += ((uint32_t)rxBuf1[i + pos]) << (i * 8);
    }
    return output;
}

int16_t byte2Int(int16_t output, int pos, uint8_t data[2]){
      for (int i = 0; i < 2; i++)
    {
      output += ((int16_t)rxBuf1[i + pos]) << (i * 8);
    }
    return output;
}
/*********************************************************************************************************
  END FILE
*********************************************************************************************************/