Build a Digital Level with MPU-6050 and Arduino

https://dronebotworkshop.com/mpu-6050-level/

Build a Digital Level with MPU-6050 and Arduino | DroneBot Workshop

Introduction

The MPU-6050 is a low-cost, highly accurate IMU with 6 degrees of freedom. Don’t fret if that last sentence sounded like a foreign language, all will be explained soon!

Today we will look at the MPU-6050 module to see how it works and how to use it with an Arduino. We will also build a very sensitive electronic level.  Although our level will be constructed on a breadboard it could also serve as the basis for a permanent project.

MPU-6050 IMU

The MPU-6050 belongs to a class of devices known as Inertial Measurement Units, or IMUs.  These devices can measure acceleration, inertia and a number of other parameters to allow you to determine their spatial position and velocity.

IMUs like the MPU-6050 are used in a number of different applications:

• UAVs like quadcopters and helicopters. Here an IMU is used to determine yaw, pitch, and roll, and to level the devices while flying.
• Robotics. From balancing robots to robot arms it is common to make use of IMUs to establish position and velocity.
• Game Controllers. Games like the Wii use an IMU to detect level and motion.
• Phones and tablets. It is common to find IMUs in phones and tablets for determining the orientation of the device to switch between portrait and landscape modes.
• Hard drives. An IMU or accelerometer is included in hard drives designed for portable use. They can detect if the drive is being dropped and take appropriate action, like stopping the platters and locking the heads.

Accelerometer

One of the key components of the MPU-6050 is an Accelerometer.  As its name would imply, this is a device that can measure acceleration.

Acceleration is described as being the rate of change of the velocity of an object. An object moving at a constant speed does not have any acceleration.

A common unit of acceleration is G-force. One G-force here on Earth is 9.9 meters per second squared, which is the acceleration of gravity here. On other planets the figure is different.

An accelerometer needs to take into account static acceleration like the force of gravity when making its measurements.

The accelerometer used in the MPU-6050 is a triple-axis accelerometer, meaning that it senses acceleration on an X, Y and Z axis.

Gyroscope

Another key component in the MPU-6050 is a Gyroscope.

A gyroscope can measure angular momentum or rotation along the X, Y and Z axis. These components are critical in maintaining balance for aircraft and spacecraft.

A mechanical gyroscope consists of a wheel or disk mounted so that it can spin rapidly about an axis which is free to move in any direction.

The type of gyroscope used in the MPU-6050 is a “Micro Electro Mechanical System” or MEMS gyroscope. It consists of three sensors, one per axis, that produce a voltage when they are rotated. This voltage is internally sampled using a 16-bit analog to digital converter.

External Sensors

The MPU-6050 can also make use of external sensors. These external devices are interfaced to the MPU-6050 using a second I2C bus, independent of the main I2C bus.

A common use for this external connection is to attach a magnetometer, which can measure magnetic fields on three axes.

This adds an extra three Degrees of Freedom, or DOF, to the sensor. By itself, the MPU-6050 has 6 DOF, three each for the accelerometer and the gyroscope.  Adding a magnetometer would give it 9 DOF.

The MPU-6050 also has an internal temperature sensor.

DMP

The data from the accelerometer, gyroscope and any third-party sensors is passed to an internal Digital Motion Processor, or DMP.

This device correlates the data and formats it to be used on the I2C bus. It is essentially a high-performance microprocessor that is dedicated to processing motion data.

The term DMP was coined by Invensense, a division of TDK. This is the company that developed the MPU-6050, along with many other IMUs.

Pinouts

The MPU-6050 is available in several different configurations and on several different breakout modules.

Here is an illustration of the pinouts of the module that I am using in these experiments:

This module has an internal voltage regulator, so it is supplied with 5-volts power. The MPU-6050 chip itself uses 3.3-volt logic. The module also has resistors to change the data levels to 3.3-volts.

The pin descriptions of the module are as follows:

• VCC – 5-volt DC power supply.
• GND – Ground
• SCL – This is the I2C clock line.
• SDA – The I2C data line.
• XDA – This is the external I2C data line. The external I2C bus is for connecting external sensors.
• XCL – This is the external I2C clock line.
• AD0 – This line allows you to change the internal I2C address of the MPU-6050 module. It can be used if the hmodule is conflicting with another I2C device, or if you wish to use two MPU-6050s on the same I2C bus.
• INT – This is the Interrupt Output.

If you are using a different module you can still perform the experiments listed here., Pay attention to the power requirements of your module as some do not have an internal voltage regulator and operate on 3.3-volt logic.

Note that the module also has orientation markings, to allow you to position it correctly in your device.

MPU-6050 with Arduino

Before we build our electronic level let’s do an experiment using the MPU-6050. To make things a bit easier we will use a couple of libraries and run a demonstration sketch that is included with one of them

I2C and MPU-6050 Libraries

The libraries we are going to be using in our demo are part of the collection of libraries developed by Jeff Rowberg for working with the I2C bus and some common I2C sensors. You can find the complete set of libraries on Github.

This is an extensive collection of libraries, not only for the Arduino but for a number of other common microcontrollers as well.

As it is a bit overwhelming you might find it useful just to get the two libraries we are going to use today – the I2C Development library and the MPU-6050 library.

If you would like more information regarding this excellent set of libraries visit Jeff’s webpage dedicated to I2C development.

These libraries are provided in ZIP format. You can install the ZIP files directly into your Arduino IDE.

• Open the Arduino IDE.
• Select the Sketch menu from the top menu bar.
• Select Include Library. A sub-menu will open.
• Select Add ZIP Library.
• A dialog box will open. Navigate to where you saved the libraries and select one of them.
• The library will be installed into your Arduino IDE.
• You will need to repeat this step for the second library.

Now that you have the two libraries added to your Arduino IDE it’s time to hook up the MPU-6050 to the Arduino/

MPU-6050 and Arduino Hookup

Here is the hook up that we will use for the experiment:

The connections are pretty simple, as the MPU-6050 interfaces using the I2C bus. On the Arduino Uno the I2C connections are made using the Analog input pins, A4 is the SDA connection and A5 is the SCL.

We are also using the interrupt output from the MPU-6050. This connects to pin 2 on the Arduino Uno, which is interrupt number 0.

Demo Sketch

When you have everything hooked up try running one of the demo sketches provided in the MPU-6050 library we just installed.

• Open the Arduino IDE.
• Select the File menu from the top menu bar.
• Select Examples. A sub-menu will open up.
• Scroll down the sub-menu to the section entitled Examples from Custom Libraries.
• Look for the MPU6050 entry and select it.
• Another sub-menu will open that says Examples. Select it and two example sketches will be listed
• Select the MPU6050_DMP6 sketch.

The sketch is quite complex but is very well commented. It will give you a taste of how much math is involved in extracting useful information from the MPU-6050 sensor.

Note that you will need to set your serial monitor to a speed of 115200 baud to try the sketch. There is a lot of data being sent back from the IMU and it requires this higher speed to display it.

When you open the serial monitor with the sketch loaded to the Arduino you should see a message saying that the connection to the MPU-6050 has been made successfully.  In order to start running the sketch you need to place your cursor in the serial monitor input box and type a letter, then press Send (or just hit Enter on your keyboard).

You will see a wealth of data displaying the Yaw, Pitch and Roll values. Try moving your sensor around and note how the data changes.

Electronic Level Project

Now onto our electronic level project.

We will build a sensitive level that has both a digital and LED display. The LED display will show you if you are within a degree of being level, if you dare close or if you are over 2 degrees off.

Although I am building this on a solderless breadboard as an experiment this is a good candidate for a permanent project, as it is a truly useful device. If you do decide to build a permanent version of this device you may want to use an Arduino Nano or Pro Mini to save space.

Electronic Level Hookup

