MPU6050 Accelerometer & Gyroscoop Handleiding voor Arduino

In deze handleiding leer je de MPU6050 Accelerometer en Gyroscoop sensor gebruiken met Arduino. Eerst uitleg over werking en data uitlezen, daarna twee praktische voorbeelden.

Bekijk de video of lees de handleiding.

Overzicht

Voorbeeld 1: Met Processing IDE een 3D-visualisatie van de sensororiëntatie. Voorbeeld 2: Een zelfstabiliserend platform (DIY Gimbal) – op basis van MPU6050 oriëntatie en gefuseerde accelerometer/gyroscoop data besturen we drie servo's die het platform vlak houden.

Hoe het Werkt

De MPU6050 IMU heeft een 3-as accelerometer en 3-as gyroscoop geïntegreerd op één chip.

De gyroscoop meet rotatiesnelheid (graad/seconde) langs X, Y, Z. Het gebruikt MEMS-technologie en het Corioliseffect. Uitgang in °/s; voor hoek positioneren we de hoeksnelheid (integratie).

De accelerometer meet versnelling (zoals ADXL345). Meet zwaartekrachtversnelling langs 3 assen; met trigonometrie bereken je de hoek. Samen (sensor fusion) geven accelerometer en gyroscoop zeer nauwkeurige oriëntatie-informatie.

De MPU6050 heet ook 6-as motion tracking device of 6 DoF (6 Degrees of Freedom) door de 6 uitgangen (3 accel + 3 gyro).

Arduino en MPU6050

Communicatie via I2C → alleen 2 draden (+ 2 voor voeding).

Componenten: MPU6050 IMU, Arduino Board, Breadboard en Jumper Draden.

MPU6050 Arduino Code

Code voor het uitlezen van MPU6050 data. Onder de code gedetailleerde uitleg.

/* Arduino en MPU6050 Accelerometer en Gyroscoop Tutorial
 * by Dejan, https://howtomechatronics.com
 */
#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C adres
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;

void setup() {
  Serial.begin(19200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // Register 6B (PWR_MGMT_1)
  Wire.write(0x00); // Reset - 0 in register 6B
  Wire.endTransmission(true);
  /*
  // Optioneel: gevoeligheid instellen (standaard +/-2g, +/-250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C); // ACCEL_CONFIG
  Wire.write(0x10); // +/- 8g
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU);
  Wire.write(0x1B); // GYRO_CONFIG
  Wire.write(0x10); // 1000deg/s
  Wire.endTransmission(true);
  delay(20);
  */
  calculate_IMU_error(); // Foutwaarden berekenen
  delay(20);
}

void loop() {
  // === Accelerometer data lezen ===
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start register ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // 6 registers (2 per as)
  // Voor +/-2g: raw / 16384 (volgens datasheet)
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

  // Roll en Pitch berekenen uit accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY

  // === Gyroscoop data lezen ===
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000.0; // seconden

  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data start register 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // 6 registers
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Voor 250deg/s: /131
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

  // Foutcorrectie toepassen
  GyroX = GyroX + 0.56;   // GyroErrorX ~(-0.56)
  GyroY = GyroY - 2;      // GyroErrorY ~(2)
  GyroZ = GyroZ + 0.79;   // GyroErrorZ ~(-0.8)

  // Integreren: deg/s * s = graden
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw = yaw + GyroZ * elapsedTime;

  // Complementair filter: fuseer accel en gyro
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

  // Print op Serial Monitor
  Serial.print(roll);
  Serial.print("/");
  Serial.print(pitch);
  Serial.print("/");
  Serial.println(yaw);
}

void calculate_IMU_error() {
  // Roep aan in setup voor foutwaarden. Zet IMU vlak en stil.
  // Lees accelerometer 200x
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;
    AccErrorX += atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI;
    AccErrorY += atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI;
    c++;
  }
  AccErrorX /= 200;
  AccErrorY /= 200;
  c = 0;

  // Lees gyroscoop 200x
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroErrorX += GyroX;
    GyroErrorY += GyroY;
    GyroErrorZ += GyroZ;
    c++;
  }
  GyroErrorX /= 200;
  GyroErrorY /= 200;
  GyroErrorZ /= 200;

  // Print foutwaarden
  Serial.print("AccErrorX: "); Serial.println(AccErrorX);
  Serial.print("AccErrorY: "); Serial.println(AccErrorY);
  Serial.print("GyroErrorX: "); Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: "); Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: "); Serial.println(GyroErrorZ);
}

Code Beschrijving: Include Wire.h voor I2C. Variabelen definieren. In setup(): Wire init, sensor reset via power management register (0x6B). Optioneel: gevoeligheid instellen via ACCEL_CONFIG (0x1C) en GYRO_CONFIG (0x1B). Standaard +/-2g en +/-250°/s gebruikt (code uitgecommentarieerd).

