Up until now we have looked at all of the individual topics behind self-balancing robots. In this final part of the tutorial, I’ll bring it all together and give you some guidelines to designing and assembling your own robot!

Designing the Robot

Weight Distribution: Self-balancing robots work on the principle of an inverted pendulum. This means that the system is most stable when all of the mass is positioned as high as possible. This seems to go against common sense; usually systems are more stable when they have a low centre of gravity. In this case keeping the mass on top increases the inertia of the system, meaning that the robot has more time to respond to changes in balance. Therefore my first recommendation is to place the heaviest objects, such as the battery, at the top of the robot.

Sensor Positioning: The positioning of the accelerometer/gyroscope module is also important. When I was demonstrating my balancing robot at the Dublin Maker Faire this year, I asked a number of people where they think the sensor should be positioned. Most guessed that it should be on top, as this is where it would record the largest amount of movement!

We actually want to avoid as much of this translational movement as possible, as we are only interested in the rotation of the robot. Therefore the sensor should be placed exactly on the axis of rotation, between both wheels. Placing the sensor further up on the frame introduces noise and jitter into the readings, and may cause a feedback loop (similar to the squeaking noise made when a microphone is too close to its own speaker).

Frame Design: For the rest of the frame, it is up to your own imagination what you want to do with it. I’ve included a couple of pictures and sketches below to help you come up with your own designs. Although I 3D printed two of my frames, I made my first prototype out of lollipop sticks (and it worked really well)!

Assembling the Frame

Putting together the frame and electronics is actually the easiest part of the project! Once you have your frame designed and ready to go, all you have to do is stick/screw all of the components together. Here is a schematic I made to help you with the wiring of the robot:

Combined Program

In the previous parts of the tutorial I included snippets of code to show you how each part of the self-balancing robot should work. Here I have compiled all of the parts together into one code that you can use and modify for your own robot. I included a horrendous amount of comments, so that the program is as easy to follow as possible!

Note: This code is programmed for the specific components I was using, such as an Arduino motor shield, and the MPU6050 Accel-Gyro module.

/* * * * * * * * * * * * * * * * * * * * * *
 * SELF-BALANCING ROBOT
 * =========================================
 *
 * Code by: Simon Bluett
 * Email: hello@chillibasket.com
 * Website: wired.chillibasket.com
 *
 * 7/10/15, Version 2.0
 *
 * Here are some hints when you try to use this code:
 *
 *  > Ensure pin-mapping is correct for your robot (line 54)
 *  > Ensure calibration values are correct for your sensor (line 181)
 *  > Uncomment (line 700) in order to see if your sensor is working
 *  > Play with your PID values on (line 93)
 *  > Ensure that your left & right motors aren't inverted (line 355)
 *  > Confirm whether you want the Pitch ypr[1] or Roll ypr[2] sensor readings!
 * * * * * * * * * * * * * * * * * * * * * */


/* * * * * * * * * * * * * * * * * * * * * *
 *  This Demo makes use of the I2Cdev and MPU6050 libraries, and the demonstration
 *  sketch written by (Jeff Rowberg <jeff@rowberg.net>), modified to work
 *  with the Intel Galileo Development Board:
 *  -- -- -- -- -- -- -- -- -- -- -- -- -- --
 *  I2Cdev device library code is placed under the MIT license
 *  Copyright (c) 2012 Jeff Rowberg
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in
 *  all copies or substantial portions of the Software.
 * * * * * * * * * * * * * * * * * * * * * */



// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <Wire.h>

// Specific I2C addresses may be passed as a parameter here
MPU6050 mpu;        			// Default: AD0 low = 0x68


// Define the pin-mapping
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
#define DIR_A 12                // Direction Pin, Motor A
#define DIR_B 13                // Direction Pin, Motor B
#define PWM_A 3                 // PWM, Motor A (Left Motor)
#define PWM_B 11                // PWM, Motor B (Right Motor)
#define BRK_A 9                 // Brake, Motor A
#define BRK_B 8                 // Brake, Motor B

#define BTN_1 10                 // On/Off Button
#define BTN_2 7                 // Set Centre of Gravity Button

#define LED_1 5                 // Low-battery Warning LED
#define LED_2 4                // Current mode LED


// Max PWM parameters
#define MAX_TURN 30


// MPU Control/Status
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
bool dmpReady = false;         	// Set true if DMP init was successful
uint8_t devStatus;              // Return status after device operation (0 = success, !0 = error)
uint8_t mpuIntStatus;           // Holds actual interrupt status byte from MPU
uint16_t packetSize;            // Expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;             // Count of all bytes currently in FIFO
uint8_t fifoBuffer[64];         // FIFO storage buffer


// Orientation/Motion
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
Quaternion q;                   // [w, x, y, z]       Quaternion Container
VectorFloat gravity;           	// [x, y, z]            Gravity Vector
int16_t gyro[3];               	// [x, y, z]            Gyro Vector
float ypr[3];                   // [yaw, pitch, roll]   Yaw/Pitch/Roll & gravity vector
float averagepitch[50];        	// Used for averaging pitch value


// For PID Controller
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
float Kp = 8;                   // (P)roportional Tuning Parameter
float Ki = 2;					// (I)ntegral Tuning Parameter        
float Kd = 5;					// (D)erivative Tuning Parameter       
float lastpitch;                // Keeps track of error over time
float iTerm;              		// Used to accumulate error (integral)
float targetAngle = 2.1;       	// Can be adjusted according to centre of gravity 

// You can Turn off YAW control, by setting
// the Tp and Td constants below to 0.
float Tp = 0.6;        			// Yaw Proportional Tuning Parameter
float Td = 0.1;					// Yaw Derivative Tuning Parameter
float targetYaw = 0;            // Used to maintain the robot's yaw
float lastYawError = 0;

float PIDGain = 0;				// Used for soft start (prevent jerking at initiation)


