CODE
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <math.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDR 0x3C
// Pin definitions for ESP32-C3 Super Mini
#define BUTTON_CALIBRATE 6 // Any free GPIO
#define SDA_PIN 7
#define SCL_PIN 8
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_MPU6050 mpu;
float accX, accY, accZ;
float pitch, roll;
float pitchOffset = 0.0;
float rollOffset = 0.0;
#define SAMPLES 8
float pitchSamples[SAMPLES];
float rollSamples[SAMPLES];
int sampleIndex = 0;
bool bufferFilled = false;
// Car icon 45x22
#define CAR_ICON_WIDTH 45
#define CAR_ICON_HEIGHT 22
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
};
void setup() {
Serial.begin(115200);
pinMode(BUTTON_CALIBRATE, INPUT_PULLUP);
// Initialize I2C with custom pins
Wire.begin(SDA_PIN, SCL_PIN);
if(!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
Serial.println("SSD1306 allocation failed");
while(1);
}
display.setRotation(2);
display.clearDisplay();
display.setTextColor(SSD1306_WHITE);
display.setTextSize(1);
display.setCursor(0,0);
display.println("Initializing...");
display.display();
delay(1000);
if(!mpu.begin()){
Serial.println("MPU6050 not found");
display.clearDisplay();
display.setCursor(0,0);
display.println("MPU6050 Error!");
display.display();
while(1);
}
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
for(int i=0;i<SAMPLES;i++){
pitchSamples[i]=0;
rollSamples[i]=0;
}
display.clearDisplay();
display.setCursor(0,0);
display.println("MPU6050 Ready!");
display.println("Press button to calibrate");
display.display();
delay(2000);
}
void loop() {
// Check button for calibration
if(digitalRead(BUTTON_CALIBRATE)==LOW){
delay(50);
if(digitalRead(BUTTON_CALIBRATE)==LOW){
calibrateSensor();
while(digitalRead(BUTTON_CALIBRATE)==LOW) delay(10);
}
}
// Read MPU6050
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
accX = a.acceleration.x;
accY = a.acceleration.y;
accZ = a.acceleration.z;
pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ))*180/PI;
roll = atan2(accY, accZ)*180/PI;
pitch -= pitchOffset;
roll -= rollOffset;
pitchSamples[sampleIndex] = pitch;
rollSamples[sampleIndex] = roll;
sampleIndex = (sampleIndex+1)%SAMPLES;
if(sampleIndex==0) bufferFilled=true;
displayInclinometer();
delay(50);
}
void calibrateSensor(){
display.clearDisplay();
display.setCursor(0,0);
display.println("Calibrating...");
display.println("Place flat");
display.display();
delay(3000);
float pSum=0,rSum=0;
int readings=20;
for(int i=0;i<readings;i++){
sensors_event_t a,g,t;
mpu.getEvent(&a,&g,&t);
float p = atan2(-a.acceleration.x, sqrt(a.acceleration.y*a.acceleration.y + a.acceleration.z*a.acceleration.z))*180/PI;
float r = atan2(a.acceleration.y,a.acceleration.z)*180/PI;
pSum += p; rSum += r;
delay(50);
}
pitchOffset=pSum/readings;
rollOffset=rSum/readings;
for(int i=0;i<SAMPLES;i++){ pitchSamples[i]=0; rollSamples[i]=0; }
sampleIndex=0; bufferFilled=false;
display.clearDisplay();
display.setCursor(0,0);
display.println("Calibration Done!");
display.print("Pitch: "); display.println(pitchOffset,1);
display.print("Roll: "); display.println(rollOffset,1);
display.display();
delay(2000);
}
float getAveragePitch(){
float sum=0; int cnt = bufferFilled? SAMPLES : sampleIndex;
for(int i=0;i<cnt;i++) sum+=pitchSamples[i];
return sum/cnt;
}
float getAverageRoll(){
float sum=0; int cnt = bufferFilled? SAMPLES : sampleIndex;
for(int i=0;i<cnt;i++) sum+=rollSamples[i];
return sum/cnt;
}
void drawRotatedBitmap(int16_t x,int16_t y,const uint8_t *bmp,int16_t w,int16_t h,float angle){
int16_t cx=w/2,cy=h/2;
float c=cos(angle*PI/180.0), s=sin(angle*PI/180.0);
int16_t bpr=(w+7)/8;
int16_t bw=w*1.5,bh=h*1.5;
int16_t bcx=bw/2,bcy=bh/2;
display.fillRect(x-bw/4,y-bh/4,bw,bh,SSD1306_BLACK);
for(int16_t j=0;j<h;j++){
for(int16_t i=0;i<w;i++){
uint8_t byteIndex = j*bpr + i/8;
uint8_t bitMask = 0x80 >> (i%8);
if(pgm_read_byte(&bmp[byteIndex]) & bitMask){
int16_t rx = round((i-cx)*c-(j-cy)*s + bcx);
int16_t ry = round((i-cx)*s+(j-cy)*c + bcy);
display.drawPixel(x+rx-bcx, y+ry-bcy, SSD1306_WHITE);
}
}
}
}
void drawRollGauge(int16_t x,int16_t y,int16_t w,float angle){
float displayRoll = constrain(angle,-45,45);
int16_t ind = map(displayRoll*10,-450,450,0,w);
display.drawRect(x,y,w,8,SSD1306_WHITE);
display.drawLine(x+w/2,y,x+w/2,y+8,SSD1306_WHITE);
display.fillRect(x+ind-2,y+1,5,6,SSD1306_WHITE);
for(int i=0;i<=4;i++){
int t=map(i,0,4,0,w/2);
display.drawLine(x+w/2-t,y,x+w/2-t,y+3,SSD1306_WHITE);
display.drawLine(x+w/2+t,y,x+w/2+t,y+3,SSD1306_WHITE);
}
}
void displayInclinometer(){
float avgP=-getAveragePitch();
float avgR=getAverageRoll();
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0);
display.print(avgP,1); display.print((char)247);
drawRotatedBitmap(90,20,carIcon,CAR_ICON_WIDTH,CAR_ICON_HEIGHT,-constrain(avgP,-45,45));
display.setTextSize(1);
display.setCursor(20,50);
display.print("ROLL: "); display.print(avgR,1); display.print((char)247);
drawRollGauge(10,58,display.width()-20,avgR);
display.display();
}