Tag Archives: BalanceBot

The Cubli – A balancing, jumping, walking robot cube !

I found this online. A robot developed at ETH Zurich. It is brilliant and beautiful. I had to share …

BalanceBot – Console App

Debugging Arduino programs can be difficult, as the only easy output method is a serial console. With lots of debug messages the console soon becomes full and difficult to read efficiently. The biggest issue is the console scrolls, so with lots of output the text scrolls past too fast to read. One solution is to write a simple app on your laptop/desktop that listens to the serial data and displays it in an app with a better layout. You can create individual text boxes that just display the values of key variables, and the app can scan the serial stream looking for these variables and update the text box every time it sees a new value.

Depending on your microcontroller code purpose, you can also chose more appropriate visual representations of your debug variables. For example, you could show an arrow on a compass to represent the value you get from a magnetometer, you can plot a point on a Google map page in and HTML control to show the position you get from a GPS, and you can display green and red dots to show the value of switches, etc.

I use Visual Basic, just because I find it easy. You can design the visual layout of the app easily, and the code is very simple, but *any* programming language will work. Pick one you are most familiar with. You don’t want to waste time building the debugging console, when you could spend that time debugging your microcontroller code.

I use two techniques in the serial protocol; I prefix each line with a symbol that defines what type of data the line contains and I use name / value pairs for data to display.


#Init Motor Driver
#Init Wire library
#Init IMU
#Get first YPR
#Set initial Input variable
#Init IMU Setpoint
#Exit Setup
$StatusA = 1011
$CountA = 4294967255
$StatusB = 11101010
$CountB = 0
$Yaw = -130.54
$Pitch = -6.03
$Roll = 23.43
$Input = 23.4341
$Output = -400
$OutputA = -400
$OutputB = -400
$Heading = -137
$Button = 1

# – denotes debug messages. I add a time stamp to them and display them in a “messages” text box.

$ – denotes a name / value pair. The value is displayed in a text box for that name.

Instead of a difficult to read scrolling text window, I now have a stable windows app that displays each variable I want to track in an easy to read layout. In addition, I have a log of messages, with time stamps, I can scroll back through looking for interesting events. Finally, you can also decide to output some values are different frequencies, i.e. the fast changing values every 500ms, and the slow changing values every 5 seconds. The app also shows the current value for everything.

You can also get your microcontroller to listen to the input serial stream and use the app to send commands to your device. Add an XBee and now your microcontroller is wirelessly remotely monitored and controlled by your app on your desktop !

BalanceBot – Laser cut Acrylic

The frame of the robot is built from MicroRax but it has no “sides”. I needed to mount some switches, etc. plus I wanted to mount the Arduino board securely and the two mounting holes in the Arduino are at very specific locations which would be better secured to a flat surface. They didn’t line up with the MicroRax.

The initial prototype had a hardboard base, but I thought I’d use the opportunity to learn how to get laser cut plastic. I found a supplier, Ponoko, which had a very simple interface. A lot of laser cutting houses use AutoCAD or Illustrator file formats to define the shape to cut. As I didn’t have an applications that can produce these, I liked Ponoko, as it used PNG. This can be generated huge number of apps, a lot of which are free.

Ponoko provide a sample template. The format is very straight forward. The colour of the line in the file defines the cut the laser will make. E.g. Cut, engrave, etc. They also have a huge array of materials, including acrylic, leather, card, wood, paper, rubber, etc.

I designed a top and a bottom. The bottom has a hole for the cables. The top is engraved (no reason other than I wanted to try it and it looks cool :-)) I didn’t get the holes for the switches laser cut, I drilled them, as I didn’t know at the time what switches I would be using.

Hardboard BalanceBot floor with Arduino PCB mounts

Hardboard BalanceBot floor with Arduino PCB mounts

PNG file used to laser cut the acrylic. Different coloured lines represent different types of cut or engraving.

PNG file used to laser cut the acrylic. Different coloured lines represent different types of cut or engraving.

Laser Cut Acryilic sheets. Cut by Ponoko.

Laser Cut Acryilic sheets. Cut by Ponoko.

Laser cut acryilic top with laser engraving

Laser cut acryilic top with laser engraving

The acrylic panels made the robot look very professional, in fact people ask me where I bought it 🙂