// Motor Control
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int direction_A = 0;            // 0 - Forwards, 1 - Backwards
int direction_B = 0;            //
int brake_A = 1;                // 1 - On, 0 - Off
int brake_B = 1;                //


// Runtime variables
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int modeSelect = 1;             // System Mode (0 = off, 1 = normal, 2 = guided)
bool initialised = true;        // Is the balancing system on?

char inchar = 0;                // Hold any incoming characters
float angular_rate = 0;         // Used to make sure rate is ~0 when balance mode is initiated

bool newCalibration = false;	// If set TRUE, the target angles are recalibrated


// Variables used for timing control
// Aim is 10ms per cycle (100Hz)
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
#define STD_LOOP_TIME 9

unsigned long loopStartTime = 0;
unsigned long lastTime;             // Time since PID was called last (should be ~10ms)

// 0 = Off, 1 = On
int modes = 0;



// ------------------------------------------------------------------
// 					      INITIAL SETUP
// ------------------------------------------------------------------

void setup() {

    Wire.begin();

    // Initialize serial communication for debugging
    Serial.begin(115200);
	
	 // Configure LED for output
    pinMode(LED_1, OUTPUT);
    pinMode(LED_2, OUTPUT);

    digitalWrite(LED_1, LOW);
    digitalWrite(LED_2, LOW);

    // Set as input, internal pullup for buttons
    pinMode(BTN_1, INPUT_PULLUP);
    pinMode(BTN_2, INPUT_PULLUP);

    // Configure Motor I/O
    pinMode(DIR_A, OUTPUT);     // Left Motor Direction
    pinMode(DIR_B, OUTPUT);     // Right Motor Direction
    pinMode(BRK_A, OUTPUT);     // Left Motor Brake
    pinMode(BRK_B, OUTPUT);     // Right Motor Brake

    // Initialize MPU6050
    mpu.initialize();
    Serial.println("Testing MPU connection:");

    Serial.println(mpu.testConnection() ? "> MPU6050 connection successful" : "> MPU6050 connection failed");
    Serial.println("Initialising DMP");
    devStatus = mpu.dmpInitialize();

    /* * * * * * * * * * * * * * * * * * * *
     * IMPORTANT!
     * Supply your own MPU6050 offsets here
     * Otherwise robot will not balance properly.
     * * * * * * * * * * * * * * * * * * * */
    mpu.setXGyroOffset(93);
    mpu.setYGyroOffset(-15);
    mpu.setZGyroOffset(30);
    mpu.setXAccelOffset(-2500);
    mpu.setYAccelOffset(1783);
    mpu.setZAccelOffset(877);

    // Make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        Serial.println("Enabling DMP");
        mpu.setDMPEnabled(true);
        mpuIntStatus = mpu.getIntStatus();

        // Set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println("DMP Ready! Let's Proceed.");
        Serial.println("Robot is now ready to balance. Hold the robot steady");
        Serial.println("in a vertical position, and the motors should start.");
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();

    } else {
		// In case of an error with the DMP
        if(devStatus == 1) Serial.println("> Initial Memory Load Failed");
        else if (devStatus == 2) Serial.println("> DMP Configuration Updates Failed");
    }

}



// -------------------------------------------------------------------
// 			 PID CONTROLLER
// -------------------------------------------------------------------

int PID(float pitch) {            

    // Calculate time since last time PID was called (~10ms)
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    unsigned long thisTime = millis();
    float timeChange = float(thisTime - lastTime);

    // Calculate Error
    float error = targetAngle - pitch;


    // Calculate our PID terms
    // PID values are multiplied/divided by 10 in order to allow the
    // constants to be numbers between 0-10.
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float pTerm = Kp * error * 10;
    iTerm += Ki * error * timeChange / 10;  
    float dTerm = Kd * (pitch - lastpitch) / timeChange * 100; 
	
	if (Ki == 0) iTerm = 0;
    lastpitch = pitch;
    lastTime = thisTime;


    // Obtain PID output value
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float PIDValue = pTerm + iTerm - dTerm;

    // Set a minimum speed (motors will not move below this - can help to reduce latency)
    //if(PIDValue > 0) PIDValue = PIDValue + 10;
    //if(PIDValue < 0) PIDValue = PIDValue - 10;

	// Limit PID value to maximum PWM values
    if (PIDValue > 255) PIDValue = 255;
    else if (PIDValue < -255) PIDValue = -255; 

    return int(PIDValue);

}



// -------------------------------------------------------------------
// 			 YAW CONTROLLER
// -------------------------------------------------------------------

int yawPD(int yawError) {            


    // Calculate our PD terms
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float pTerm = Tp * yawError;
    float dTerm = Td * (yawError - lastYawError) / 10; 
	
    lastYawError = yawError;

    // Obtain PD output value
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    int yawPDvalue = int(-pTerm + dTerm);

	// Limit PD value to maximum
    if (yawPDvalue > MAX_TURN) yawPDvalue = MAX_TURN;
    else if (yawPDvalue < -MAX_TURN) yawPDvalue = -MAX_TURN; 

    //Serial.print("Error: ");
    //Serial.print(yawError);
    //Serial.print(" - PD: ");
    //Serial.println(yawPDvalue);
    return yawPDvalue;

}



// -------------------------------------------------------------------
// 			 	MOVEMENT CONTROLLER
// -------------------------------------------------------------------
// This function calculate the PWM output required to keep the robot 
// balanced, to move it back and forth, and to control the yaw.