In loop(): accelerometer uitlezen. Data per as in 2 registers (adressen uit datasheet).

Start bij register 0x3B, requestFrom() leest 6 registers. Bytes combineren (two's complement), delen door 16384 voor +/-2g → -1g tot +1g.

AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

Roll en Pitch uit accelerometer data:

accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58;
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58;

Gyroscoop data uitlezen (registers 0x43–0x48):

6 registers lezen, combineren, delen door 131 (voor 250°/s) → °/s.

GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

Foutcorrectie toepassen (waarden uit calculate_IMU_error()). Integreren: °/s × s = °. Tijd via millis().

GyroX = GyroX + 0.56;
GyroY = GyroY - 2;
GyroZ = GyroZ + 0.79;
gyroAngleX = gyroAngleX + GyroX * elapsedTime;
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;

Complementair filter: 96% gyro (nauwkeurig, geen externe krachten), 4% accel (langetermijn correctie voor gyro drift). Yaw kan niet uit accelerometer → geen filter mogelijk.

roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

Foutwaarden bepalen: roep calculate_IMU_error() aan in setup met sensor vlak en stil. 200 metingen, gemiddelde = fout. Implementeer in formules.

Print Roll, Pitch, Yaw op Serial Monitor voor controle.

MPU6050 Oriëntatie Tracking – 3D Visualisatie

Voor 3D-visualisatie: Arduino seriële data ontvangen in Processing IDE. Complete Processing code:

/* Arduino en MPU6050 IMU - 3D Visualisatie
 * by Dejan, https://howtomechatronics.com
 */
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;

Serial myPort;
String data="";
float roll, pitch, yaw;

void setup() {
  size(2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200);
  myPort.bufferUntil('\n');
}

void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + " Pitch: " + int(pitch), -100, 265);
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  textSize(30);
  fill(0, 76, 153);
  box(386, 40, 200);
  textSize(25);
  fill(255, 255, 255);
  text("www.HowToMechatronics.com", -183, 10, 101);
}

void serialEvent(Serial myPort) {
  data = myPort.readStringUntil('\n');
  if (data != null) {
    data = trim(data);
    String[] items = split(data, '/');
    if (items.length > 1) {
      roll = float(items[0]);
      pitch = float(items[1]);
      yaw = float(items[2]);
    }
  }
}

Data ontvangen via serialEvent(), in Roll/Pitch/Yaw variabelen. In draw() 3D-object (box) roteren.

Resultaat: 3D-object volgt sensororiëntatie nauwkeurig en responsief.

Nadeel: Yaw drift (geen complementair filter mogelijk). Verbetering: magnetometer voor langetermijn Yaw-correctie. MPU6050 heeft ook Digital Motion Processor (DMP) voor onboard berekeningen en Yaw-drift eliminatie.

Met DMP: zeer nauwkeurig, geen Yaw-drift. Onboard processor berekent quaternions (voor 3D-oriëntatie/rotaties, geen Gimbal-lock).

DMP-data uitlezen is complexer: extra interrupt-pin naar Arduino nodig.

Code complexer → gebruik i2cdevlib bibliotheek van Jeff Rowberg (GitHub). Voorbeeld MPU6050_DMP6 uit bibliotheek (goed gedocumenteerd). Output selecteren: quaternions, Euler-hoeken, yaw/pitch/roll, versnellingen, of quaternions voor 3D-visualisatie. Bibliotheek bevat ook Processing sketch. Aangepast voor box-vorm:

/* Arduino en MPU6050 IMU - 3D Visualisatie (DMP)
 * by Dejan, https://howtomechatronics.com
 */
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;

Serial myPort;
String data="";
float roll, pitch, yaw;

void setup() {
  size(2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200);
  myPort.bufferUntil('\n');
}

void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + " Pitch: " + int(pitch), -100, 265);
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  textSize(30);
  fill(0, 76, 153);
  box(386, 40, 200);
  textSize(25);
  fill(255, 255, 255);
  text("www.HowToMechatronics.com", -183, 10, 101);
}

void serialEvent(Serial myPort) {
  data = myPort.readStringUntil('\n');
  if (data != null) {
    data = trim(data);
    String[] items = split(data, '/');
    if (items.length > 1) {
      roll = float(items[0]);
      pitch = float(items[1]);
      yaw = float(items[2]);
    }
  }
}

Via serialEvent() quaternions ontvangen, in draw() 3D-object roteren. Quaternions zijn superieur voor 3D-rotaties.

Het tweede voorbeeld (DIY Arduino Gimbal / zelfstabiliserend platform) staat in een apart artikel.

Vragen? Reacties hieronder. Bekijk mijn Arduino Projecten Collectie.