#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(
SCREEN_WIDTH,
SCREEN_HEIGHT,
&Wire,
OLED_RESET
);
const int MPU = 0x68;
float gyroZOffset = 0;
float heading = 0;
unsigned long lastTime = 0;
void setup() {
Wire.begin();
Serial.begin(9600);
if (!display.begin(
SSD1306_SWITCHCAPVCC, 0x3C)) {
while (1);
}
display.clearDisplay();
display.setTextColor(SSD1306_WHITE);
// Wake up MPU6050
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
// Set gyro range to +/- 250 degrees/sec
Wire.beginTransmission(MPU);
Wire.write(0x1B);
Wire.write(0x00);
Wire.endTransmission();
display.setTextSize(1);
display.setCursor(10, 20);
display.println("MPU6050 Compass");
display.setCursor(10, 40);
display.println("Calibrating...");
display.display();
// Gyroscope calibration
long sum = 0;
for (int i = 0; i < 1000; i++) {
Wire.beginTransmission(MPU);
Wire.write(0x47);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 2, true);
int16_t gz = Wire.read() << 8 | Wire.read();
sum += gz;
delay(3);
}
gyroZOffset = sum / 1000.0;
lastTime = micros();
display.clearDisplay();
display.display();
}
void loop() {
Wire.beginTransmission(MPU);
Wire.write(0x47);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 2, true);
if (Wire.available() < 2) {
return;
}
int16_t gz = Wire.read() << 8 | Wire.read();
unsigned long now = micros();
float dt = (now - lastTime) / 1000000.0;
lastTime = now;
// Gyroscope sensitivity at +/-250 degrees/sec
float gyroRate = (gz - gyroZOffset) / 131.0;
// Integrate rotation
heading += gyroRate * dt;
// Keep angle between 0 and 359
while (heading >= 360) heading -= 360;
while (heading < 0) heading += 360;
const char* direction;
if (heading >= 315 || heading < 45) {
direction = "N";
} else if (heading < 135) {
direction = "E";
} else if (heading < 225) {
direction = "S";
} else {
direction = "W";
}
display.clearDisplay();
display.setTextSize(1);
display.setCursor(20, 0);
display.println("MPU6050 COMPASS");
display.drawLine(0, 12, 127, 12, SSD1306_WHITE);
display.setTextSize(3);
display.setCursor(48, 18);
display.println(direction);
display.setTextSize(2);
display.setCursor(25, 46);
display.print(heading, 1);
display.print((char)247);
display.display();
delay(50);
}