void MoveControl(int pidValue, float yaw){
	
    // Set both motors to this speed
    int left_PWM = pidValue;
    int right_PWM = pidValue;


    /* YAW CONTROLLER */

    // Check if turning left or right is faster
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    int leftTurn, rightTurn;

    float newYaw = targetYaw;

    if((yaw > 0) && (newYaw < 0)){
        rightTurn = yaw + abs(newYaw);
        leftTurn = (180 - yaw) + (180 - abs(newYaw));

    } else if ((yaw < 0) && (newYaw > 0)){
        rightTurn = (180 - abs(yaw)) + (180 - newYaw);
        leftTurn = abs(yaw) + newYaw;

    } else if (((yaw > 0) && (newYaw > 0)) || ((yaw < 0) && (newYaw < 0))){
        rightTurn = newYaw - yaw;

        if (rightTurn > 0){
            leftTurn = rightTurn;
            rightTurn = 360 - leftTurn;
        } else if (rightTurn < 0){
            rightTurn = abs(rightTurn);
            leftTurn = 360 - abs(rightTurn);
        } else if (rightTurn == 0){
            rightTurn = leftTurn = 0;
        }
    }

    // Apply yaw PD controller to motor output
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    if ((leftTurn == 0) && (rightTurn == 0)){
        // Do nothing
    } else if (leftTurn <= rightTurn){
    	leftTurn = yawPD(leftTurn);
        left_PWM = left_PWM - leftTurn;
        right_PWM = right_PWM + leftTurn;

    } else if (rightTurn < leftTurn){
        rightTurn = yawPD(rightTurn);
        left_PWM = left_PWM + rightTurn;
        right_PWM = right_PWM - rightTurn;
        
    }


    // Limits PID to max motor speed
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    if (left_PWM > 255) left_PWM = 255;
    else if (left_PWM < -255) left_PWM = -255; 
    if (right_PWM > 255) right_PWM = 255;
    else if (right_PWM < -255) right_PWM = -255; 

    // Send command to left motor
    if (left_PWM >= 0) Move(0, 0, int(left_PWM));   	// '0' = Left-motor, '1' = Right-motor
    else Move(0, 1, (int(left_PWM) * -1));
	// Send command to right motor
    if (right_PWM >= 0) Move(1, 1, int(right_PWM)); 	// '0' = Forward, '1' = Backward
    else Move(1, 0, (int(right_PWM) * -1));    

}



// -------------------------------------------------------------------
// 			 MOTOR CONTROLLER
// -------------------------------------------------------------------

void Move(int motor, int direction, int speed) {            

	// Left Motor
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
	if (motor == 0){
	
		// Set motor direction (only if it is currently not that direction)
		if (direction == 0){
            if (direction_A == 1) digitalWrite(DIR_A, HIGH);		// Forwards
			direction_A = 0;
		} else {
			if (direction_A == 0)  digitalWrite(DIR_A, LOW);		// Backwards
			direction_A = 1;
		}
        
		// Release brake (only if brake is active)
		if (brake_A == 1){
			digitalWrite(BRK_A, LOW);
			brake_A = 0;
		}
		
		// Send PWM data to motor A
		analogWrite(PWM_A, speed);


    // Right Motor
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
    } else if (motor == 1){
	
		// Set motor direction (only if it is currently not that direction)
		if (direction == 0){
			if (direction_B == 1) digitalWrite(DIR_B, HIGH);		// Forwards
			direction_B = 0;
		} else {
			if (direction_B == 0)  digitalWrite(DIR_B, LOW);		// Backwards
			direction_B = 1;
		}
        
		// Release brake (only if brake is active)
		if (brake_B == 1){
			digitalWrite(BRK_B, LOW);
			brake_B = 0;
		}
		
		// Send PWM data to motor A
		analogWrite(PWM_B, speed);


    // Stop both motors
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
    } else if (motor = 3){  

        analogWrite(PWM_A, 0);
        analogWrite(PWM_B, 0);
        digitalWrite(BRK_A, HIGH);
        digitalWrite(BRK_B, HIGH);
        brake_A = 1;
        brake_B = 1;

    }
}



// -------------------------------------------------------------------
// 			 READ INPUT FROM SERIAL
// -------------------------------------------------------------------

void readSerial() {

    // Initiate all of the variables
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
	int changestate = 0;		// Which action needs to be taken?
	int no_before = 0;			// Numbers before decimal point
	int no_after = 0;			// Numbers after decimal point
	bool minus = false;			// See if number is negative
	inchar = Serial.read();		// Read incoming data

    if (inchar == 'P') changestate = 1;
    else if (inchar == 'I') changestate = 2;
    else if (inchar == 'D') changestate = 3;

    // Tell robot to calibrate the Centre of Gravity
    else if (inchar == 'G') calibrateTargets();


    // Records all of the incoming data (format: 00.000)
    // And converts the chars into a float number
    if (changestate > 0){
        if (Serial.available() > 0){

            // Is the number negative?
            inchar = Serial.read();
            if(inchar == '-'){
                minus = true;
                inchar = Serial.read();
            }
            no_before = inchar - '0';

            if (Serial.available() > 0){
                inchar = Serial.read();

                if (inchar != '.'){
                    no_before = (no_before * 10) + (inchar - '0');

                    if (Serial.available() > 0){
                        inchar = Serial.read();
                    }
                }

                if (inchar == '.'){
                    inchar = Serial.read();
                    if (inchar != '0'){
                        no_after = (inchar - '0') * 100;
                    }

                    if (Serial.available() > 0){
                        inchar = Serial.read();
                        if (inchar != '0'){
                            no_after = no_after + ((inchar - '0') * 10);
                        }

                        if (Serial.available() > 0){
                            inchar = Serial.read();
                            if (inchar != '0'){
                                no_after = no_after + (inchar - '0');
                            }
                        }
                    }
                }
            }

            // Combine the chars into a single float
            float answer = float(no_after) / 1000;
            answer = answer + no_before;
            if (minus) answer = answer * -1;

            // Update the PID constants
            if (changestate == 1){
                Kp = answer;
                Serial.print("P - ");
            } else if (changestate == 2){
                Ki = answer;
                Serial.print("I - ");
            } else if (changestate == 3){ 
                Kd = answer;
                Serial.print("D - ");
            }
            Serial.print("Constant Set: ");
            Serial.println(answer, 3);

        } else {
            changestate = 0;
        }
    }
}



