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

How to connect a battery to the Arduino

When working with sensitive circuits and micro-controllers, batteries can be a tricky thing to get right. It is not as simple as just buying a battery with the same voltage as what the micro-controller requires; the issue with batteries is that their voltage does not remain constant. The voltage stated on the battery usually tells you the approximate voltage when it is at 50% charge. The voltage tends to be slightly higher when fully charged, and drops below this value as it is drained. Battery power level indicators work by analysing this voltage and then calculating how much power is left.

Most Arduino compatible micro-controllers have several different ways in which they can be powered. Through the (1) USB jack, the (2) Power Jack, the (3) 5V Pin, and the (4) Vin Pin. Many Arduino’s also have a 3.3V pin, however this is only a voltage output and cannot be used to power the board. The voltage input (Vin) pin and power jack are both connected to the same power regulation circuitry, which can take in voltages between 7-12V and convert them into stable 5V used by the micro-controller. If you use the 5V pin to provide power, this regulation circuitry is bypassed and feeds the chip directly. This means that if the voltage provided to the 5V pin deviates by as little as ±0.1v, you could potentially fry the board.

As the voltage of a battery varies with use, I would always connect it through the Vin pin or the power jack. This allows the power regulation circuitry to deal with any variances in the voltage and prevent the battery from frying the board.

However, frying the board isn’t the only danger when using a battery! As the battery is drained and its voltage drops below the micro-controller’s minimum voltage requirement, a brown-out can occur. Brown-outs happen when the micro-controller partially shuts-down due to a power shortage. These usually cause the controller to restart, but they could also potentially damage the controller. Some batteries (especially LiPos) also are damaged if they are over-discharged and the voltage drops too low. This makes it important to monitor the voltage of the battery during use, so that we know when the battery level is too low and it needs to be recharged.

Protecting your Battery from being Over-discharged

The best way to prevent a brown-out is to make sure that the voltage being supplied is always above the minimum required voltage for the controller. If the battery falls below this minimum voltage, the micro-controller should display a warning message, or switch itself off. A nice way of dealing with this is to make the micro-controller monitor its own power supply by using one of the analogue input pins. If the voltage of the battery is less than 5V, it can be connected directly into the analogue pin of an Arduino. Otherwise, we will have to use a potential divider circuit in order to reduce the voltage to a safe level.

For my second version of the self-balancing robot, I am using a 11.1V lithium quad-copter battery to power a Galileo Gen2 Board and two 12V motors. As the voltage of the battery is greater than 5V, I used the following circuit in order to monitor the battery as it is being used:

When two resistors are connected in series like this, with one end connected to a certain voltage and the other to ground, the voltage at the junction of the two resistors is determined by the ratio of the two resistances. The formula is as follows, where (Vin) is the input voltage, (R1) and (R2) the resistances of the first and second resistor, and (Vdiv) the voltage at the junction between both resistors:

The ratio of the resistors should be chosen, so that the voltage at the junction (Vdiv) is below 5V when the battery is at its maximum voltage. Since the Arduino measures the lower voltage (Vdiv) at the junction of both resistors, this formula can be rearranged to calculate the voltage of the battery (Vin):

The code I use to monitor the battery percentage on the Arduino is shown below. The same formula is used on line 22 to calculate the actual battery voltage. For the 11.1V LiPo battery I used, the maximum voltage at full charge was 12.6V. The minimum voltage below which it should no longer be used was 10.2V.

// Pin number of the analog input being used for the reading
#define ANALOG_PIN 2

// Values for the two resistors used in the potential divider
float resistorOne = 100000;
float resistorTwo = 47000;

float vRef = 5;               // Reference voltage - usually 5v or 3.3v
float maxVoltage = 12.6;      // The voltage of the battery when fully charged
float minVoltage = 10.2;      // Voltage below which battery isn't safe to use

void setup() {
    // Initialize serial communication
    Serial.begin(9600);
}

void loop() {
    // Calculate voltage at the analog pin
    float voltage = analogRead(ANALOG_PIN) * vRef / 1024.0;

    // Calculate the battery voltage
    voltage = voltage * (resistorOne / resistorTwo + 1);

    // Calculate the battery percentage
    float batteryPercentage = (voltage - minVoltage) / (maxVoltage - minVoltage) * 100;

    // Limit the percentage value to be between 0 to 100%
    if (batteryPercentage > 100) batteryPercentage = 100;
    else if (batteryPercentage < 0) batteryPercentage = 0;

    // Output the calculated values to the serial monitor
    Serial.print(voltage); Serial.print("V, ");
    Serial.print(batteryPercentage); Serial.println("%");
    delay(1000);
}

The serial output I got from the system then looked like this:

