Hello,
So I am building a project where an OLED display can print the angles of yaw, pitch, and roll using data from the Adafruti BNO08x. Using the example codes, I could print out the angles on the serial monitor and the OLED display. However, this will only work when I connect it using a cable. Without the cable, it won't show the yaw, pitch, and roll onto the OLED display. I will show my code here, but to my understanding, I think my code is getting stuck somewhere, preventing the printing on the OLED from happening. Can someone help?
Here is my code:
#include <Arduino.h>
// This demo explores two reports (SH2_ARVR_STABILIZED_RV and SH2_GYRO_INTEGRATED_RV) both can be used to give
// quartenion and euler (yaw, pitch roll) angles. Toggle the FAST_MODE define to see other report.
// Note sensorValue.status gives calibration accuracy (which improves over time)
#include <Adafruit_BNO08x.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// For SPI mode, we need a CS pin
#define BNO08X_CS 36
#define BNO08X_INT 32
// #define FAST_MODE
// For SPI mode, we also need a RESET
#define BNO08X_RESET 34
// but not for I2C or UART
// #define BNO08X_RESET -1
struct euler_t {
float yaw;
float pitch;
float roll;
} ypr;
Adafruit_BNO08x bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;
#ifdef FAST_MODE
// Top frequency is reported to be 1000Hz (but freq is somewhat variable)
sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;
long reportIntervalUs = 2000;
#else
// Top frequency is about 250Hz but this report is more accurate
sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV;
long reportIntervalUs = 5000;
#endif
void setReports(sh2_SensorId_t reportType, long report_interval) {
Serial.println("Setting desired reports");
if (! bno08x.enableReport(reportType, report_interval)) {
Serial.println("Could not enable stabilized remote vector");
}
}
void setup(void) {
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
Serial.println(F("SSD1306 allocation failed"));
for (;;);
}
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println(F("Welcome ShivPat!"));
display.println(F("Code will run shortly..."));
display.display();
delay(5000);
while (!Serial) delay(10); // will pause Zero, Leonardo, etc until serial console opens
Serial.println("Adafruit BNO08x test!");
// Try to initialize!
// if (!bno08x.begin_I2C()) {
// if (!bno08x.begin_UART(&Serial1)) { // Requires a device with > 300 byte UART buffer!
if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) {
Serial.println("Failed to find BNO08x chip");
while (1) { delay(10); }
}
Serial.println("BNO08x Found!");
setReports(reportType, reportIntervalUs);
Serial.println("Reading events");
delay(100);
}
void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {
float sqr = sq(qr);
float sqi = sq(qi);
float sqj = sq(qj);
float sqk = sq(qk);
ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));
if (degrees) {
ypr->yaw *= RAD_TO_DEG;
ypr->pitch *= RAD_TO_DEG;
ypr->roll *= RAD_TO_DEG;
}
}
void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}
void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {
quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}
void loop() {
if (bno08x.wasReset()) {
Serial.print("sensor was reset ");
setReports(reportType, reportIntervalUs);
}
if (bno08x.getSensorEvent(&sensorValue)) {
// in this demo only one report type will be received depending on FAST_MODE define (above)
switch (sensorValue.sensorId) {
case SH2_ARVR_STABILIZED_RV:
quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, true);
case SH2_GYRO_INTEGRATED_RV:
// faster (more noise?)
quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true);
break;
}
static long last = 0;
long now = micros();
Serial.print(now - last); Serial.print("\t");
last = now;
Serial.print(sensorValue.status); Serial.print("\t"); // This is accuracy in the range of 0 to 3
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print(F(" Yaw: "));
display.println(ypr.yaw);
display.print(F(" Pitch: "));
display.println(ypr.pitch);
display.print(F(" Roll: "));
display.println(ypr.roll);
display.display();
Serial.print(ypr.yaw); Serial.print("\t");
Serial.print(ypr.pitch); Serial.print("\t");
Serial.println(ypr.roll);
}
}