// -------------------------------------------------------------------
// 			 RECALIBRATE TARGET VALUES
// -------------------------------------------------------------------
// Takes a number of readings and gets new values for the target angles.
// Robot must be held upright while this process is being completed.

void calibrateTargets(){

	targetAngle = 0;
	targetYaw = 0;
	
    for(int calibrator = 0; calibrator < 50; calibrator++){
	
		accelgyroData();
		targetAngle += pitch();
		targetYaw += yaw();
		delay(10);
	}
	
	// Set our new value for Pitch and Yaw
	targetAngle = targetAngle / 50;
	targetYaw = targetYaw / 50;
	Serial.print("Target Pitch: ");
	Serial.print(targetAngle, 3);
	Serial.print(", Target Yaw: ");
	Serial.print(targetYaw, 3);

	newCalibration = false;
}



// -------------------------------------------------------------------
// 			 GET PITCH AND YAW VALUES
// -------------------------------------------------------------------
// This simply converts the values from the accel-gyro arrays into degrees.

float pitch(){
	return (ypr[1] * 180/M_PI);
}

float yaw(){
	return (ypr[0] * 180/M_PI);
}

float angRate(){
	return -((float)gyro[1]/131.0);
}



// -------------------------------------------------------------------
// 			 GET ACCEL_GYRO DATA
// -------------------------------------------------------------------

void accelgyroData(){

    // Reset interrupt flag and get INT_STATUS byte
    mpuIntStatus = mpu.getIntStatus();

    // Get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // Check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // Reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println("Warning - FIFO Overflowing!");

    // otherwise, check for DMP data ready interrupt (this should happen exactly once per loop: 100Hz)
    } else if (mpuIntStatus & 0x02) {
        // Wait for correct available data length, should be less than 1-2ms, if any!
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();


        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        // Get sensor data
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGyro(gyro, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        mpu.resetFIFO();

        //Serial.print(ypr[1]);
        //Serial.print(" - ");
        //Serial.println(ypr[0]);
    }
}



// -------------------------------------------------------------------
// 			 MAIN PROGRAM LOOP
// -------------------------------------------------------------------

void loop() {

	// If the "SET" button is pressed
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
	if (digitalRead(BTN_2) == LOW){

		digitalWrite(LED_1, HIGH);
	    calibrateTargets();

	    lastpitch = 0;
	    iTerm = 0;

	    Serial.println("> Setting new centre of gravity <");

		delay(250);
	    mpu.resetFIFO();
	    digitalWrite(LED_1, LOW);
	}


	// If the "POWER" button is pressed
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
	if (digitalRead(BTN_1) == LOW){
	    if (modeSelect == 1){
	        Serial.println("> Turning off balancing system <");
	        initialised = false;
	        modeSelect = 0;
	        Move(3,0,0);        // Stop both motors from moving
	        digitalWrite(LED_2, LOW);
	    } else if (modeSelect == 0){
	        Serial.println("> Turning on balancing system <");
	        initialised = false;
	        modeSelect = 1;
	        digitalWrite(LED_2, HIGH);
	    }
	    delay(500);
	    mpu.resetFIFO();
	}
		
	// Gather data from MPU6050
	accelgyroData();
		
	// If the Balance System is turned on:
	if (modeSelect == 1){
				
		if (!initialised){

	        // Wait until robot is vertical and angular rate is almost zero:
	        if ((pitch() < targetAngle+0.1) && (pitch() > targetAngle-0.1) && (abs(angRate()) < 0.3)){
	            Serial.println(">>>> Balancing System Active <<<<");
	            initialised = true;
	            lastpitch = pitch();
	            iTerm = 0;
	        }
	
	    // Otherwise, run the PID controller
		} else {

			// Stop the system if it has fallen over:
			if ((pitch() < -45) || (pitch() > 45)){
					
				// Stop the motors
				Move(3, 0, 0);
				// Reset runtime variables
				lastpitch = 0;
				iTerm = 0;
				initialised = false;
				Serial.println(">>>> Balancing System Stopped <<<<");

			} else {
				// A bit of function-ception happening here:
				//Serial.println(pitch());
				MoveControl(PID(pitch()), yaw());
			}
		}
	}

    if (Serial.available() > 0){    // If new PID values are being sent by the interface
        readSerial();               // Run the read serial method
    }

    // Call the timing function
    // Very important to keep the response time consistent!
    timekeeper();
}



// -------------------------------------------------------------------
// 			 	TIME KEEPER
// -------------------------------------------------------------------

void timekeeper() {

    // Calculate time since loop began
    float timeChange = millis() - loopStartTime;

    // If the required loop time has not been reached, please wait!

    if (timeChange < STD_LOOP_TIME) {
        delay(STD_LOOP_TIME - timeChange);
    } 


    // Update loop timer variables
    loopStartTime = millis();   
}

Here is a breakdown of how to use this code with your robot:

1. Before starting the program, connect the board via USB to your computer, and open a terminal window in the Arduino software (baud rate: 115200).
2. Wait until robot is ready: At the start the robot automatically initialises the MPU6050 module. Once this is done, the following message should appear:
   DMP Ready! Let's Proceed.
3. Set Centre of Gravity: You should set the centre of gravity of the robot, so that the robot knows which way is up! Do this by steadily holding the robot upright, with the wheels off the floor, and pressing the button connected to GPIO-4. The LED on GPIO-2 will flash, and the following message should appear:
   > Setting new centre of gravity <
4. Automatic On/Off: The motors of the robot automatically turn off if the robot has fallen over, or is lying on its side. To turn them back on, hold the robot steadily in an upright position. The motors should start and the following message appear:
   >>>> Balancing System Active <<<<
   If the robot has fallen over and motors are off, this message appears:
   >>>> Balancing System Stopped <<<<
