I recently purchased a SparkFun (InvenSense) MPU-6050, six degrees of freedom Gyroscope & Accelerometer from Robosavvy. It’s a great bit of kit, which combines a 3-axis gyroscope and a 3-axis accelerometer on the same board. It hooks up easily to an Arduino using the I2C bus. So far, so good…
I then begin searching the Internet for example code with these two devices working together. My search always led me to a huge, unwieldy library, which seemed very bloated, considering all I wanted to do was read some values from the board.
I began to dig deeper and experiment, which enabled me to create the code sample below. It relies on using default values, which are fine for my application – and it’s light! The accelerometer channels are very twitchy, so the general advice is to incorporate a low pass filter, which I’ve done.
Hopefully, you will also find it useful for your projects.
Main Program
#include "Wire.h" #include "MPU6050.h" MPU6050 mpu; int ax, ay, az, gx, gy, gz; float smooth_ax, smooth_ay, smooth_az; void setup() { Wire.begin(); Serial.begin(38400); mpu.wakeup(); } void loop() { /* Read device to extract current accelerometer and gyroscope values. */ mpu.read6dof(&ax, &ay, &az, &gx, &gy, &gz); /* Apply low pass filter to smooth accelerometer values. */ smooth_ax = 0.95 * smooth_ax + 0.05 * ax; smooth_ay = 0.95 * smooth_ay + 0.05 * ay; smooth_az = 0.95 * smooth_az + 0.05 * az; /* Output to serial monitor. */ Serial.print(smooth_ax); Serial.print("t"); Serial.print(smooth_ay); Serial.print("t"); Serial.print(smooth_az); Serial.print("t"); Serial.print(gx); Serial.print("t"); Serial.print(gy); Serial.print("t"); Serial.println(gz); }
MPU6050.h
#include "Arduino.h" #include "Wire.h" #define MPU6050_DEVICE_ADDRESS 0x68 #define MPU6050_RA_ACCEL_XOUT_H 0x3B #define MPU6050_RA_PWR_MGMT_1 0x6B #define MPU6050_PWR1_SLEEP_BIT 6 class MPU6050 { public: MPU6050(); void wakeup(); void read6dof(int* ax, int* ay, int* az, int* gx, int* gy, int* gz); private: byte id; byte buffer[14]; void readByte(byte reg, byte *data); void readBytes(byte reg, byte len, byte *data); void writeBit(byte reg, byte num, byte data); void writeByte(byte reg, byte data); };
MPU6050.cpp
#include "MPU6050.h" MPU6050::MPU6050() { id = MPU6050_DEVICE_ADDRESS; } /* Wake up device and use default values for accelerometer (±2g) and gyroscope (±250°/sec). */ void MPU6050::wakeup() { writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0); } /* Read device memory to extract current accelerometer and gyroscope values. */ void MPU6050::read6dof(int* ax, int* ay, int* az, int* gx, int* gy, int* gz) { readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, buffer); *ax = (((int)buffer[0]) << 8) | buffer[1]; *ay = (((int)buffer[2]) << 8) | buffer[3]; *az = (((int)buffer[4]) << 8) | buffer[5]; *gx = (((int)buffer[8]) << 8) | buffer[9]; *gy = (((int)buffer[10]) << 8) | buffer[11]; *gz = (((int)buffer[12]) << 8) | buffer[13]; } /* Read a single byte from specified register. */ void MPU6050::readByte(byte reg, byte *data) { readBytes(reg, 1, data); } /* Read multiple bytes starting at specified register. */ void MPU6050::readBytes(byte reg, byte len, byte *data) { byte count = 0; Wire.beginTransmission(id); Wire.write(reg); Wire.requestFrom(id, len); while (Wire.available()) data[count++] = Wire.read(); Wire.endTransmission(); } /* Write bit to specified register and location. */ void MPU6050::writeBit(byte reg, byte num, byte data) { byte b; readByte(reg, &b); b = (data != 0) ? (b | (1 << num)) : (b & ~(1 << num)); writeByte(reg, b); } /* Write byte to specified register. */ void MPU6050::writeByte(byte reg, byte data) { Wire.beginTransmission(id); Wire.write(reg); Wire.write(data); Wire.endTransmission(); }