The panels made mounting the boards and switches trivial. I used self-adhesive PCB supports for the Arduino. The holes on the Arduino are 3.2mm. I found PCB supports 6.4mm high from RS which fitted perfectly. The Acryilic is easy to drip, so I hand drilled holes for the switches and buttons.

BalanceBot – Arduino Code

With the use of the various libraries the main code loop is very simple. Once all the objects are initialised, the loop reads the robots angle, computes the PID output and uses that to set the motor speed and direction.

void loop()
Input = ypr[IMU_ROLL];

The system worked (ish). As mentioned previously, it took a while to find the PID parameters, and they are still not perfect. However I did get a problem.

The robot would balance, but after a few minutes, both motors would just turn on full. Sometimes one motor. The strange thing was sometimes the other motor would still be managed by the PID algorithm, and would change direction when the robot was tilted back and forth, so it wasn’t as if the Arduino had crashed. I initially thought there was a short circuit or lose connection but I couldn’t find one.

I had trouble in the past with other projects, where the code is large and complex, the Arduino sometimes exhibits strange behaviour. Not quite crashing, but being erroneous. I has a suspicion the device is running out of RAM. The programs either had too many variables, or too many recursive functions. I’ve not researched a way to model programs to estimate their RAM usage, or if there is a way to report RAM usage via the code. I’m sure there’s a way. Let me know if you have any ideas.

So, I just threw hardware at the issue, and upgraded the Arduino with a Mega, which has four times the RAM (8k on the Mega verses 2k on the Uno). This solved the problem immediately. Whereas the robot would “crash” every time on the Uno, the same code worked without any issue on the Mega.

One problem was the Mega though is it’s physically longer, so it sticks out of the side of the robot 😦

The other issue was the SPI and TWI pins on the Mega are on different pins to the Uno. Not a huge problem, but until I update the shield PCB, it means jumper wires.

Full code listing below:

#include <ADXL345.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <Wire.h>
#include <EEPROM.h>
#include <FreeIMU.h>
#include <CommunicationUtils.h>
#include <DualVNH5019MotorShield.h>
#include <SPI.h>
#include <PID_v1.h>

//#define DEBUG
#include <DebugUtils.h>

// *** Duemilanova / Uno
// IMU uses I2C - pins A4 (SDA) & A5 (SCL)
// Quad counter uses SPI - pins D11 (MOSI), D12 (MISO) & D13 (SCK)

// *** Mega 2560
// IMU uses I2C - pins 20 (SDA) & 21 (SCL).
// Quad counter uses SPI - pins D50 (MISO), D51 (MOSI) & D52 (SCK)

// IMU constansts
#define IMU_YAW 0
#define IMU_PITCH 1
#define IMU_ROLL 2

#define Ku 20    // where is oscilates
#define Pu 0.4   // period of oscilation
#define Kp 0.6 * Ku
#define Ki (2 * Kp) / Pu
#define Kd (Kp * Pu) / 8

//IMU variables
float ypr[3]; // yaw/pitch/roll
FreeIMU my3IMU = FreeIMU();

//PID variables
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); //or REVERSE

DualVNH5019MotorShield md;

void setup()

  // Initialise Motor Driver:
  Serial.println("#Init Motor Driver");

  // Initialise Wire Library:
  Serial.println("#Init Wire library");

  // Initialise SPI:
  Serial.println("#Init SPI library");

  // Initialise IMU:
  Serial.println("#Init IMU");

  // Get Initial variables:
  Serial.println("#Get first YPR");

  // Initialise PID:
  Serial.println("#Set initial Input variable");
  Input = ypr[IMU_ROLL];
  Serial.println("#Init IMU Setpoint");
  Setpoint = 0;

  Serial.println("#Exit Setup");

void loop()
  Input = ypr[IMU_ROLL];

void stopIfFault()
  if (md.getM1Fault())
    Serial.println("#M1 fault");
  if (md.getM2Fault())
    Serial.println("#M2 fault");

BalanceBot – PID Controller

A  PID Controller is a software algorithm typically used to control industrial processes. A PID Controller (Proportional Integral Derivative Controller) calculates the difference between a measured variable and a desired set point. The controller attempts to minimize the error by adjusting the input.

