-
Notifications
You must be signed in to change notification settings - Fork 0
/
connecting_bno055.ino
77 lines (61 loc) · 1.75 KB
/
connecting_bno055.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
#include<math.h>
#include<string.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
TFT_eSPI tft = TFT_eSPI(); // Invoke library, pins defined in User_Setup.h
Adafruit_BNO055 bno = Adafruit_BNO055(55);
const uint8_t LOOP_PERIOD = 10; //milliseconds
uint32_t primary_timer = 0;
float x,y,z; //variables for grabbing x,y,and z values
float current[3];
float previous[3];
float before_previous[3];
float average[3];
void setup(void)
{
Serial.begin(115200); //for debugging if needed.
delay(50); //pause to make sure comms get set up
Wire.begin();
delay(50); //pause to make sure comms get set up
Serial.println("Orientation Sensor Test"); Serial.println("");
/* Initialise the sensor */
if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
delay(1000);
bno.setExtCrystalUse(true);
for (int i=0; i<3; i++){
current[i] = 0;
previous[i] = 0;
before_previous[i] = 0;
average[i] = 0;
}
}
void loop(void)
{
imu::Vector<3> accelerometer = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
current[0] = accelerometer.x();
current[1] = accelerometer.y();
current[2] = accelerometer.z();
for (int i=0; i<3; i++){
average[i] = (current[i] + previous[i] + before_previous[i])/3;
}
/* Display the floating point data */
Serial.print("X: ");
Serial.print(average[0]);
Serial.print(" Y: ");
Serial.print(average[1]);
Serial.print(" Z: ");
Serial.print(average[2]);
Serial.println("");
for (int i=0; i<3; i++){
before_previous[i] = previous[i];
previous[i] = current[i];
}
delay(100);
}