5. Manual On/Off: To manually turn the balancing system on/off, press the button connected to GPIO-7. The LED on GPIO-10 will be bright, if the balancing system is turned on. One of the following messages will appear to let you know which state the robot is in:
   > Turning off balancing system <
> Turning on balancing system <
6. Sending new PID values: Please read my guide The PID Controller, to see how to calibrate your robot. You can send new PID values via the console window, by typing the letter of the constant (P, I or D) you want to set, followed by the number you want to set it at. Then press the enter/return key to send. The code accepts any numbers between 0.01 – 99.99. For example:
   P8.2 I1.51 D15
   This sets the [P]roportional Constant to 8.2, the [I]ntegral Constant to 1.51, and the [D]erivative Constant to 15.

Dealing with Common Errors

I have found that most of the common errors can be dealt with by checking the following:

1. Check the Pin Mapping: Make sure that the GPIO number on (lines 54-65) match up with the ones you are using on your robot.
2. Update the MPU-6050 Offsets: Each sensor has unique offset values, which have to be inputted on (lines 183-188). I explain how to find these offsets in my “Calibrating & Optimising the MPU6050” part of this tutorial.
3. Ensure sensor is working properly: Check that the MPU-6050 is working, by uncommenting (line 704) of my code. While running, the robot should display the current angle on the console. When held upright, the angle should be 0. When pitching forwards/backwards, the number should be positive/negative in degrees.
4. Both motors should spin in same direction: Set the turning constants on (lines 102-103) to zero. Now both motors should spin in same direction. If not, then one of the motors is wired backwards.
5. Motors balance in wrong direction: Instead of stopping the robot from falling, the motors speed up the fall. This means that both motors are wired in backwards!

This finally concludes my tutorial about self-balancing robots! If you have any questions or suggestions, please leave a comment below.

Updated: 23rd May 2019 – Reformatted post

The most important element of any robot is the controller. The control algorithm determines how the robot should react to different sensor inputs, allowing it to intelligently adapt to and interact with its environment. Especially for a self-balancing robot, the control program is vital as it interprets the motion sensor data and decides how much the motors need to be moved in order for the robot to remain stable and upright. The most common controller used for stabilisation systems is the PID controller. So let’s look at how it works…

The PID Controller

PID stands for Proportional, Integral and Derivative, referring to the mathematical equations used to calculate the output of the controller. This is perhaps the most common type of controller used in industry, as it is able to control relatively complex systems even though the calculations are actually quite straightforward (making it easy to program and fast to compute). Mathematically, the PID controller can be described by the following formula:

u(t)=K_pe(t) + K_i\int_{0}^{t}e(t)dt +K_d\frac{de(t)}{dt}

In this formula, the output of the controller u(t) is determined by the sum of three different elements, each dependent on the error e(t). The error simply is the difference between the target value and the current value (measured by the sensor). Now let’s quickly look at what each of the terms in the equation is doing for our robot…

K_pe(t)

This is the proportional component, which takes the current error value and multiplies it by a constant number (Kp). For our self-balancing robot, this simply takes in the current angle of the robot and makes the motors move in the same direction as the robot is falling. The further the robot falls off target, the faster the motors move. If the P-component is used on its own, the robot might stabilise for a while, but the system will tend to overshoot, oscillate and ultimately fall over.

K_i\int_0^te(t)dt

The integral component is used to accumulate any errors over time, and multiplies this accumulated value by a constant number (Ki). For example if the robot tends to fall over to one side, it knows that it needs to move in the opposite direction in order to keep on target and to prevent drifting left or right.

K_d\frac{de(t)}{dt}

Finally, the derivative component is responsible for dampening any oscillations and ensures that the robot does not overshoot the target value. Each time the controller is called, this term calculates the change in the error value and multiplies it by a constant number (Kd). Often this is simplified to calculate only the change in the current sensor value, rather than the change in error. If the target position remains constant, this gives the same result. This simplification helps to prevent sudden jumps in the output value which happen when the target position it changed.

Summing all three of these terms together then gives us an output value which we can send to the motor of the self-balancing robot. However, before the controller can successfully balance the robot, the three constants (Kp), (Ki) and (Kd) need to be tuned to suit our specific application. By increasing or decreasing the values of these constants, we control how much each of the three components of the controller contributes to the output of the system.

The formula I used above is called the “Independent” PID equation. If you read up on the PID controller online, you may also see the formula for the algorithm expressed in a different format (known as the “Dependent” PID equation):

u(t) = K_ce(t)+\frac{K_c}{T_i}\int_0^te(t)dt + K_cT_d\frac{de(t)}{dt}

The term (Kc) is known as controller gain, (Ti) the integral time and (Td) the derivative time. This format can be useful when using some automated tuning techniques, however the final result is the same.

Implementing the Controller

Now that we know the basic theory, we can start to write this in code. The basic layout of the function is shown below. The controller can be used by calling the “pid” function at regular intervals, providing the target position and the current position (as recorded by the sensor) as parameters of the function. The function then outputs the calculated result of the controller. Tuning of the controller can be done by changing the values of the (Kp), (Ki) and (Kd) variables at the top of the code.

// Declare variables
float Kp = 7;          // (P)roportional Tuning Parameter
float Ki = 6;          // (I)ntegral Tuning Parameter        
float Kd = 3;          // (D)erivative Tuning Parameter       
float iTerm = 0;       // Used to accumulate error (integral)
float lastTime = 0;    // Records the time the function was last called
float maxPID = 255;    // The maximum value that can be output
float oldValue = 0;    // The last sensor value

/**
 * PID Controller
 * @param  (target)  The target position/value we are aiming for
 * @param  (current) The current value, as recorded by the sensor
 * @return The output of the controller
 */
