BRIX5

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

double xPos = 0, yPos = 0, headingVel = 0;

uint16_t BNO055_SAMPLERATE_DELAY_MS = 10; //how often to read data from the board

uint16_t PRINT_DELAY_MS = 500; // how often to print the data

uint16_t printCount = 0; //counter to avoid printing every 10MS sample

//velocity = accel*dt (dt in seconds)

//position = 0.5*accel*dt^2

double ACCEL_VEL_TRANSITION =  (double)(BNO055_SAMPLERATE_DELAY_MS) / 1000.0;

double ACCEL_POS_TRANSITION = 0.5 * ACCEL_VEL_TRANSITION * ACCEL_VEL_TRANSITION;

double DEG_2_RAD = 0.01745329251; //trig functions require radians, BNO055 outputs degrees

Adafruit_BNO055 bno = Adafruit_BNO055(55);

void setup(void)

{

  Serial.begin(115200);

  if (!bno.begin())

  {

    Serial.print("No BNO055 detected");

    while (1);

  }

  delay(1000);

}

void loop(void)

{

  //

  unsigned long tStart = micros();

  sensors_event_t orientationData , linearAccelData;

  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);

  //  bno.getEvent(&angVelData, Adafruit_BNO055::VECTOR_GYROSCOPE);

  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);

  xPos = xPos + ACCEL_POS_TRANSITION * linearAccelData.acceleration.x;

  yPos = yPos + ACCEL_POS_TRANSITION * linearAccelData.acceleration.y;

  // velocity of sensor in the direction it's facing

  headingVel = ACCEL_VEL_TRANSITION * linearAccelData.acceleration.x / cos(DEG_2_RAD * orientationData.orientation.x);

  if (printCount * BNO055_SAMPLERATE_DELAY_MS >= PRINT_DELAY_MS) {

    //enough iterations have passed that we can print the latest data

    Serial.print("Heading: ");

    Serial.println(orientationData.orientation.x);

    Serial.print("Position: ");

    Serial.print(xPos);

    Serial.print(" , ");

    Serial.println(yPos);

    Serial.print("Speed: ");

    Serial.println(headingVel);

    Serial.println("-------");

    printCount = 0;

  }

  else {

    printCount = printCount + 1;

  }

  while ((micros() - tStart) < (BNO055_SAMPLERATE_DELAY_MS * 1000))

  {

    //poll until the next sample is ready

  }

}

void printEvent(sensors_event_t* event) {

  Serial.println();

  Serial.print(event->type);

  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem

  if (event->type == SENSOR_TYPE_ACCELEROMETER) {

    x = event->acceleration.x;

    y = event->acceleration.y;

    z = event->acceleration.z;

  }

  else if (event->type == SENSOR_TYPE_ORIENTATION) {

    x = event->orientation.x;

    y = event->orientation.y;

    z = event->orientation.z;

  }

  else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {

    x = event->magnetic.x;

    y = event->magnetic.y;

    z = event->magnetic.z;

  }

  else if ((event->type == SENSOR_TYPE_GYROSCOPE) || (event->type == SENSOR_TYPE_ROTATION_VECTOR)) {

    x = event->gyro.x;

    y = event->gyro.y;

    z = event->gyro.z;

  }

  Serial.print(": x= ");

  Serial.print(x);

  Serial.print(" | y= ");

  Serial.print(y);

  Serial.print(" | z= ");

  Serial.println(z);

}