Building a Self-Balancing Robot: Basic Balancing is Working

A lot has happened since my last blog. The big news is that basic balancing is working now. Let’s start with the now almost obligatory photo of the current robot state:

2010-06-12-16-20-26-0741  2010-06-12-16-20-46-075

For better weight distribution the batteries, motor controller and the Arduino Mega board have been rearranged. The gray box between the wheels underneath the robot platform houses the 3-axis accelerometer ADX330 and the dual axis gyroscope IDG300. I used the same components as in my Lego prototype (for details see Building a Self-Balancing Robot – The Prototype). All connections are fed into the protoboard that sits on top of the Arduino Mega. The wooden bar with caster wheels at the end is used to prevent the robot from completely falling over. Naturally the caster wheels don’t touch the ground when the robot is balancing.

Enough about the hardware. The vast bulk of the changes since my last blog entry are software related. All the source code is now available as open source and can be accessed from the Google code hosting site The source code includes the Arduino program with its libraries plus a wxPython UI application that is used to communicate with the Arduino controller to retrieve tilt and speed data and to send calibration data to the board. This blog is based on the code as of revision 37.

Let’s start with …

The Arduino Program

The main program is BalancingRobot.pde. It uses a number of custom libraries that encapsulate the details of various ‘function blocks’. The code for these libraries resides under the folder Arduino\libraries. The majority of the libraries are straight forward:

  • ADXL300: Access to the ADXL330 accelerometer and associated angle calculations
  • IDG300: Access to and calibration of the IDG300 gyroscope
  • QuadratureEncoder: Handling of the quadrature encoder signals

Others deserve a little bit of explanation.

Communicating with the Sabertooth Motor Driver

The setup of the Sabertooth motor driver has slightly changed since my last post. I now use the much more capable Packetized Serial Mode to talk to the controller. The details are encapsulated in the Sabertooth classSabertooth.h. Using a serial port to communicate with the motor driver has the advantage that apart from the motor drive signals, we can also specify a cutoff voltage. Once the voltage drops below the specified threshold the driver will automatically stop the motors, thus protecting the batteries.

The Sabertooth class is used from within the Arduino program like so:

#include <HardwareSerial.h>
#include <Sabertooth.h>

// Sabertooth address 128 set with the DIP switches on the controller (OFF OFF ON ON  ON ON)
// Using Serial3 for communicating with the Sabertooth motor driver. Requires a connection
// from pin 14 on the Arduino Mega board to S1 on the Sabertooth device.
Sabertooth _Sabertooth = Sabertooth(128, &Serial3);


void setup()

 // give Sabertooth motor driver time to get ready

 // Setting baud rate for communication with the Sabertooth device.



Another advantage is that only one of the inputs (S1) of the motor driver is used for speed control. The S2 input is configured as an active-low emergency stop. Having an emergency stop is essential when powerful motors are involved! Here is my ‘high-tech’ solution for pulling S2 low when a cord is ripped:


Tilt Angle Calculation and Balancing Algorithm

As discussed in my earlier post Building a Self-Balancing Robot – The Prototype, it is necessary to fuse the output of the accelerometer with the output of the gyroscope to get a reliable tilt angle. This is accomplished by a Kalman filter (TiltCalculator.h and TiltCalculator.cpp) that is based on code from Trammell Hudson.

The tilt angle and the angular velocity are passed to the balancer class (Balancer.h). It uses four coefficients to calculate the torque:

float CalculateTorque(float angleError, float angularVelocityError, float positionError, float velocityError)
 float torque =
 angleError * K1 +
 angularVelocityError * K2 +
 positionError * K3 +
 velocityError * K4;

 return torque;

Currently I only use the first two terms. This allows for simple balancing but does not prevent the robot from drifting. The wheel encoders give the information that is required to plug in the last two terms but I just haven’t gotten around doing it yet. David P. Anderson’s excellent nBot pages include a succinct high-level description of the balancing algorithm. A more technical short description of the ‘Segway’ or inverted pendulum can be foundhere.

Provided all the required physical properties of the robot are known, the four parameters K1, … K4 can be calculated. However, in my case it is a matter of trial and error – lots of trial and error. This requires an effective way to see the values that are calculated by the control algorithm and to send parameters to the controller. This ability is provided by a Python desktop application.

The Robot Control User Interface