float pid(float target, float current) {
	// Calculate the time since function was last called
	float thisTime = millis();
	float dT = thisTime - lastTime;
	lastTime = thisTime;

	// Calculate error between target and current values
	float error = target - current;

	// Calculate the integral term
	iTerm += error * dT; 

	// Calculate the derivative term (using the simplification)
	float dTerm = (oldValue - current) / dT;

	// Set old variable to equal new ones
	oldValue = current;

	// Multiply each term by its constant, and add it all up
	float result = (error * Kp) + (iTerm * Ki) + (dTerm * Kd);

	// Limit PID value to maximum values
	if (result > maxPID) result = maxPID;
	else if (result < -maxPID) result = -maxPID;

	return result;
}

What does this program do?

First of all, the algorithm calculates the time since the last loop was called, using the “millis()” function. The error is then calculated; this is the difference between the current value (the angle recorded by the sensor), and the target value (the angle of 0 degrees we are aiming to reach).

The PID values are then calculated and summed up to give an output for the motors. The output is then constrained to ±255 as this is the maximum PWM value that can be output to the motors of the self-balancing robot.

Although this program is almost complete, I found that my robot only worked well once I included a timing function. This is a system that ensures the PID controller function is called at regular intervals. In my self-balancing robot, I set the loop time to be 10ms (meaning the function is run 100 times per second). Here is the timer code and a sample loop function:

// Any variables for the PID controller go here!
float targetValue = 0;

// Variables for Time Keeper function:
#define LOOP_TIME 10          // Time in ms (10ms = 100Hz)
unsigned long timerValue = 0;

/******** SETUP ************/
void setup() {
	// Put all of your setup code here
	timerValue = millis();
}

/******* MAIN LOOP *********/
void loop() {
	// Only run the controller once the time interval has passed
	if (millis() - timerValue > LOOP_TIME) {
		timerValue = millis();

		// Replace getAngle() with your sensor data reading
		float currentValue = getAngle();

		// Run the PID controller
		float motorOutput = pid(targetValue, currentValue);

		// Replace moveMotors() with your desired output
		moveMotors(motorOutput);
	}
}

/****** PID CONTROLLER *****/
float pid(float target, float current) {          

	// PID code from above goes in here!
	return result;
}

Unfortunately this is not the end of the story! Although the PID controller code is complete, suitable PID constants still need to be found to tune the controller for your specific robot. These constants depend on things such as weight, motor speed and the shape of the robot, and therefore they can vary significantly from robot to robot. Here is a quick explanation of how you should go about calibrating your PID values:

Calibrating your PID Controller

1. Create some way in which you can change the PID constant of your robot while it is running. One option is to use a potentiometer or some other analogue input to be able to increase or decrease the PID constant. I personally used the USB connection and the serial monitor to send new PID values. This is important as you can then see straight away how well the new PID values are working, and you won’t have to re-upload the code hundreds of times!
2. Set all PID constants to zero. This is as good a place to start as any…
3. Slowly increase the P-constant value. While you are doing this, hold the robot to make sure it doesn’t fall over and smash into a million pieces! You should increase the P-constant until the robot responds quickly to any tilting, and then just makes the robot overshoot in the other direction.
4. Now increase the I-constant. This component is a bit tricky to get right. You should keep this relatively low, as it can accumulate errors very quickly. In theory, the robot should be able to stabilise with only the P and I constants set, but will oscillate a lot and ultimately fall over.
5. Raise the D-constant. A lot. The derivative components works against any motion, so it helps to dampen any oscillations and reduce overshooting. I found that this constant has to be set significantly higher than the other two (x10 to x100 more) in order to have any effect. All the same, don’t set it too high, as it will reduce the robot’s ability to react to external forces (aka. being pushed around).
6. Spend many fruitless hours slightly modifying the PID values. This is probably the longest part of the procedure, as there isn’t much of a method to it. You just have to increase and decrease the values until you reach that perfect sweet-spot for your robot!

Please leave a comment below if you have any questions or suggestions.

Updated: 23rd May 2019 – Reformatted post and improved the code snippets.

Updated: 1st June 2020 – Add PID equations and improved the descriptions.

In this part of the tutorial I will cover how to get the most performance out of the MPU-6050 Accelerometer and Gyroscope module, using the “Motion Apps” library. This library is really amazing as the author reverse engineered how to use the Digital Motion Processor (DMP) integrated within the MPU-6050. This allows all the complicated sensor processing and fusion to be done using the DMP instead of by the micro-controller! I have found that reading the sensor through the Motion Apps Library is faster than doing the calculations manually, and the readings tend to be significantly more accurate.

1. Analysing the Output

Here is the code that I used to get the yaw, pitch and roll sensor data. It is based on the “Teapot” example sketch which comes with the MPU-6050 Motion Apps library. To use this sketch, you will need to have the “MPU6050” and the “I2Cdev” libraries installed.

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class 
// using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.

   For the Galileo Gen1/2 Boards, there is no INT pin support. Therefore
   the INT pin does not need to be connected, but you should work on getting
   the timing of the program right, so that there is no buffer overflow.
 * ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * ========================================================================= */


#define OUTPUT_READABLE_YAWPITCHROLL

// Unccomment if you are using an Arduino-Style Board
// #define ARDUINO_BOARD

// Uncomment if you are using a Galileo Gen1 / 2 Board
#define GALILEO_BOARD

#define LED_PIN 13      // (Galileo/Arduino is 13)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
VectorFloat gravity;    // [x, y, z]            gravity vector
Quaternion q;           // [w, x, y, z]         quaternion container
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

// This function is not required when using the Galileo 
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(115200);
    while (!Serial);

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(F("MPU6050 connection "));
    Serial.print(mpu.testConnection() ? F("successful") : F("failed"));

    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available

    #ifdef ARDUINO_BOARD
        while (!mpuInterrupt && fifoCount < packetSize) {
        }
    #endif

    #ifdef GALILEO_BOARD
        delay(10);
    #endif

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