11.07V, 36%
11.29V, 45%
11.29V, 45%
11.29V, 45%

In my test, the actual voltage of the battery according to the Lithium battery charger was 11.3V, so the value recorded by the micro-controller was relatively accurate.

Here is a quick summary of the points I discussed above:

1. Never connect a battery directly into the 5v or 3.3v pins of the micro-controller. These inputs require the voltage to be properly regulated.

2. Always ensure that the battery is within  the controller’s specified voltage input range. For the Arduino Uno this is 7-12V.

3. Avoid brown-outs and battery over-discharge by monitoring the voltage of the battery and switching off the system if it drops below the minimum voltage.

4. When monitoring the voltage, take caution that the voltage being measured does not exceed the reference voltage (5V). Reduce the voltage by using a potential divider circuit.

Updated: 24th May 2019 – Reformatted post

Updated: 28th May 2020 – Added extra detail about the potential divider circuit

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

Why build a self-balancing robot?

The aim of a self-balancing robot is to balance itself on two wheels, being able to drive around without toppling over. Self-balancing robots use a “closed-loop feedback control” system; this means that real-time data from motion sensors is used to control the motors and quickly compensate for any tilting motion in order to keep the robot upright. Similar self-balancing feedback control systems can be seen in many other applications. Some of the obvious examples include Segways, bipedal robots and space rockets (A few rockets have been lost due to a faulty balancing system).

But what many people don’t realise is that often the same type of controller is also used in a large variety of other applications which aren’t related to balance. Proportional-integral-derivative (PID) controllers are used by elevators to control their motion and position, used by air-conditioning units to control the temperature within a room, and even used to control the operation of jet engines. Of course rockets use significantly more complex controllers than air-conditioners, but the underlying principle is still the same: how to adjust the system in order to get as close to the desired target value (be it temperature, angle, or position) as possible. That is why building a self-balancing robot is so educational; you can use the same control methods over and over again for other projects. And don’t forget, self-balancing robots are a fun toy to play with! Here is a video of my self-balancing in action:

A Brief Overview

It takes several steps to build a self-balancing robot. The easiest part is the hardware, so that is always a good place to start. The robot requires two motors, a motor controller, a sensor to detect its current tilt angle, a micro-controller and some type of frame. Surprisingly enough due to the inertia of an inverted pendulum, the robot actually finds it easier to stabilise if the frame is very tall with a lot of weight on top, instead of a small frame with a low centre of gravity!

Once all of the physical components have been assembled, we can proceed to the tricky stuff; the software. As the system is inherently unstable and wants to topple over, the micro-controller needs to continuously monitor the current angle of the robot and be as fast and efficient as possible in order to retain its balance. While the loop time (refresh rate) should be short, it also needs to remain regular so that the control system can properly performs its calculations. For my robot I used a standard loop time of 10ms (100Hz). Therefore, the controller recalculates its response 100 times per second!

Required Components

Before we dive into how to make a self-balancing robot, we need to get our hardware together. Here is a list of the required component, with some links as reference:

• Essential:
• 1 x Arduino-compatible Controller
• 1 x Power Supply/Battery
• 1 x Motor Controller
• 1 x Gyroscope/Accelerometer Module (Eg. MPU-6050)
• 2 x High Torque Gearbox Motors
• Colour-coded wires, preferably single core
• Some type of frame!

• Optional:
• Some buttons to control the robot
• A on/off power switch
• A number of colourful LEDs

The links above are for reference only! I recommend that you look around for the best deals before buying anything.

I decided to use the Intel Galileo Gen2 Development Board (now discontinued) for my micro-controller as I still had a couple lying around, and it is compatible with the Arduino environment. In reality you could use any board (Eg. Uno, Mega, Red-board) to make your self-balancing robot. In all of my robots I used the MPU-6050 sensor, which contains both a gyroscope and an accelerometer. The benefit of using both of the sensors together is that their data can be fused, giving us much stabler and more accurate readings for angle.

To create my frame I designed all of the components on the computer using SketchUp, and then sent them to be 3D printed. If you don’t have access a printer, an alternative would be to build your own frame out of scrap parts! I built my first self-balancing robot prototype out of lollipop sticks and glue, and I was surprised how well it worked! A frame design which I have commonly seen other people implement is to use a couple of plastic/wood rectangles as platforms, and to connect these using a long threaded bolt and some nuts (as shown in the diagram below).

Please leave a comment below if you have any questions or suggestions. In the next part of this tutorial I will cover the basics of how accelerometer and gyroscope sensors work, and how we can use them in our robot.

Updated: 23rd May 2019 – Reformatted post

Updated: 29th May 2020 – Updated the descriptions and reformatted post