Here is the hookup diagram for our electronic level:

The LCD display I am using has an I2C adapter on it to make it easier to connect. It is a 2-line, 16-character display, a very common item.

I used a green LED to indicate that it is level, yellow LEDs to indicate that you are close and red LEDs to indicate you are more than 2 degrees off. You can substitute other colors if you wish, or just use a LED bargraph display.

The dropping resistors I used were 220 ohms, but any value from 150 to 470 ohms will suffice.

Electronic Level Sketch

Here is the sketch I used to make the electronic level work:

/*
  Electronic Level
  mpu-6050-level.ino
  Uses MPU-6050 IMU
  Displays on 128x64 OLED and LED
  
  DroneBot Workshop 2019
  https://dronebotworkshop.com
*/
 
// Include Wire Library for I2C
#include <Wire.h>
 
// Include NewLiquidCrystal Library for I2C
#include <LiquidCrystal_I2C.h>
 
// Define LCD pinout
const int  en = 2, rw = 1, rs = 0, d4 = 4, d5 = 5, d6 = 6, d7 = 7, bl = 3;
 
// Define I2C Address - change if reqiuired
const int i2c_addr = 0x3F;
 
LiquidCrystal_I2C lcd(i2c_addr, en, rw, rs, d4, d5, d6, d7, bl, POSITIVE);
 
// Level LEDs
int levelLED_neg1 = 9;
int levelLED_neg0 = 10;
int levelLED_level = 11;
int levelLED_pos0 = 12;
int levelLED_pos1 = 13;
 
 
 
//Variables for Gyroscope
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
 
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
 
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
 
// Setup timers and temp variables
long loop_timer;
int temp;
 
// Display counter
int displaycount = 0;
 
void setup() {
 
  //Start I2C
  Wire.begin();
  
  // Set display type as 16 char, 2 rows
  lcd.begin(16,2); 
  
  // Set Level LEDs as outputs
  pinMode(levelLED_neg1, OUTPUT);
  pinMode(levelLED_neg0, OUTPUT);
  pinMode(levelLED_level, OUTPUT);
  pinMode(levelLED_pos0, OUTPUT);
  pinMode(levelLED_pos1, OUTPUT);
  
  
  //Setup the registers of the MPU-6050                                                       
  setup_mpu_6050_registers(); 
  
  //Read the raw acc and gyro data from the MPU-6050 1000 times                                          
  for (int cal_int = 0; cal_int < 1000 ; cal_int ++){                  
    read_mpu_6050_data(); 
    //Add the gyro x offset to the gyro_x_cal variable                                            
    gyro_x_cal += gyro_x;
    //Add the gyro y offset to the gyro_y_cal variable                                              
    gyro_y_cal += gyro_y; 
    //Add the gyro z offset to the gyro_z_cal variable                                             
    gyro_z_cal += gyro_z; 
    //Delay 3us to have 250Hz for-loop                                             
    delay(3);                                                          
  }
 
  // Divide all results by 1000 to get average offset
  gyro_x_cal /= 1000;                                                 
  gyro_y_cal /= 1000;                                                 
  gyro_z_cal /= 1000;
  
  // Start Serial Monitor                                                 
  Serial.begin(115200);
  
  // Init Timer 
  loop_timer = micros();                                               
}
 