After completing several trials, I noticed that the accelerometer-gyroscope module and the Motion Apps library seem to have some type of auto-calibration feature, which requires a couple of seconds to complete. Here is a graph of the orientation output I got directly after initiating the sensor:

There is a large variation in the values as the sensor starts up, especially in the yaw data. This sensor drift stops after around 13 seconds, probably due to the completion of an integrated auto-calibration process. I repeated this test a number of times and it seems that the sensor can on some occasions take up to 40 seconds to complete its calibration. Therefore we should take this delay into account in our program… If we want to use the yaw data, the robot should wait for around 40 seconds before beginning to use the sensor and starting the main program. If only the pitch and roll data is used in a non-critical control system (no quadcopters!), we probably could get away with not waiting for the sensor to settle.

2. Calibrating the Sensor

Just like most sensors, the MPU-6050 needs to be calibrated before it is used for the first time. What we want to do is remove the zero-error; this refers to when the sensor records a small angle even though it is totally level. This error can be removed by applying an offset to the raw accelerometer and gyroscope sensor readings. The offset needs to be adjusted until the gyroscope readings are zero (no rotation) and the accelerometer records the acceleration due to gravity pointing directly downwards. Fortunately I found a program that can calibrate the MPU-6050 for us! The original calibration sketch can be found on the I2Cdev library forum. To use the program, first make sure that the MPU-6050 is correctly wired up to the Arduino (or equivalent). Then upload the sketch and open up the serial monitor in the Arduino IDE, setting the baud rate to 115200. To start calibration, place the accel-gyro module in a flat and level position and send any character in the serial monitor. The program will make an average of a few hundred readings and display the offsets required to remove zero error.

// Arduino sketch that returns calibration offsets for MPU6050 
//   Version 1.1  (31th January 2014)
// Done by Luis Ródenas <luisrodenaslorda@gmail.com>
// Based on the I2Cdev library and previous work by Jeff Rowberg <jeff@rowberg.net>
// Updates (of the library) should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// These offsets were meant to calibrate MPU6050's internal DMP, but can be also useful for reading sensors. 
// The effect of temperature has not been taken into account so I can't promise that it will work if you 
// calibrate indoors and then use it outdoors. Best is to calibrate and use at the same room temperature.

/* ==========  LICENSE  ==================================
 I2Cdev device library code is placed under the MIT license
 Copyright (c) 2011 Jeff Rowberg
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 
 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 =========================================================
 */

// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

///////////////////////////////////   CONFIGURATION   /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone=8;     //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone=1;     //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // <-- use for AD0 high

