#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
);
#define MPU 0x68
float gyroOffset = 0;
float heading = 0;
unsigned long lastTime;
const float PI2 = 6.2831853;
void setup() {
Wire.begin();
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
while (1);
}
// Wake up MPU6050
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission();
// Gyroscope range +/- 250 degrees/sec
Wire.beginTransmission(MPU);
Wire.write(0x1B);
Wire.write(0);
Wire.endTransmission();
display.clearDisplay();
display.setTextColor(SSD1306_WHITE);
display.setTextSize(1);
display.setCursor(20, 25);
display.println(F("Calibrating..."));
display.display();
long total = 0;
for (int i = 0; i < 500; i++) {
Wire.beginTransmission(MPU);
Wire.write(0x47);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 2, true);
if (Wire.available() >= 2) {
int16_t gz = (int16_t)(
(Wire.read() << 8) | Wire.read()
);
total += gz;
}
delay(5);
}
gyroOffset = total / 500.0;
lastTime = micros();
}
void drawCompass(float angle) {
display.clearDisplay();
const int cx = 64;
const int cy = 31;
const int radius = 23;
// Circular dial
display.drawCircle(cx, cy, radius, SSD1306_WHITE);
// Compass markings
for (int deg = 0; deg < 360; deg += 10) {
float a = deg * PI / 180.0;
int r1 = (deg % 30 == 0) ? radius - 6 : radius - 3;
int r2 = radius;
int x1 = cx + sin(a) * r1;
int y1 = cy - cos(a) * r1;
int x2 = cx + sin(a) * r2;
int y2 = cy - cos(a) * r2;
display.drawLine(x1, y1, x2, y2, SSD1306_WHITE);
}
// Direction labels
display.setTextSize(1);
display.setCursor(61, 1);
display.print(F("N"));
display.setCursor(61, 56);
display.print(F("S"));
display.setCursor(28, 31);
display.print(F("W"));
display.setCursor(95, 31);
display.print(F("E"));
// Center angle
display.setTextSize(1);
display.setCursor(55, 27);
if (angle < 100) display.print(' ');
display.print((int)angle);
display.print((char)247);
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 = (int16_t)(
(Wire.read() << 8) | Wire.read()
);
unsigned long now = micros();
float dt = (now - lastTime) / 1000000.0;
lastTime = now;
if (dt > 0 && dt < 0.2) {
float rate = (gz - gyroOffset) / 131.0;
heading += rate * dt;
}
while (heading >= 360) heading -= 360;
while (heading < 0) heading += 360;
drawCompass(heading);
delay(40);
}