Statistics
| Branch: | Revision:

adafruit_bno055 / examples / position / position.ino @ a64dd7d6

History | View | Annotate | Download (3.085 KB)

1
#include <Wire.h>
2
#include <Adafruit_Sensor.h>
3
#include <Adafruit_BNO055.h>
4

    
5
double xPos = 0, yPos = 0, headingVel = 0;
6
uint16_t BNO055_SAMPLERATE_DELAY_MS = 10; //how often to read data from the board
7
uint16_t PRINT_DELAY_MS = 500; // how often to print the data
8
uint16_t printCount = 0; //counter to avoid printing every 10MS sample
9

    
10
//velocity = accel*dt (dt in seconds)
11
//position = 0.5*accel*dt^2
12
double ACCEL_VEL_TRANSITION =  (double)(BNO055_SAMPLERATE_DELAY_MS) / 1000.0;
13
double ACCEL_POS_TRANSITION = 0.5 * ACCEL_VEL_TRANSITION * ACCEL_VEL_TRANSITION;
14
double DEG_2_RAD = 0.01745329251; //trig functions require radians, BNO055 outputs degrees
15

    
16
// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
17
//                                   id, address
18
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
19

    
20
void setup(void)
21
{
22
  Serial.begin(115200);
23
  if (!bno.begin())
24
  {
25
    Serial.print("No BNO055 detected");
26
    while (1);
27
  }
28

    
29

    
30
  delay(1000);
31
}
32

    
33
void loop(void)
34
{
35
  //
36
  unsigned long tStart = micros();
37
  sensors_event_t orientationData , linearAccelData;
38
  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
39
  //  bno.getEvent(&angVelData, Adafruit_BNO055::VECTOR_GYROSCOPE);
40
  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
41

    
42
  xPos = xPos + ACCEL_POS_TRANSITION * linearAccelData.acceleration.x;
43
  yPos = yPos + ACCEL_POS_TRANSITION * linearAccelData.acceleration.y;
44

    
45
  // velocity of sensor in the direction it's facing
46
  headingVel = ACCEL_VEL_TRANSITION * linearAccelData.acceleration.x / cos(DEG_2_RAD * orientationData.orientation.x);
47

    
48
  if (printCount * BNO055_SAMPLERATE_DELAY_MS >= PRINT_DELAY_MS) {
49
    //enough iterations have passed that we can print the latest data
50
    Serial.print("Heading: ");
51
    Serial.println(orientationData.orientation.x);
52
    Serial.print("Position: ");
53
    Serial.print(xPos);
54
    Serial.print(" , ");
55
    Serial.println(yPos);
56
    Serial.print("Speed: ");
57
    Serial.println(headingVel);
58
    Serial.println("-------");
59

    
60
    printCount = 0;
61
  }
62
  else {
63
    printCount = printCount + 1;
64
  }
65

    
66

    
67

    
68
  while ((micros() - tStart) < (BNO055_SAMPLERATE_DELAY_MS * 1000))
69
  {
70
    //poll until the next sample is ready
71
  }
72
}
73

    
74
void printEvent(sensors_event_t* event) {
75
  Serial.println();
76
  Serial.print(event->type);
77
  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
78
  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
79
    x = event->acceleration.x;
80
    y = event->acceleration.y;
81
    z = event->acceleration.z;
82
  }
83
  else if (event->type == SENSOR_TYPE_ORIENTATION) {
84
    x = event->orientation.x;
85
    y = event->orientation.y;
86
    z = event->orientation.z;
87
  }
88
  else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
89
    x = event->magnetic.x;
90
    y = event->magnetic.y;
91
    z = event->magnetic.z;
92
  }
93
  else if ((event->type == SENSOR_TYPE_GYROSCOPE) || (event->type == SENSOR_TYPE_ROTATION_VECTOR)) {
94
    x = event->gyro.x;
95
    y = event->gyro.y;
96
    z = event->gyro.z;
97
  }
98

    
99
  Serial.print(": x= ");
100
  Serial.print(x);
101
  Serial.print(" | y= ");
102
  Serial.print(y);
103
  Serial.print(" | z= ");
104
  Serial.println(z);
105
}
106