         Serial.println(F(")"));
     }
 
+    // init can bus, baudrate: 500k
+    if(CAN.begin(CAN_500KBPS) == CAN_OK) Serial.print("can init ok!!\r\n");
+    else Serial.print("Can init fail!!\r\n");
+
     // configure LED for output
     pinMode(LED_PIN, OUTPUT);
 

#include "MPU6050_9Axis_MotionApps41.h"   //MPU6050 function library
#include "mcp_can.h"    //CAN communication library

#define CAN_ID 0x00

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h

        Serial.print("\n");
        
        //Prep data to send over CAN
        ypr[0] = map(ypr[0]*100,-M_PI*100, M_PI*100, 0, 65535);
        aaReal.x = map(aaReal.x, -32768, 32768, 0, 65535);
        aaReal.y = map(aaReal.y, -32768, 32768, 0, 65535);
        aaReal.z = map(aaReal.z, -32768, 32768, 0, 65535);


        //separates value into two 8 bit sets for transmission by typecasting as an integer and shifting and bitwise &ing into 8bit int vars
        //break down Direction data
        unsigned char hiDir = ((((unsigned int) (ypr[0])) >> 8) & 0xff);
        unsigned char loDir = ((((unsigned int) (ypr[0])) >> 0) & 0xff);
        //break down X data
        unsigned char hiX = ((((unsigned int) (aaReal.x)) >> 8) & 0xff);
        unsigned char loX = ((((unsigned int) (aaReal.x)) >> 0) & 0xff);
