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.

/* * * * * * * * * * * * * * * * * * * * * *
 * =========================================
 * 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() {


    // Initialize serial communication for debugging
	 // 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
    Serial.println("Testing MPU connection:");

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

    /* * * * * * * * * * * * * * * * * * * *
     * Supply your own MPU6050 offsets here
     * Otherwise robot will not balance properly.
     * * * * * * * * * * * * * * * * * * * */

    // Make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        Serial.println("Enabling DMP");
        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");


// -------------------------------------------------------------------
// -------------------------------------------------------------------

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);


// -------------------------------------------------------------------
// -------------------------------------------------------------------

int yawPD(int yawError) {            

    // Calculate our PD terms
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float pTerm = Tp * yawError;
    float dTerm = Kd * (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(" - PD: ");
    return yawPDvalue;


// -------------------------------------------------------------------
// -------------------------------------------------------------------
// 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;


    // 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));    


// -------------------------------------------------------------------
// -------------------------------------------------------------------

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;


// -------------------------------------------------------------------
// -------------------------------------------------------------------

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;

// -------------------------------------------------------------------
// -------------------------------------------------------------------
// 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++){
		targetAngle += pitch();
		targetYaw += yaw();
	// 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;

// -------------------------------------------------------------------
// -------------------------------------------------------------------
// 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);

// -------------------------------------------------------------------
// -------------------------------------------------------------------

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
        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);

        //Serial.print(" - ");

// -------------------------------------------------------------------
// -------------------------------------------------------------------

void loop() {

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

		digitalWrite(LED_1, HIGH);

	    lastpitch = 0;
	    iTerm = 0;

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

	    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);
	// Gather data from MPU6050
	// 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:
				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!

// -------------------------------------------------------------------
// -------------------------------------------------------------------

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

This Post Has 61 Comments

  1. I got the same code for the L298N on intel galileo but in the serial dont show nothing and dont moved.

    1. It’s really difficult to help you out as your description of the problem is very vague! Have you tried testing the components separately with the Galileo? Are the components wired properly? Is the power plugged in… 🙂

  2. Hi! my question is, why use the yaw control at all?

    1. I included it as I planned to use it later to be able to steer and drive the robot around with a wireless controller. I also found that it helps to keep the robot facing the same direction, even if the motors are slightly mismatched.

  3. nice tutorial ..thank you very much ..nice lesson step by step

  4. Hello Simon,
    Thank you very much for the great tutorial! I have assembled the robot and am using the L298N motor controller, your code modified per your instructions. I have run code to test both the L298N (code here: http://tronixstuff.com/2014/11/25/tutorial-l298n-dual-motor-controller-modules-and-arduino/) and the MPU6050 (code here: http://playground.arduino.cc/Main/MPU-6050#short).
    However, when I open the serial monitor, all I get is “Testing MPU connection” and then nothing else. I don’t have real switches wired in–just using jumpers for testing, nor do I have LED’s (using a freshly charged LiPo).
    Any ideas where I’ve gone wrong?

    [Update] Found the possible source of the problem–line 173 says printf, where it should be serial.println (?)

    1. Yes you can remove that line. Printf() does work on the Galileo micro-controller I was using, but may not work for some Arduino boards.

      Other issues may be:
      > Incorrect wiring of the sensor – test the sensor using some sample code first to make sure it is working.

    2. Thanks for the input, Simon. I have tested the sensor and input new MPU offset values. The problem I’m having now comes with the L298N motor controller. I modified the code per your earlier post. As long as I do not power the L298N on, the serial monitor displays the pwm correctly. As soon as I turn the motor power on, the motors immediately start spinning one way or the other and the serial monitor either freezes or returns “nan” until I reset the board.
      One of the other little problems is that it never starts automatically. I always have to manually enable the balancing system. I’m using an UNO board, BTW.

    3. (1) If the program freezes just as the motors turn on, then you might be experiencing a power brown-out. This happens when the motors use up all available power, causing the Arduino to turn off. Check that your power supply can provide enough current. Alternatively, you can use a separate power supply just for the motors. What are the specs on your LiPo?
      (2) To turn the balancing system on by default change the variable on line 120 to “int modeSelect = 1;”.
      (3) If your MPU-6050 has a logic voltage of 3.3V and you don’t have any logic voltage level shifter on the I/O lines, you will be more prone to getting misreadings of the sensor data. The Arduino works at 5V, so it will only have a margin of (3.3 – 2.5 = 0.8V) for detecting a high data bit. In most cases, the sensor will probably still work, but it can lead to some unexpected errors.

    4. Simon,
      1. I have a 2-cell (7.4V) 1300 mAh battery. All of this is with the USB providing power to the UNO though.
      2. I did have it turned off-thanks!
      Got it working! It turns out that the UNO board I have does not have the PWM-capable pins marked with a tilde like most other boards, so I assumed that a change had been made to support PWM on all pins and just used whatever pin looked good. I switched them to honour known PWM-capable pins and problem solved! Also, my battery is old and dies quickly, so a new one is in order.

      Thanks again for your help!!!!!!

  5. Hi Mr. Simon, first one I’d like to tell you your robot is awesome, congratulations. second one, I understand you chose to implement Galileo to process capability about it, I’m interesting to implement it on Arduino Uno to academic’s applications but, I have a little question, could you tell me what kind of DC motor are you using? you’re using DC 5V motor or 12V motor.
    And one more time, congratulation excellent blog.

    1. Hi, all of the details about the parts I used are in the first post of this tutorial. I used two 12V DC motors, and an Arduino Motor Shield to drive them.

    2. hi again, i have a question about buttons and leds number pin, or your code you’re using different pins that your schematics, i don’t know if it’s only a mistake or i have to follow code and schematic as you have them?

    3. It’s a mistake, thanks for spotting it. I’ve updated the code now to suit the schematic.

    4. I implement your code and schematic but im not sure what happen, only one motor is working, i tested two motor with a DC power supply and they’re fine or my problem can be by my switches could be damaged?

  6. Hi there Simon, I managed to adapt your code for my MD25 motor controller, but it seems I’m having some problems in making the motors run in correct direction.
    My idea is to change

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

    And remove the * -1 should fix my problem, because my robot is running on – as backwards and + as forward. The thing is that whenever I do that and upload my code it won’t run.. In serial it will always loop in Testing MP ( Looks like its trying to print Testing MPU…, but keeps looping it with a cut off). Hope you understand what I mean and give me a quick reply.

    1. Hi, removing the -1 will not solve your problem. What the -1 is doing is changing the negative PID result into a positive number which can be used as a PWM output. PWM values have to be in range of 0-255. To adapt the code to work with your motor controller, all you have to change is the code in the “void Move(int motor, int direction, int speed)” function, as well as your pin numbers. The easiest way to change the direction the motor is spinning is to swap the ‘+’ and ‘-‘ wires connected to the motor.

  7. If I don’t want to use 2 button like you, How can I change this code? Please help me!!

  8. Hi, i tried to connect it to button pin 5 instead of pin 7, pin 3 and 4 for led . i modified the code as per the adafruit motor shield like as you said. it works but i have doubt what does this mean shows in error column . But still robot is trying to stand

    sketch\MPU6050_6Axis_MotionApps20.h:639:52: warning: left shift count >= width of type [enabled by default]

    data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);

  9. Hello Simon,
    Your tutorial on self balancing robot is awesome, i am new to programming and was searching a lot on net couldn’t understand how to start. your post really helped a lot .
    i have used adafruit motor shield v1 and tried to connect it as similarly. everything seem to be fine but the motors run only when the button 2 is pressed. tell me how can i solve it .
    thanks for your great post ,
    Maria dass

    1. Is BTN_2 connected to power or to ground? Try changing the value to modeSelect = 1 on line 120 of the code. Those the robot work now?
      I think that the code for the Adafruit Motor shield is slightly different than for the motor controller that I was using. Adafruit has a separate library for its shield, and you can see some examples of how to use it here.

  10. heeelllpppp me please… i make all conection right but the motors dont work :(, i´m use arduino uno, MPU 6050, drive l298n, i´m follow all steps of your tutorial ( include the changes with l298n) but it dont work… in serial monitor, bauds is 115200 but this showing only symbols., but i think than code run correct because when i press one of the buttons, LED bright… i need realy realy your help… pls take me back. (sorry for my bad english, i´m Brazilian

    1. That does sound weird. Could you send me a picture of your setup, showing how the Arduino and motor controller is wired up?

  11. Hi Simon, after all, gratz for this amazing tuto,
    I’m Brazilian , I’m following your tutorial, but I’m using an Arduino ( UNO ) and L298N bridge, i make ​​the right connections and the change in the code as you indicated , but nothing happens when I turn it on, and the “Serial Monitor” shows only symbols … Bauld is the same as the code. sorry for my bad English, and grateful for an answer.

    1. The serial monitor only shows symbols? This usually happens only when the baud rate is wrong, or when there are other devices connected to the ‘0’ and ‘1’ digital pins on the Arduino. Have you tested each of the parts of the robot individually, to see if they work (IMU, motor controller, etc.)?

  12. Hello Simon I want to say thanks a lot about your tutorial for the self balancing robot it was really helpful. I have just one question, I have two xbee module and i’m trying to use them for controlling my robot buy your code do you have any idea how to implement and use them and thank you very much.

    1. Hi, sorry for the delay! Integrating xBees is pretty easy; you can just connect the RX and TX pins on the xBee shield to those on the Arduino. You can then use the xBees as if they simply are a wireless serial port cable. Check out this tutorial about using xBees.

  13. Hi Simon, very nice project man, but I am using the L298N shield insted of your shield could you please help me to change the code for me.

    1. I’ve already given a solution to the L298N motor controller in a previous comment here [link]. If you have any questions about the fix, please let me know!

  14. Hi! Mr.Simon
    When I write this Serial.println(PIDValue); so PWM sent on, the motor is constantly a no (i.e. 3) continuously. couldn’t understand why only PWM is 3 sent to the motor.. Please Suggest.

    1. That is weird; was your code working normally before you put that one line in? Are you using the same code that I have displayed in the post above? If the PWM value really does stay at around 3, then the only possible explanation is that the sensor is not working properly. Common problems with the sensor are: incorrect wiring, incorrect pin-mapping, wrong offsets, or that the sensor is broken.

  15. Hi Simon. First, thank you very much for sharing this. I bought android controlled balance robot for final project. But I could not. I had little time. I used the code you’re sharing but it did not work. Why could it be? I am using the L293. I made the right connections. But it did not… thank you.

    1. Hi there! Before I can give you some advice, I’ll need to know a lot more about the robot you are using, and what exactly ‘did not work’. Here are a couple of questions:

      1) What micro-controller are you using (you mentioned Android; if so, then this is the wrong tutorial for you)?
      2) Is your pin-mapping correct? I know that the L293 requires different wiring, which I have already discussed in the comments here.
      3) What accelerometer or gyroscope are you using?
      4) Have you tested to see if all of the components work individually?
      5) What happened when you tried to run the code?
      6) How experienced are you with programming & electronics?

    2. http://www.robimek.com/android-kontrollu-dengede-duran-robot-yapimi/
      I built the circuit here. First I need to do balance, then I’ll do the last part of the android. I connect pins correctly; I checked a lot of times. I am using MPU6050. I checked all the elements, my electronic and software information not bad. I use the Arduino. I hope you will help me, thank you.

    3. Hi GökhN, sorry for the delayed reply, I’m very busy with college exams at the moment. In my post above I have a section which discusses how to use my program and how to deal with common errors. What exactly do you see on the serial monitor when the robot is running? How does it differ from the normal running of the program which I described above? You could try printing out the sensor angle, or the PWM value to the serial (as I described in this comment) to see if there are any bugs with the sensor.

  16. Hi Simon! After all, due to Your tutorial, I made my robot to stand vertical! Thanks!
    But I want to show/print the PWM (Speed) of the motors on my Software Serial Monitor. So what changes should I want to make on your Code! Please Suggest. Thanks in Advance.

    1. Well done in getting your robot to work! To output the PWM value of the motors to serial, simple insert the following line on (line 252) of my code above:

      Please note: outputting data over serial is a slow process, so continuously outputting the PWM value will increase the latency of the robot a little bit.

  17. Hi Simon, I’m on my first try, but seems I don’t connect the Motor PIN correctly. Nothing happens on the motors; Even the most little Brrrr 😉 . I’m using a L298N Motor Driver module Board. I checked with an other Arduino program for testing and it ran, not perfectly, but let me know that module is running. Besides it, the reference of Pins are not same as yours. It looks like ENA IN1 IN2 IN3 IN4 ENB. what is the correspondence with yours? I tried different ways to plug without success. Please I need your help.
    In the serial window, when I run your program, it prints “DMP Ready! Let's Proceed.“, then stops. What should happen after?

    Thks in advance.

    1. Hi Serge, the wiring for the L298N Motor Driver is slightly different than for the one I am using. Below I have included what the required changes to my code. Please read through this tutorial first, so that you understand the syntax I am using.

      You should attach the wires as following:

      • in1 to GPIO-12
      • in2 to GPIO-13
      • in3 to GPIO-8
      • in4 to GPIO-9
      • enA to GPIO-11
      • enB to GPIO-3

      Next you have to update some parts of the code in order for the motor controller to work, as the syntax is slightly different:

       * I've included line numbers where you can insert each part
       * into my self-balancing robot code.
      // (line 54-59) - Replace pin mapping with the following:
      #define IN_1 12;
      #define IN_2 13;
      #define PWM_A 11;
      #define IN_3 8;
      #define IN_4 9;
      #define PWM_B 3;
      // (line 165-168) - Replace initialisation with the following:
      pinMode(IN_1, OUTPUT);     // Motor A - 1
      pinMode(IN_2, OUTPUT);     // Motor A - 2
      pinMode(IN_3, OUTPUT);     // Motor B - 1
      pinMode(IN_4, OUTPUT);     // Motor B - 2
      // (line 369-433) - Replace motor controller function with this:
      // -------------------------------------------------------------------
      //           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(IN_1, HIGH);        // Forwards
                      digitalWrite(IN_2, LOW);
                  direction_A = 0;
              } else {
                  if (direction_A == 0){
                      digitalWrite(IN_1, LOW);        // Backwards
                      digitalWrite(IN_2, HIGH);
                  direction_A = 1;
              // Release brake (only if brake is active)
              if (brake_A == 1){
                  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(IN_3, HIGH);        // Forwards
                      digitalWrite(IN_4, LOW);
                  direction_B = 0;
              } else {
                  if (direction_B == 0){
                      digitalWrite(IN_3, LOW);        // Backwards
                      digitalWrite(IN_4, HIGH);
                  direction_B = 1;
              // Release brake (only if brake is active)
              if (brake_B == 1){
                  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(IN_1, LOW);
              digitalWrite(IN_2, LOW);
              digitalWrite(IN_3, LOW);
              digitalWrite(IN_4, LOW);
              brake_A = 1;
              brake_B = 1;
    2. Thanks Simon for your fast reply, I did what you told me nicely. I noticed a change. The motors have turned :). Beside it, reaction of the robot is not the expected, I guess it is now I have to calibrate the PID? May you tell me what should exactly happen when I enter P .. then I … The robot is always on vertical position, when I enter P .. then P again, etc. what should happen? Should I try to balance it to see it reaction ?
      Thks a lot for all your reply 🙂


    3. A number of people have asked about how to use the program, so I am thinking of making a video to explain. I’ve updated the post above to include a quick breakdown of how to use the program, and how to deal with some common errors. Hopefully this will answer your questions!

    4. Ur Last post is a good help!!
      I didn’t understood the way to enter the P I D value 😉
      Beside it what the
      >>>> Initializing <<< >>> Stopped <<<<
      messages mean? Video will surely be a good help too!

      Thks so much for all ur sharing.


    5. I have updated the program code in my post, since you first used it, so
      >>>> Initialing <<<< is now called >>>> Balancing System Active <<<< and
      >>>>Stopped <<<< is now >>>> Balancing System Stopped <<<<. This is the automatic fall-detection system turning the motors on/off.

      I’ve tried to make the PID updating method easier to understand, but there isn’t much more I can do without a video. Unfortunately it will be at least 2-3 weeks until I have time to post the video.

    6. Hi Simon, some news about my project 🙂
      Yesterday evening I finally succeeded to make it “standing up” alone, I started to initialise PID … but, because there is one “but”, I think the motors I use are too greedy on power 🙁
      For now I’m using some “normal” batteries, not rechargeable, my mistake. Even it is right level on voltage, it empty them too fast.
      It will be the next step, I have to modify it soon 😉
      A little question anyway, I noticed that for correcting it position, it is tendency to turn on itself, as right motor respond more than left, it is not on right line as I thought it should be. Do you think it is due some setting or the powering?

      Have a good day

    7. My code has an integrated yaw controller. This is a function that uses the sensor in order to prevent the robot from changing direction. Obviously this isn’t working well on your robot. Try turning off my direction controller by setting the values on lines 102 and 103 to zero. Let me know if this makes any difference!

  18. Hi Simon, I’m french and on the way to build my own balancing robot. There are some days long I read and read again all about your so good explain, i already calibrating my MPU 6050, with no problem. I’m not really a beginner on Arduino, but not as good as u are 🙂 I’m Using Arduino Uno. I’m close to be able to do my first rolling, I just a bit “anxious” about the calibration of PID. My wish is to add some simple IR command or sensor, later, so I will maybe need your help 🙂 Beside it I asked myself if it will be possible to delete the command line about the calibration, after it is complete and running the best, to have some more line and bytes available?
    I hope I’m clear enough, forgive my not perfect english anyway.
    Thanks for your concern.


    1. Hello!
      It’s great to know that you find my tutorials helpful! 🙂 Of course you can remove the serial communication / calibration part of the code, if you think it is no longer needed. My program only uses about 70% of the Arduino Uno’s memory, so you still have a bit of room to add additional features. Calibrating the PID controller is tricky, but there is no need to be anxious!

    2. Thks again, I will come back, soon ….. will let you know how things ran 😉 …
      I will like “create” my own MIP, a little dream 🙂


    3. Simon, I keep for now the first program you made, I will see later for replacing the “automatic” part.
      I succeeded to make the wheels reacting on the movement.. but there still is a problem, maybe due the battery power I use for the motors. It seems they are not reacting enough to the pulse sent to them for really turning frankly, and just turn slowly.
      Is there a value (as speed) linked to the program that can be influencing this? Or should I really look my side about the powering!

      I run forward slowly, but it is passionating 🙂 thats cool.


    4. It really depends on the motors you are using. For example if they are rated for 12v, then the power supply for the motor controller should also be 12v. If the voltage is correct, then the only other way of increasing the speed is to increase the [P]-value constant of the PID controller.


    5. Good evening Simon,

      I finally succeeded to make it standing up a while 🙂 Houra !
      I changed the 102 & 103 line to 0, as u suggested, it is more right deplacement for sure and better for now.
      Is there a way to share with you a short video ?
      I have to do progress with D .. (i changed P I and D on p i and d, was easier to enter )

      Next Step: Level battery status.. that’s important! Then IR command or some distance sensor… what a challenge!

      Did you make some change your side, some “evolution” on yours? I think some bluetooth or wifi can be good too, but I haven’t such card for adding to my Arduino. IR is for me the easiest solution. The distance sensor will make it closer to the MIP robot, able to reacting to some gestures.

      I hope having some feedback from you.

      PS maybe, having some more personal chat will be easier.

    6. Hi, well done with your robot! You could either send me an email with the video, or you could upload it to YouTube and send me the link.

      I personally added a battery monitoring system to self-balancing robot, and I have all of the equipment to control the robot via WiFi. Unfortunately I haven’t found the time to get the WiFi controller to work yet, especially because the Galileo Gen2 Controller that I am using can be a bit tedious… 😛

  19. Hi Simon! When uploaded and run your code, these two problems are occurring:
    1. The program Sticks on this line and not going ahead. On Serial Monitor:
    Testing MPU connection:
    > MPU6050 connection successful
    Initialising DMP
    Enabling DMP
    DMP Ready! Let's Proceed.

    2. And when press Button_1 or Button_2, It is showing FIFO Overflowing, On Serial Monitor:
    Turning on balancing system
    Turning off balancing system
    Turning on balancing system
    Setting new centre of gravity
    Setting new centre of gravity
    Warning - FIFO Overflowing!

    I am using Arduino UNO with Interrupt 0 Pin 2 of Arduino UNO.
    Please help and resolve these; I will be thankful to You. Thanks In Advance.

    1. I don’t think the program is sticking, as what you are seeing on the Serial monitor appears to be completely normal:
      (1) The MPU6050 is initialising and connecting to Arduino successfully
      (2) Button 1 turns the Balancing system on/off (you probably pressed it more than once), hence the multiple messages.
      (3) Button 2 sets a new centre of gravity for the robot – this is the upright position the robot is trying to maintain.
      (4) Warning – FIFO Overflowing! My program does not use interrupts, so the buffer tends to overflow when the program does something else, such as dealing with the button presses. This is not a major problem and won’t affect the performance of the program.

    2. Thanks for your correction and informing about FIFO Overflowing, but a main query remains that: after all these normal behaviour showing on Serial Monitor:
      Testing MPU connection:
      MPU6050 connection successful
      Initialising DMP
      Enabling DMP
      DMP Ready! Let’s Proceed.

      Also after DMP Ready! Let’s proceed line on serial monitor, there is no movement by the motors when tilt or any position, I cross checked all my connections and mapped the pins correctly and carefully, but no idea why not any movement by the motors or any response by the system. Please, kindly let me know what is going wrong?

    3. Thanks for all your helps dear Simon. I have corrected these all, I was doing a mistake. Now motors are synchronised and working with my system. Thanks again.

  20. Dear Simon!
    Your tutorial about two wheel self balancing robot is awesome. i am working on it. I am not understanding where i can placed my Bluetooth based Tuning program on your Code. Please tell me as soon as possible, i will be great full to you.

    1. Hi! Unfortunately I can’t say much as I don’t exactly know what you are trying to do. Do you want to connect the robot, using a bluetooth module, to the computer so that you can tune the PID controller remotely? What micro-controller are you using?


    2. HI! Sorry for inconvenience dear, Yes i want to connect my android/laptop builtin blue tooth to connect my robot using Bluetooth Module, for tune the PID Controller remotely. I am using Bluetooth Module HC-05 on Robot Side with Arduino Board to connect with Android Bluetooth. I am using Arduino Uno Board.

    3. Ok from what I can see, the HC-05 can communicate to the Arduino using software serial. Digital pins 5 and 6 are the only ones not used by my program, so you could use those as RX and TX respectively. To update the PID constants, your bluetooth function will simply have to update the values of the “Kp”, “Ki” and “Kd” variables. Here is a quick guideline of where you could put your code:

       * I've included line numbers where you can insert each part
       * into my self-balancing robot code.
      // (line 47) - Include the soft serial library
      // (line 50) - Set up soft serial pins
      SoftwareSerial BTSerial(5, 6);   // RX = pin 5, TX = pin 6
      // (line 152) - This initialises the UART communication
      BTSerial.begin(38400);  // HC-05 default speed in AT command more
      // (line 625) - This is the function that will deal with incoming bluetooth messages
      void bluetoothReceive(){
          // Do something with incoming characters here!
      // (line 626) - This is the function that will deal with sending bluetooth messages
      void bluetoothSend(string message){
      // (line 711) - This checks if there are any incoming bluetooth characters
      if (BTSerial.available()){
    4. Thanks a lot Simon for such a good help and quick response. You are Great. God Bless You.

Leave a Reply