My ‘native’ language is C# but, inspired by a visit to Willow Garage and their use of Python in ROS, I decided to give Python a try. Python per se is fairly easy to learn; but add to it the desire to decide on a UI framework and to learn how to use it in conjunction with a plotting library and you end up with a bigger job than I was originally planning for. Anyway, I decided on using wxPython as the UI framework, Pydev for Eclipse as the IDE, andmatplotlib as the plotting library. I got it working but I am sure I am breaking the Zen of Python in numerous ways.

I won’t describe the code in detail. It can be downloaded from the Google code project page drh-balancing-robotalong with the controller code discussed above. The browsing entry point is Again, please note that this blog entry is based on revision 37 of the code. The code is very likely to change in the future.

The starting point for the UI app is \UI\MainApp. It creates the MainModel which contains the logic behind the UI and then brings up the main application window ( This window contains two plot panels. The plot panel on the top shows a real time graph of

  • The raw tilt angle as determined from the accelerometer values
  • The tilt angle value that is calculated from the fusion of the accelerometer and the gyroscope values
  • The angular velocity as measured by the gyroscope

The lower panel shows a real time graph of the speed of the two motors. I used it to calibrate the speed controller logic. However, since it is not used for balancing I’ll ignore it here. Here is a short video that shows the real time graphs while I tilt the robot back and forth.

Menu entries allow the user to define or change the balancing parameters and speed controller (again, not used here) parameters and send them to the Arduino controller.

The bidirectional communication with the Arduino controller over the serial port is handled by \Model\DataAdapter .py in conjunction with Support\


The robot is stable as long as it is not pushed too hard. Unfortunately the used wheelchair motors have quite a bit of play in their gears resulting in jerky back and forth swings around the center point. Also the motors are quite slow and hence cannot compensate for even moderate pushes of the robot. Running the motors at 24V instead of the current 12V will improve the speed issue somewhat but ultimately I am afraid the motors might not cut it. I will post a video of the balancing in action shortly.

Building a Self-Balancing Robot – The Prototype

Like many others I am fascinated by two-wheeled, self-balancing robots. I am currently in the process of building a self-balancing robot that should be big enough to allow me to ride on it. However, before I blog about this new project I want to briefly introduce a LEGO prototype that I built a couple of months ago. The structure and the motors are LEGO but as the ‘brain’ I used the Make Controller Kit (see image below). The Make controller board can be seen in the center. Only the larger diameter wheels are used for balancing; the small wheel that is visible in the front (there is another one in the back) is only there to prevent the robot from falling over too far if the power is off or the balancing fails.


The following short video shows the balancing in action. At the end of the video I turn off the power supply, making the robot ‘fall’ as a result.

The tilt sensor consists of the 3-axis accelerometer ADX330 and the dual axis gyroscope IDG300. The x and z accelerations from the ADX330 sensor are used to calculate the raw tilt angle. It is accurate provided the sensor is static; i.e., the acceleration is purely based on the gravitational forces. In a moving robot the actual forces are based on the gravitational forces and the acceleration of the robot itself. To compensate for acceleration the gyroscope sensor is used. The tilt angle is calculated by integrating over the angle velocity that is measured by the gyroscope. This calculated tilt angle is heavily impacted by drift and hence cannot be used in isolation. For this reason the output of the gyroscope and the output of the accelerometer are combined via a Kalman filter.

The following graph shows the difference between the angle that is calculated from the accelerometer (blue) and the angle based on the application of the Kalman filter (red):


The calculated tilt angle is passed into a PID loop that is used to control the motors. For details see the complete controller code. In addition to the code that calculates the tilt angle and drives the motor, the code also contains two asynchronous USB tasks (UsbReceiverTask and UsbSenderTask) that were used to communicate directly with the PC. This allowed for fairly efficient debugging of the code and fine tuning of the various control parameters as can be seen in the screen shot below. The included graph shows the tilt angle (red), the angular rate (black) and the normalized speed (green) over time. It is clearly visible how the speed oscillates in order to keep the robot balanced. Below the graph the PID parameters and parameters that are used to assemble the input into the PID loop are shown.


As a prototype the robot worked very well. The balancing was very stable as long as the robot was not pushed. When pushed the robot would counteract but would start moving in the direction that it was pushed towards. This shortcoming is caused by the fact that the control code has no insight into the speed of the robot. The typical remedy is to add wheel encoders to measure the speed and incorporate the speed signal into the PID algorithm. Since wheel encoders are a bit cumbersome I decided not to bother with them for the prototype. I will use wheel encoders in the ‘official’ project though.


I learned a lot from what other balancing robot builders and Segway clone builders have published. Here is a list of some of the links (in no particular order) that I used: