Centrale inertielle MPU6050

/*
 * MPU6050 : lecture brute sans bibliothèque — Kaihatsu Lab
 * Lit accélération, température et rotation directement dans les registres, et affiche le résultat converti.
 * Adresse I2C : 0x68 (0x69 si la broche AD0 est reliée au 3,3 V).
 */
#include <Wire.h>

const uint8_t MPU_ADDR = 0x68;
const int PIN_SDA = 21;
const int PIN_SCL = 22;

// Sensibilités pour les plages choisies plus bas : ±2 g -> 16384 LSB/g, ±250 °/s -> 131 LSB/(°/s)
const float LSB_PER_G   = 16384.0;
const float LSB_PER_DPS = 131.0;

void writeReg(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

int16_t read16() {                        // deux octets, poids fort en premier
  uint8_t hi = Wire.read();
  uint8_t lo = Wire.read();
  return (int16_t)((hi << 8) | lo);
}

bool readAll(int16_t &ax, int16_t &ay, int16_t &az, int16_t &tmp, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);                       // ACCEL_XOUT_H : début des 14 octets de mesure
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)14) != 14) return false;
  ax = read16(); ay = read16(); az = read16();
  tmp = read16();
  gx = read16(); gy = read16(); gz = read16();
  return true;
}

void setup() {
  Serial.begin(115200);
  Wire.begin(PIN_SDA, PIN_SCL);
  Wire.setClock(400000);

  writeReg(0x6B, 0x00);                   // PWR_MGMT_1 : sort du mode veille (le capteur démarre endormi)
  writeReg(0x1C, 0x00);                   // ACCEL_CONFIG : ±2 g
  writeReg(0x1B, 0x00);                   // GYRO_CONFIG  : ±250 °/s
  writeReg(0x1A, 0x03);                   // CONFIG : filtre passe-bas interne à environ 44 Hz

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x75);                       // WHO_AM_I : 0x68 attendu
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, (uint8_t)1);
  Serial.print(F("WHO_AM_I = 0x"));
  Serial.println(Wire.read(), HEX);
}

void loop() {
  int16_t ax, ay, az, tmp, gx, gy, gz;
  if (!readAll(ax, ay, az, tmp, gx, gy, gz)) {
    Serial.println(F("Lecture impossible : verifie SDA, SCL et l'adresse"));
    delay(500);
    return;
  }
  Serial.print(F("acc (g) "));
  Serial.print(ax / LSB_PER_G, 2); Serial.print(' ');
  Serial.print(ay / LSB_PER_G, 2); Serial.print(' ');
  Serial.print(az / LSB_PER_G, 2);
  Serial.print(F(" | gyro (deg/s) "));
  Serial.print(gx / LSB_PER_DPS, 1); Serial.print(' ');
  Serial.print(gy / LSB_PER_DPS, 1); Serial.print(' ');
  Serial.print(gz / LSB_PER_DPS, 1);
  Serial.print(F(" | temp "));
  Serial.print(tmp / 340.0 + 36.53, 1);
  Serial.println(F(" C"));
  delay(100);
}

/*
 * MPU6050 : angles roll et pitch par filtre complémentaire — Kaihatsu Lab
 * Le gyroscope donne la réactivité, l'accéléromètre corrige la dérive.
 * Copie ici les offsets trouvés avec l'outil de calibration (capteur posé à plat, immobile).
 * Le yaw n'est pas calculé : sans magnétomètre, il dérive.
 */
#include <Wire.h>

const uint8_t MPU_ADDR = 0x68;

const float LSB_PER_G   = 16384.0;        // ±2 g
const float LSB_PER_DPS = 131.0;          // ±250 °/s
const float ALPHA       = 0.98;           // poids du gyroscope (0 à 1)

// Offsets bruts à retrancher (valeurs de l'outil de calibration)
const int16_t AX_OFFSET = 0, AY_OFFSET = 0, AZ_OFFSET = 0;
const int16_t GX_OFFSET = 0, GY_OFFSET = 0, GZ_OFFSET = 0;

float roll = 0, pitch = 0;
uint32_t lastUs = 0;