void loop(){
 
  // Get data from MPU-6050
  read_mpu_6050_data();
     
  //Subtract the offset values from the raw gyro values
  gyro_x -= gyro_x_cal;                                                
  gyro_y -= gyro_y_cal;                                                
  gyro_z -= gyro_z_cal;                                                
         
  //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
  
  //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_pitch += gyro_x * 0.0000611;  
  //Calculate the traveled roll angle and add this to the angle_roll variable
  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians                                
  angle_roll += gyro_y * 0.0000611; 
                                     
  //If the IMU has yawed transfer the roll angle to the pitch angle
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
  //If the IMU has yawed transfer the pitch angle to the roll angle               
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               
  
  //Accelerometer angle calculations
  
  //Calculate the total accelerometer vector
  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); 
   
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  //Calculate the pitch angle
  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; 
  //Calculate the roll angle      
  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       
  
  //Accelerometer calibration value for pitch
  angle_pitch_acc -= 0.0;
  //Accelerometer calibration value for roll                                              
  angle_roll_acc -= 0.0;                                               
 
  if(set_gyro_angles){ 
  
  //If the IMU has been running 
  //Correct the drift of the gyro pitch angle with the accelerometer pitch angle                      
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; 
    //Correct the drift of the gyro roll angle with the accelerometer roll angle    
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        
  }
  else{ 
    //IMU has just started  
    //Set the gyro pitch angle equal to the accelerometer pitch angle                                                           
    angle_pitch = angle_pitch_acc;
    //Set the gyro roll angle equal to the accelerometer roll angle                                       
    angle_roll = angle_roll_acc;
    //Set the IMU started flag                                       
    set_gyro_angles = true;                                            
  }
  
  //To dampen the pitch and roll angles a complementary filter is used
  //Take 90% of the output pitch value and add 10% of the raw pitch value
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; 
  //Take 90% of the output roll value and add 10% of the raw roll value 
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; 
  //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop  
  
  // Print to Serial Monitor   
  //Serial.print(" | Angle  = "); Serial.println(angle_pitch_output);
  
  
  // Increment the display counter
  displaycount = displaycount +1;
  
  if (displaycount > 100) {
 
  lcd.clear();
  // Print on first row of LCD
  lcd.setCursor(0,0);
  lcd.print("Pitch: ");
  lcd.print(angle_pitch_output);
  lcd.setCursor(0,1);
  lcd.print("Roll: ");
  lcd.print(angle_roll_output);
  
  
  // Check Angle for Level LEDs
  
    if (angle_pitch_output < -2.01) {
    // Turn on Level LED
    digitalWrite(levelLED_neg1, HIGH);
    digitalWrite(levelLED_neg0, LOW);
    digitalWrite(levelLED_level, LOW);
    digitalWrite(levelLED_pos0, LOW);
    digitalWrite(levelLED_pos1, LOW);
    
    } else if ((angle_pitch_output > -2.00) && (angle_pitch_output < -1.01)) {
    // Turn on Level LED
    digitalWrite(levelLED_neg1, LOW);
    digitalWrite(levelLED_neg0, HIGH);
    digitalWrite(levelLED_level, LOW);
    digitalWrite(levelLED_pos0, LOW);
    digitalWrite(levelLED_pos1, LOW);
    
    } else if ((angle_pitch_output < 1.00) && (angle_pitch_output > -1.00)) {
    // Turn on Level LED
    digitalWrite(levelLED_neg1, LOW);
    digitalWrite(levelLED_neg0, LOW);
    digitalWrite(levelLED_level, HIGH);
    digitalWrite(levelLED_pos0, LOW);
    digitalWrite(levelLED_pos1, LOW);
    
    } else if ((angle_pitch_output > 1.01) && (angle_pitch_output < 2.00)) {
    // Turn on Level LED
    digitalWrite(levelLED_neg1, LOW);
    digitalWrite(levelLED_neg0, LOW);
    digitalWrite(levelLED_level, LOW);
    digitalWrite(levelLED_pos0, HIGH);
    digitalWrite(levelLED_pos1, LOW);
    
    } else if (angle_pitch_output > 2.01) {
    // Turn on Level LED
    digitalWrite(levelLED_neg1, LOW);
    digitalWrite(levelLED_neg0, LOW);
    digitalWrite(levelLED_level, LOW);
    digitalWrite(levelLED_pos0, LOW);
    digitalWrite(levelLED_pos1, HIGH);
    
    }
    
  displaycount = 0;
  
  }
  
 
 while(micros() - loop_timer < 4000); 
 //Reset the loop timer                                
 loop_timer = micros();
  
}
 
