#include <Wire.h> // I2C communication
#include <Adafruit_MPU6050.h> // MPU6050 sensor library
#include <Adafruit_SH110X.h> // SH1106 OLED display library
// MPU6050 sensor object
Adafruit_MPU6050 mpu;
// OLED display settings
#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_ADDR 0x3C // I2C address of SH1106 display (might be 0x3C or 0x3D)
// Create the OLED display object
Adafruit_SH1106G display = Adafruit_SH1106G(OLED_WIDTH, OLED_HEIGHT, &Wire, -1);
// Button pins
#define BUTTON_CALIBRATE 12 // Button for calibration
// Variables for angle calculation
float accX, accY, accZ;
float pitch, roll;
// Calibration offsets
float pitchOffset = 0.0;
float rollOffset = 0.0;
// Averaging variables
#define SAMPLES 8
float pitchSamples[SAMPLES];
float rollSamples[SAMPLES];
int sampleIndex = 0;
bool bufferFilled = false;
// icon bitmap (W x H pixels)
#define CAR_ICON_WIDTH 45
#define CAR_ICON_HEIGHT 22
// icon bitmap array
const unsigned char carIcon[] PROGMEM = {
0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x0f, 0xcc, 0xf8,
0x00, 0x00, 0x00, 0x3e, 0x0c, 0x1e, 0x00, 0x00, 0x00, 0x78, 0x0c, 0x07, 0x00, 0x00, 0x01, 0xf8,
0x0c, 0x03, 0x80, 0x00, 0x03, 0xfe, 0x0c, 0x01, 0xe0, 0x00, 0x07, 0xff, 0xff, 0xff, 0xfe, 0x00,
0x0f, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x0f, 0x83, 0xff, 0xff,
0x87, 0xe0, 0x0f, 0x3c, 0xff, 0xff, 0x33, 0xe0, 0x0f, 0x7e, 0xff, 0xfe, 0x7d, 0xf0, 0x0e, 0xfe,
0x7f, 0xfe, 0xfc, 0xf0, 0x0e, 0xff, 0x7f, 0xfc, 0xfe, 0xf0, 0x06, 0xfe, 0x7f, 0xfd, 0xfe, 0xe0,
0x00, 0x7e, 0x4f, 0xfc, 0xfe, 0xe0, 0x00, 0x7c, 0x00, 0x00, 0xfc, 0x00, 0x00, 0x18, 0x00, 0x00,
0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x7f, 0xff,
0xff, 0xff, 0xff, 0xf0
};
// Function prototypes
void displayCarInclinometer();
void calibrateSensor();
float getAveragePitch();
float getAverageRoll();
void drawRotatedBitmap(int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, float angle);
void drawRollGauge(int16_t x, int16_t y, int16_t width, float rollAngle);
void setup() {
// Initialize serial communication
Serial.begin(115200);
while (!Serial) delay(10); // Wait for serial port to connect
Serial.println("DHKDUDE - Car Inclinometer");
// Initialize I2C communication
Wire.begin();
// Initialize the OLED display
display.begin(OLED_ADDR, true); // Address 0x3C default
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SH110X_WHITE);
display.setCursor(0, 0);
display.println("Initializing...");
display.println("DHK DUDE");
display.println("Car Inclinometer");
display.display();
delay(2000);
// Initialize the MPU6050 sensor
Serial.println("Initializing MPU6050...");
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
display.clearDisplay();
display.setCursor(0, 0);
display.println("MPU6050 Error!");
display.println("Check connections");
display.display();
while (1) {
delay(10);
}
}
Serial.println("MPU6050 Found!");
// Set up the MPU6050 sensor
mpu.setAccelerometerRange(MPU6050_RANGE_8_G); // Set range to ±8G
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); // Set bandwidth to 21 Hz
// Initialize button
pinMode(BUTTON_CALIBRATE, INPUT_PULLUP);
// Initialize sample arrays
for (int i = 0; i < SAMPLES; i++) {
pitchSamples[i] = 0.0;
rollSamples[i] = 0.0;
}
// Show ready message
display.clearDisplay();
display.setCursor(0, 0);
display.println("MPU6050 Ready!");
display.println("Press button to calibrate");
display.display();
delay(2000);
}
void loop() {
// Check for calibration button press
if (digitalRead(BUTTON_CALIBRATE) == LOW) {
delay(50); // Debounce
if (digitalRead(BUTTON_CALIBRATE) == LOW) {
calibrateSensor();
while (digitalRead(BUTTON_CALIBRATE) == LOW) delay(10); // Wait for button release
}
}
// Get new sensor events with the readings
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
// Read accelerometer data
accX = a.acceleration.x;
accY = a.acceleration.y;
accZ = a.acceleration.z;
// Calculate pitch and roll angles in degrees
// Pitch (X-axis rotation)
pitch = atan2(-accX, sqrt(accY * accY + accZ * accZ)) * 180.0 / PI;
// Roll (Y-axis rotation)
roll = atan2(accY, accZ) * 180.0 / PI;
// Apply calibration offsets
pitch -= pitchOffset;
roll -= rollOffset;
// Add to averaging buffer
pitchSamples[sampleIndex] = pitch;
rollSamples[sampleIndex] = roll;
sampleIndex = (sampleIndex + 1) % SAMPLES;
if (sampleIndex == 0) {
bufferFilled = true;
}
// Print angles to serial monitor
Serial.print("Pitch: ");
Serial.print(getAveragePitch());
Serial.print("° Roll: ");
Serial.print(getAverageRoll());
Serial.println("°");
// Update display
displayCarInclinometer();
// Small delay
delay(50);
}
void calibrateSensor() {
display.clearDisplay();
display.setTextSize(1);
display.setCursor(0, 0);
display.println("Calibrating...");
display.println("Place on level surface");
display.display();
delay(5000); // Give user time to place device
// Take multiple readings and average them
float pitchSum = 0.0;
float rollSum = 0.0;
int numReadings = 20;
for (int i = 0; i < numReadings; i++) {
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
float p = atan2(-a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;
float r = atan2(a.acceleration.y, a.acceleration.z) * 180.0 / PI;
pitchSum += p;
rollSum += r;
delay(50);
}
// Set offsets to the average readings
pitchOffset = pitchSum / numReadings;
rollOffset = rollSum / numReadings;
// Reset averaging buffer
for (int i = 0; i < SAMPLES; i++) {
pitchSamples[i] = 0.0;
rollSamples[i] = 0.0;
}
sampleIndex = 0;
bufferFilled = false;
// Show calibration complete message
display.clearDisplay();
display.setCursor(0, 0);
display.println("Calibration complete!");
display.println("Offsets:");
display.print("Pitch: ");
display.println(pitchOffset, 1);
display.print("Roll: ");
display.println(rollOffset, 1);
display.display();
delay(5000);
}
float getAveragePitch() {
if (!bufferFilled && sampleIndex == 0) {
return pitch; // Not enough samples yet
}
float sum = 0.0;
int count = bufferFilled ? SAMPLES : sampleIndex;
for (int i = 0; i < count; i++) {
sum += pitchSamples[i];
}
return sum / count;
}
float getAverageRoll() {
if (!bufferFilled && sampleIndex == 0) {
return roll; // Not enough samples yet
}
float sum = 0.0;
int count = bufferFilled ? SAMPLES : sampleIndex;
for (int i = 0; i < count; i++) {
sum += rollSamples[i];
}
return sum / count;
}
// Draw a rotated bitmap
void drawRotatedBitmap(int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, float angle) {
// Calculate center of bitmap
int16_t centerX = w / 2;
int16_t centerY = h / 2;
// Calculate sine and cosine of angle
float cosAngle = cos(angle * PI / 180.0);
float sinAngle = sin(angle * PI / 180.0);
// Calculate bytes per row for the bitmap
int16_t bytesPerRow = 6; // Hardcoded for the bitmap
// Create a temporary buffer for the rotated bitmap
// This approach avoids the need to calculate a bounding box
int16_t bufferW = w * 1.5; // Make buffer larger to accommodate rotation
int16_t bufferH = h * 1.5;
int16_t bufferCenterX = bufferW / 2;
int16_t bufferCenterY = bufferH / 2;
// Clear display area where the bitmap will be drawn
// This helps with visual clarity
display.fillRect(x - bufferW/4, y - bufferH/4, bufferW, bufferH, SH110X_BLACK);
// Draw the rotated bitmap directly
for (int16_t j = 0; j < h; j++) {
for (int16_t i = 0; i < w; i++) {
// Check if the pixel is set in the original bitmap
// For the specific bitmap format with bytes per row
uint8_t byteIndex = j * bytesPerRow + i / 8;
uint8_t bitMask = 0x80 >> (i % 8); // MSB first
// Only process set pixels
if (pgm_read_byte(&bitmap[byteIndex]) & bitMask) {
// Calculate rotated position
int16_t rotX = round((i - centerX) * cosAngle - (j - centerY) * sinAngle + bufferCenterX);
int16_t rotY = round((i - centerX) * sinAngle + (j - centerY) * cosAngle + bufferCenterY);
// Draw the pixel at the rotated position
display.drawPixel(x + rotX - bufferCenterX, y + rotY - bufferCenterY, SH110X_WHITE);
}
}
}
}
// Draw a roll indicator gauge
void drawRollGauge(int16_t x, int16_t y, int16_t width, float rollAngle) {
// Constrain roll angle to display range
float displayRoll = constrain(rollAngle, -45, 45);
// Calculate indicator position
int16_t indicatorPos = map(displayRoll * 10, -450, 450, 0, width);
// Draw gauge background
display.drawRect(x, y, width, 8, SH110X_WHITE);
// Draw center marker
display.drawLine(x + width/2, y, x + width/2, y + 8, SH110X_WHITE);
// Draw indicator
display.fillRect(x + indicatorPos - 2, y + 1, 5, 6, SH110X_WHITE);
// Draw tick marks
for (int i = 0; i <= 4; i++) {
int tickPos = map(i, 0, 4, 0, width/2);
// Left side ticks
display.drawLine(x + width/2 - tickPos, y, x + width/2 - tickPos, y + 3, SH110X_WHITE);
// Right side ticks
display.drawLine(x + width/2 + tickPos, y, x + width/2 + tickPos, y + 3, SH110X_WHITE);
}
}
void displayCarInclinometer() {
// Get averaged values
float avgPitch = -getAveragePitch();
float avgRoll = getAverageRoll();
// Clear the display
display.clearDisplay();
// Draw car icon rotated based on pitch angle
int carX = 100;
int carY = 20;
// Use a slightly smaller text size for the pitch value to prevent cutoff
display.setTextSize(2);
display.setCursor(2, 12);
display.print(avgPitch, 1);
display.print((char)247); // Degree symbol
// Draw the car icon rotated based on pitch angle (limited to ±45 degrees for visibility)
float displayPitch = -constrain(avgPitch, -45, 45);
drawRotatedBitmap(carX, carY, carIcon, CAR_ICON_WIDTH, CAR_ICON_HEIGHT, displayPitch);
// Display roll angle with smaller text size to prevent cutoff
display.setTextSize(1);
display.setCursor(20, 45);
display.println("ROLL ANGLE:");
// Use a slightly smaller text size for the roll value to prevent cutoff
display.setTextSize(1);
display.setCursor(85, 45);
display.print(avgRoll, 1);
display.print((char)247); // Degree symbol
// Draw roll gauge below the roll value
drawRollGauge(10, 55, display.width() - 20, avgRoll);
// Update the display
display.display();
}