void writeReg(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

int16_t read16() {
  uint8_t hi = Wire.read();
  uint8_t lo = Wire.read();
  return (int16_t)((hi << 8) | lo);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(400000);
  writeReg(0x6B, 0x00);
  writeReg(0x1A, 0x03);
  delay(100);
  lastUs = micros();
}

void loop() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)14) != 14) return;
  int16_t ax = read16() - AX_OFFSET;
  int16_t ay = read16() - AY_OFFSET;
  int16_t az = read16() - AZ_OFFSET;
  read16();                               // température ignorée
  int16_t gx = read16() - GX_OFFSET;
  int16_t gy = read16() - GY_OFFSET;
  read16();                               // gz inutile pour roll et pitch

  uint32_t now = micros();
  float dt = (now - lastUs) / 1e6;
  lastUs = now;

  float axg = ax / LSB_PER_G, ayg = ay / LSB_PER_G, azg = az / LSB_PER_G;
  float rollAcc  = atan2(ayg, azg) * RAD_TO_DEG;
  float pitchAcc = atan2(-axg, sqrt(ayg * ayg + azg * azg)) * RAD_TO_DEG;

  roll  = ALPHA * (roll  + (gx / LSB_PER_DPS) * dt) + (1.0 - ALPHA) * rollAcc;
  pitch = ALPHA * (pitch + (gy / LSB_PER_DPS) * dt) + (1.0 - ALPHA) * pitchAcc;

  Serial.print(F("roll "));
  Serial.print(roll, 1);
  Serial.print(F("  pitch "));
  Serial.println(pitch, 1);
  delay(10);
}

/*
 * Niveau à bulle numérique — Kaihatsu Lab
 * MPU6050 + écran OLED SSD1306, tous deux sur le même bus I2C (SDA 21, SCL 22).
 * La bulle va du côté le plus haut, comme sur un vrai niveau. Bibliothèques : Adafruit GFX et SSD1306.
 *
 * Monte le MPU6050 à plat avec ses axes dans le même sens que l'écran : X vers la droite, Y vers le haut.
 * Chiffres affichés : P = pitch (autour de Y), R = roll (autour de X).
 */
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_W  128
#define SCREEN_H  64
#define OLED_ADDR 0x3C
const uint8_t MPU_ADDR = 0x68;

Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1);

const int   CX = 40, CY = 32, R_OUT = 28;   // cadran à gauche, chiffres à droite
const float PIXELS_PER_DEG = 2.0;           // sensibilité de la bulle
float roll = 0, pitch = 0;                  // degrés, lissés

int16_t read16() {
  uint8_t hi = Wire.read();
  uint8_t lo = Wire.read();
  return (int16_t)((hi << 8) | lo);
}

bool readAngles(float &r, float &p) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)6) != 6) return false;
  float ax = read16() / 16384.0;
  float ay = read16() / 16384.0;
  float az = read16() / 16384.0;
  r = atan2(ay, az) * RAD_TO_DEG;
  p = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;
  return true;
}

void setup() {
  Wire.begin(21, 22);
  Wire.setClock(400000);
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
}

void loop() {
  float r, p;
  if (readAngles(r, p)) {
    roll  += 0.2 * (r - roll);              // lissage exponentiel : la bulle ne tremble pas
    pitch += 0.2 * (p - pitch);
  }

  float bx = CX - pitch * PIXELS_PER_DEG;   // tangage positif : le côté droit descend, la bulle va à gauche
  float by = CY - roll * PIXELS_PER_DEG;    // roulis positif : le haut monte, la bulle monte
  float dx = bx - CX, dy = by - CY;
  float dist = sqrt(dx * dx + dy * dy), maxD = R_OUT - 5;
  if (dist > maxD) { bx = CX + dx * maxD / dist; by = CY + dy * maxD / dist; }

  display.clearDisplay();
  display.drawCircle(CX, CY, R_OUT, SSD1306_WHITE);       // cadran
  display.drawCircle(CX, CY, 7, SSD1306_WHITE);           // cible centrale
  display.drawFastHLine(CX - R_OUT, CY, 6, SSD1306_WHITE);
  display.drawFastHLine(CX + R_OUT - 5, CY, 6, SSD1306_WHITE);
  display.fillCircle((int)bx, (int)by, 5, SSD1306_WHITE); // bulle

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(82, 14);
  display.print(F("P "));
  display.print(pitch, 1);
  display.setCursor(82, 34);
  display.print(F("R "));
  display.print(roll, 1);
  display.display();
  delay(20);
}