int16_t ax, ay, az,gx, gy, gz;

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();

  // initialize serial communication
  Serial.begin(115200);

  // initialize device
  accelgyro.initialize();

  // wait for ready
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available()){
    Serial.println(F("Send any character to start sketch.\n"));
    delay(1500);
  }                
  while (Serial.available() && Serial.read()); // empty buffer again

  // start message
  Serial.println("\nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);
  // verify connection
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);
  // reset offsets
  accelgyro.setXAccelOffset(0);
  accelgyro.setYAccelOffset(0);
  accelgyro.setZAccelOffset(0);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state==0){
    Serial.println("\nReading sensors for first time...");
    meansensors();
    state++;
    delay(1000);
  }

  if (state==1) {
    Serial.println("\nCalculating offsets...");
    calibration();
    state++;
    delay(1000);
  }

  if (state==2) {
    meansensors();
    Serial.println("\nFINISHED!");
    Serial.print("\nSensor readings with offsets:\t");
    Serial.print(mean_ax); 
    Serial.print("\t");
    Serial.print(mean_ay); 
    Serial.print("\t");
    Serial.print(mean_az); 
    Serial.print("\t");
    Serial.print(mean_gx); 
    Serial.print("\t");
    Serial.print(mean_gy); 
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:\t");
    Serial.print(ax_offset); 
    Serial.print("\t");
    Serial.print(ay_offset); 
    Serial.print("\t");
    Serial.print(az_offset); 
    Serial.print("\t");
    Serial.print(gx_offset); 
    Serial.print("\t");
    Serial.print(gy_offset); 
    Serial.print("\t");
    Serial.println(gz_offset); 
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors(){
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    accelgyro.setXAccelOffset(ax_offset);
    accelgyro.setYAccelOffset(ay_offset);
    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);
    accelgyro.setYGyroOffset(gy_offset);
    accelgyro.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

If all goes well, the sketch should output the accelerometer and gyroscope offsets through the serial monitor. It may take up to a minute for the sketch to converge on the correct offsets. If the sensor is accidentality moved during calibration, it could take even longer to complete.

To use the calibration values, all you have to do is plug them into the initialisation code at the start of any sketch in which you use the MPU-6050! Please note that the required offsets vary significantly from sensor to sensor, so you have to repeat the calibration program above for each MPU-6050 sensor you are using. That concludes the basic calibration of the MPU-6050; the sensor should now be more than accurate enough for most applications, such as self-balancing robots and quad-copters.

Updated: 23rd May 2019 – Reformatted post

Updated: 26th May 2020 – Improved the descriptions and reformatted post

If you haven’t already read the first part of this Self-balancing robot series, I would encourage you to do so now! The first section below deals with a little bit of theory behind the sensors, so if you want to get straight to the programming/building part, please feel free to skip to the second section…

Deciding which Gyroscope Module to Use

So you have decided to go ahead with the project and construct your own self-balancing robot? That’s great! Now we can start by looking at one of the most significant elements of this project; the sensor. Traditionally the sensor of preference for stabilisation is a gyroscope. Now-days gyroscopes are extremely small and very cheap to buy, so they are ideal for amateur electronics projects. Unfortunately these gyroscopes (both the cheap and the not-so-cheap versions) also come with their own problems. They are good for short-term and quick movements, but tend to drift over time as the error accumulates. They also record a lot of jitter and noise, which needs to be filtered by the micro-controller before the data can be used.

To reduce this drifting effect of the gyroscope, it is possible to combine the sensor data with that from an accelerometer. The accelerometer is good at sensing slower and more prolonged movements, rather than the fast motion. Therefore if we take the best of both worlds and fuse the data together, we will be left with an extremely accurate picture of the motion of the robot.

As a result I decided to use a combined accelerometer & gyroscope breakout module (the MPU-6050), which is slightly more expensive than a simple gyro, but should lead to a superior stabilisation performance. Note: the MPU-6050 comes with a library which does all of the sensor fusion calculations for you, so that definitely is a plus!

Getting started with the Accel-Gyro Module

The MPU-6050 uses I²C to communicate with the micro-controller, so I started by connecting up the pins as shown in the schematics: the SDA line connects to the Analog pin 4, the SCL to Analog pin 5, power input to the 3.3v pin and the ground to the GND pin. If you are using one of the newer Arduinos, you could also connect the sensor to the dedicated SDA and SCL header pins.

As I am using an Intel Galileo Gen2 board, I did not use the interrupt pin. In general it is very bad practice to disregard the interrupts, as it might cause the buffer holding the sensor data to overflow, but for some reason the Galileo does not support normal interrupts! In order to get the sensor working correctly on the Galileo Board, I spent a lot of time on getting the timing right so that there is no overflow of incoming sensor data.For all other Arduino-compatible board, you should use the interrupt so that the micro-controller deals with new sensor data the moment that it is sent. (The interrupt pin is connected to Arduino digital pin 2)

Now it is time to get some data from the accel-gyro module! To do this I simply used the sample code which came with the documentation of the MPU6050 in order to read the raw sensor data. For this sample to work, the I2Cdev and the MPU6050 libraries need to be installed. Here is the code:

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2011 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
	#include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;

#define LED_PIN 13
bool blinkState = false;

void setup() {
	// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
	Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
	Fastwire::setup(400, true);
#endif

	// initialize serial communication
	// it's really up to you depending on your project)
	Serial.begin(115200);

	// initialize device
	Serial.println("Initializing I2C devices...");
	accelgyro.initialize();

	// verify connection
	Serial.println("Testing device connections...");
	Serial.print("MPU Connection ");
	Serial.println(accelgyro.testConnection() ? "successful" : "failed");

	// configure Arduino LED
	pinMode(LED_PIN, OUTPUT);
}

void loop() {
	// read raw accel/gyro measurements from device
	accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

	// display tab-separated accel/gyro x/y/z values
	Serial.print("a/g:\t");
	Serial.print(ax); Serial.print("\t");
	Serial.print(ay); Serial.print("\t");
	Serial.print(az); Serial.print("\t");
	Serial.print(gx); Serial.print("\t");
	Serial.print(gy); Serial.print("\t");
	Serial.println(gz);

	// blink LED to indicate activity
	blinkState = !blinkState;
	digitalWrite(LED_PIN, blinkState);
}

The result I got on the Serial Monitor looked like this:

Initializing I2C devices...
Testing device connections...
MPU Connection successful 
a/g:	-1428	14240	12120	-536	131	-149
a/g:	-1416	14196	11972	-505	110	-169
a/g:	-1484	14260	11948	-524	108	-147
a/g:	-1508	14220	11968	-513	87	-151
a/g:	-1540	14176	11920	-501	15	-164
a/g:	-1408	14212	11984	-523	27	-153
a/g:	-1472	14104	11888	-526	123	-137
a/g:	-1400	14236	11936	-514	136	-148
a/g:	-1512	14216	12008	-522	132	-142
a/g:	-1412	14172	11956	-520	127	-158
a/g:	-1456	14120	11936	-533	94	-166
a/g:	-1496	14124	11936	-529	97	-161
a/g:	-1496	14208	11996	-526	108	-177
a/g:	-1420	14236	11992	-505	104	-151
a/g:	-1540	14264	11984	-510	115	-171
a/g:	-1468	14300	12068	-504	51	-143
a/g:	-1508	14172	11868	-545	105	-137
a/g:	-1416	14244	11812	-523	69	-154
a/g:	-1472	14276	11956	-504	92	-146
a/g:	-1492	14192	12028	-517	143	-175

In this data we can see the readings from the accelerometer already divided into the x/y/z values, and the readings from the gyroscope are also divided into its x/y/z components. This is a great first step, but unfortunately this data is not very usable in its current form. We still have to fuse the accelerometer and gyroscope data together, and then filter it to remove all of the noise!

Manipulating the data

It is possible to calculate the tilt of the sensor manually through the use of a number of formulas, but fortunately (at least for the MPU-6050) there is a library to do this for us! As a matter of fact, the MPU-6050 has a built-in “Motion Processing Unit” (hence the initials) which can be used to process the sensor data, therefore minimising the load on the micro-processor. This library also automatically filters the data so that we get a clean and usable result straight away.

Above is the equation used to calculate the angle of inclination from the accelerometer data, where (Ax) and (Ay) are the x- and y- accelerometer values. If you want to find out about calculating the angles manually, please visit: http://www.kerrywong.com/2012/03/08/a-self-balancing-robot-i/. Kerry Wong does an awesome job at describing the whole system, so I would encourage you to check it out! He covers all of the main topics such as the calculations, sensor fusion and the Kalman filter.

As regards the filtering, here is another great post which looks at the main advantages and disadvantages of using the a complimentary filter instead of the MPU6050 motion-apps library: http://www.geekmomprojects.com/mpu-6050-redux-dmp-data-fusion-vs-complementary-filter/

Updated: 23rd May 2019 – Reformatted post