A good example of why a PID Controller is better than other methods of control, is an electric oven. The input is the power to the heating element, the output is the temperature and the set point is the desired temperature.

Without a PID controller, If you turn the power on and off using a simple thermostat, when the temperature of the oven reaches the set point the oven will over shoot the temperature as there is residual heat left in the heating element. If you want the temperature of the oven to be 200 degrees, and have the element fully on until it reaches 200 degrees, then turn it fully off, the temperature will overshoot to maybe 205 degrees, then slowly cool until it reached 199 degrees when it will turn fully on again, however it will take time to heat up, in which time the oven has cooled to 195 degrees, and so it repeats. The oven will oscillate between 195 and 205 degrees, and not actually be a steady 200. This is fine for cooking sausages, but no good for a delicate chemical process.

The PID Controller has three values; the Proportional, the Integral and the Derivative values, denoted P, I, and D. Heuristically, these values can be interpreted in terms of time: P depends on the present error, I depends on the accumulation of past errors, and D is a prediction of future errors, based on current rate of change. By combining the three terms continuously the algorithm maintains the desired set point with minimal errors.

Simple huh ? Well kind of… The maths is a bit complex, but again, there are existing libraries that do it for you. On the Arduino site you can download a fully working PID Controller library, to integrate in to your project : http://playground.arduino.cc/Code/PIDLibrary

The only problem is you need to “tune” a PID Controller to your specific application. The P, I & D terms will be different for an oven, chemical tank, or balancing robot. There are lots of ways of tuning a PID Controller, from mathematical, to trial and error. A control systems friend of mine recommended I try the Ziegler–Nichols method.

The Ziegler–Nichols tuning method is as follows;

  • The P, I & D terms are referred to as “gains” Kp, Ki & Kd.
  • The Ki and Kd terms are first set to zero.
  • The Kp term is manually increased until the robot reaches the ultimate gain, Ku, at which point the it starts to oscillate.
  • The oscillation period Pu is measured and used to set the PID terms as follows :
  • Kp = 0.6 * Ku
  • Ki = (2 * Kp) / Pu
  • Kd = (Kp * Pu) / 8

This isn’t perfectly tuned to our specific system, but it’s a good approximation. PID tuning is a black art that requires a lot of knowledge and skill. This will do for the time being.

I am using the tilt angle of the robot as the input and the speed of the motors as the output.

The code is quite simple. The three variables are all floating point numbers.

double Setpoint, Input, Output;

The PID object is instantiated with the three PID values I calculated from my Ziegler–Nichols tuning method

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

The desired set point is 0 degrees, i.e. the robot is perfectly upright

Setpoint = 0;

Then in my main loop I get the angle of the robot, run the PID compute function, and using the output set the motor speeds.

void loop() {
  Input = ypr[IMU_ROLL];
  md.setSpeeds(Output, Output);

That’s the theory anyway … the results were mixed. It took a number of attempts to find the oscillation gain and period (Ku & Pu) accurately. And with those values, the PID controller wouldn’t balance the robot. It took lots of manually trial and error of various values to find something that was stable. PID tuning is very much a black art.

I did get it to balance though and it was more stable that the equivalent “cubed” algorithm I was using. However with the IMU code and the PID code on the Arduino, I started getting problems with the Arduino crashing. I can only assume it was running out of RAM.

I switched to an Arduino Mega and everything worked fine. So I’ve stuck with that for the time being. The Mega is much more stable, however the custom PCBs I made don’t fit the Mega, so the wiring is messy, plus the Mega sticks out of the frame. Next job is to make the PCB as a Mega Shield and turn the board round long ways so it fits better.

The PID controller is still not perfect, but it’s relatively stable. You can see it recovers from oscillation, which the basic controller didn’t. But It still drifts badly, and still falls over eventually. But progress none the less.I need to get the Quadrature decoders working so I can stop the drifting.

Here’s a short video of the PID Controlled BalanaceBot in action.

BalanceBot – Second Steps

The second test of my BalanceBot. This time on the carpet. It drifts a lot less and is quite stable. It just has a simple “cubed” function for control. Speed = Angle ^ 3. No PID yet.

BalanceBot – First steps video

The very first time my BalanceBot balanced. It drifts quite a lot, but not bad for a first attempt. It just has a simple “squared” function for control. Speed = Angle ^ 2. No PID yet.