Arduino BNO055 +Adafruit_SSD1306

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeMono9pt7b.h>

/*
TIP: in Adafruit_BNO055.h ho dovuto invertire i valori di:
BNO055_ADDRESS_B e BNO055_ADDRESS_A (0x29)
Vedi: https://forum.arduino.cc/index.php?topic=338503.0
*/

// Classi per gestire il display e il sensore BNO055
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, & Wire);#
define BNO055_SAMPLERATE_DELAY_MS(100)
Adafruit_BNO055 bno = Adafruit_BNO055();

// Variabili globali
#
define ROW_HEIGHT 16# define COL_WIDTH 64
int x = 0;

void setup() {

  Serial.begin(9600);
  Serial.print("Hello world");

  // Inizializzo il display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Address 0x3C for 128x32
  display.display();
  Serial.print("Display inizializzato");

  // Inizializzo il sensore BNO055
  if (!bno.begin()) {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Non ho rilevato il sensore BNO055. Controlla il collegamento o l'indirizzo I2C.");
    while (1);
  } else {
    Serial.print("Sensore BNO055 inizializzato");
  }

  // Impostazioni del testo
  //display.setFont(&FreeMono9pt7b);
  display.setTextColor(WHITE);

  // Test: rilevo la temperatura
  int8_t temp = bno.getTemp();
  char temperatureString[32];
  sprintf(temperatureString, "Temperatura: %d", temp);
  Serial.println(temperatureString);

  // Mostro la temperatura.
  display.clearDisplay();
  display.display();
  display.setTextSize(2);
  display.println("\nScarso!");
  //display.println(temperatureString);
  display.setCursor(0, 0);
  display.display();

  delay(2000);
}

void loop() {
  // put your main code here, to run repeatedly:

  //delay(1000);
  char string[16];

  //display init
  display.clearDisplay();

  x++;

  if (x < 100) {

    display.setTextSize(1);
    // Possible vector values can be:
    // - VECTOR_ACCELEROMETER - m/s^2
    // - VECTOR_MAGNETOMETER  - uT
    // - VECTOR_GYROSCOPE     - rad/s
    // - VECTOR_EULER         - degrees
    // - VECTOR_LINEARACCEL   - m/s^2
    // - VECTOR_GRAVITY       - m/s^2

    imu::Vector < 3 > euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
    display.setCursor(0 * COL_WIDTH, 0 * ROW_HEIGHT);
    sprintf(string, "X: %.2f", euler.x());
    display.print(string);
    display.setCursor(0 * COL_WIDTH, 1 * ROW_HEIGHT);
    sprintf(string, "Y: %.2f", euler.y());
    display.print(string);
    display.setCursor(0 * COL_WIDTH, 2 * ROW_HEIGHT);
    sprintf(string, "Z: %.2f", euler.z());
    display.print(string);

    imu::Vector < 3 > gravity = bno.getVector(Adafruit_BNO055::VECTOR_GRAVITY);
    display.setCursor(1 * COL_WIDTH, 0 * ROW_HEIGHT);
    sprintf(string, "A: %.2f", gravity.x());
    display.print(string);
    display.setCursor(1 * COL_WIDTH, 1 * ROW_HEIGHT);
    sprintf(string, "B: %.2f", gravity.y());
    display.print(string);
    display.setCursor(1 * COL_WIDTH, 2 * ROW_HEIGHT);
    sprintf(string, "C: %.2f", gravity.z());
    display.print(string);

    // counter
    sprintf(string, "ticks: %d", x);
    display.setCursor(0 * COL_WIDTH, 3 * ROW_HEIGHT);
    display.print(string);
    display.display(); // actually display all of the above

  } else if (x < 120) {

    display.setTextSize(2);
    display.setCursor(0 * COL_WIDTH, 1 * ROW_HEIGHT);
    display.print("Guarda\nquesto");
    display.display(); // actually display all of the above

  } else {

    // drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color),
    display.setCursor(0, 0);
    float G = 9.81;
    float RAD = 31;
    float X0 = 64, Y0 = 32;
    display.drawCircle(X0, Y0, RAD, WHITE);

    imu::Vector < 3 > gravity = bno.getVector(Adafruit_BNO055::VECTOR_GRAVITY);
    float x = gravity.y() * (RAD / G);
    float y = gravity.z() * (RAD / G);
    display.fillCircle(X0 + x, Y0 + y, 8, WHITE);

    display.display(); // actually display all of the above
  }

  delay(BNO055_SAMPLERATE_DELAY_MS);
}