void setup_mpu_6050_registers(){
 
  //Activate the MPU-6050
  
  //Start communicating with the MPU-6050
  Wire.beginTransmission(0x68); 
  //Send the requested starting register                                       
  Wire.write(0x6B);  
  //Set the requested starting register                                                  
  Wire.write(0x00);
  //End the transmission                                                    
  Wire.endTransmission(); 
                                              
  //Configure the accelerometer (+/-8g)
  
  //Start communicating with the MPU-6050
  Wire.beginTransmission(0x68); 
  //Send the requested starting register                                       
  Wire.write(0x1C);   
  //Set the requested starting register                                                 
  Wire.write(0x10); 
  //End the transmission                                                   
  Wire.endTransmission(); 
                                              
  //Configure the gyro (500dps full scale)
  
  //Start communicating with the MPU-6050
  Wire.beginTransmission(0x68);
  //Send the requested starting register                                        
  Wire.write(0x1B);
  //Set the requested starting register                                                    
  Wire.write(0x08); 
  //End the transmission                                                  
  Wire.endTransmission(); 
                                              
}
 
 
void read_mpu_6050_data(){ 
 
  //Read the raw gyro and accelerometer data
 
  //Start communicating with the MPU-6050                                          
  Wire.beginTransmission(0x68);  
  //Send the requested starting register                                      
  Wire.write(0x3B);
  //End the transmission                                                    
  Wire.endTransmission(); 
  //Request 14 bytes from the MPU-6050                                  
  Wire.requestFrom(0x68,14);    
  //Wait until all the bytes are received                                       
  while(Wire.available() < 14);
  
  //Following statements left shift 8 bits, then bitwise OR.  
  //Turns two 8-bit values into one 16-bit value                                       
  acc_x = Wire.read()<<8|Wire.read();                                  
  acc_y = Wire.read()<<8|Wire.read();                                  
  acc_z = Wire.read()<<8|Wire.read();                                  
  temp = Wire.read()<<8|Wire.read();                                   
  gyro_x = Wire.read()<<8|Wire.read();                                 
  gyro_y = Wire.read()<<8|Wire.read();                                 
  gyro_z = Wire.read()<<8|Wire.read();                                 
}

This sketch requires another library, the NewLiquidCrystal Library for I2C. You can download the latest version from BitBucket. It is a ZIP file, once again you will need to install this into your Arduino IDE.

Unlike the last sketch, it does not use any custom libraries for the MPU-6050. Instead, the Arduino Wire library for I2C is used and the registers in the MPU-6050 are manipulated directly.

At the beginning of the sketch in the Setup, we calibrate the sensor by reading the acceleration and gyroscope data 100 times. This is then averaged to get a baseline.  Because of this operation, the device will delay a few seconds before it gives its first readings.

Again there is a lot of math in the sketch. Unlike the previous sketch, we are just interested in obtaining the Pitch and Roll values in degrees.

As the data is extracted too quickly for the LCD display to handle we use a counter and just take a reading for every 100 passes.

During the pass that we take to update the display we also look at the Pitch value and use it to determine which LED to illuminate.

Load the sketch and run it. After a delay of a few seconds, the LCD display should show the angles, both pitch and roll. The LEDs should light appropriately.

Try moving the level around and note the readings and the LED display.  If you have a conventional level try comparing readings.

Conclusion

The MPU-6050 is a very useful device that has brought a lot of advanced technology to Arduino makers, at a very low price. It has many uses, and we will be using it again in several projects in the future (including the DB1 Robot project).

So get out your breadboard and dream up some well-balanced projects!

Resources

Electronic Level Sketch – The sketch used to create the electronic level.

Jeff Rowberg’s I2C Dev Library – The I2C development library for Arduino, by Jeff Rowberg.

Jeff Rowberg’s MPU-6050 Library – The MPU-6050 library for Arduino, by Jeff Rowberg.