Category Archives: BalanceBot

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.

e.g.

#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()
{
my3IMU.getYawPitchRoll(ypr);
Input = ypr[IMU_ROLL];
myPID.Compute();
md.setSpeeds(Output,Output);
}

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

//MotorDriver
DualVNH5019MotorShield md;

void setup()
 {
  Serial.begin(115200);

  // Initialise Motor Driver:
  Serial.println("#Init Motor Driver");
  md.init();
  delay(50);

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

  // Initialise SPI:
  Serial.println("#Init SPI library");
  SPI.begin();
  SPI.setDataMode(SPI_MODE0);
  SPI.setBitOrder(MSBFIRST);
  SPI.setClockDivider(SPI_CLOCK_DIV8);
  delay(50);

  // Initialise IMU:
  Serial.println("#Init IMU");
  my3IMU.init();
  delay(50);

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

  // Initialise PID:
  Serial.println("#Set initial Input variable");
  Input = ypr[IMU_ROLL];
  Serial.println("#Init IMU Setpoint");
  Setpoint = 0;
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-400,400);

  Serial.println("#Exit Setup");
}

void loop()
{
  my3IMU.getYawPitchRoll(ypr);
  Input = ypr[IMU_ROLL];
  myPID.Compute();
  md.setSpeeds(Output,Output);
  stopIfFault();
}

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

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() {
  my3IMU.getYawPitchRoll(ypr);
  Input = ypr[IMU_ROLL];
  myPID.Compute();
  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.

BalanceBot – Sensors and IMU Library

The BalanceBot needs to know if it’s upright to balance. If it starts tipping over in one direction the robot needs to sense how far it has tipped over and speed up the motors in that direction to counter the tipping motion. This will compensate for the tipping motion by driving the bottom of the robot under its centre of gravity and stand it back upright again.

Sensing the angle of the robot on a simple level could be achieved with a single g-force sensor or accelerometer. The accelerometer measures acceleration. Our planets gravity, acting on an object, is acceleration, so the accelerometer can be used to measure gravity. Gravity always pulls down towards ground at a constant value. By knowing the value you can calculate the angle of the sensor. If the sensor reads 1g, the sensor is “vertical” with the ground. If the accelerometer reads 0g the sensor is “horizontal” with the ground. By using trigonometry, the value of g can be used to calculate any angle of the sensor.

However this is only true if the sensor is perfectly still, and the only acceleration acting on it is gravity. As we are going to mount this in a moving robot, the acceleration of the robot will cause the accelerometer to read not just the gravitational force, but also any acceleration of the robot, which will render the reading inaccurate when calculating the angle of the robot.

Another method of measuring the angle of the robot is with a gyroscope. A gyroscope measures rotation velocity around an axis. If you know the angle of the sensor to start with, then continuously measure the rate at which it rotates, you can calculate the current angle. This has two issues; firstly you need to know your angle to start with and secondly any error in the calculation is compounded with time, so the calculated value “drifts” over time and become less accurate. However a gyroscope is immune to interface from acceleration.

So, if you can combine the accelerometer data and gyroscope data together you can use both readings to compensate for the inaccuracies of the other. The accelerometers can compensate for the gyroscope drift as they give exact readings at a known time. The gyroscopes compensate for the non-gravitational acceleration as they only measure rotation velocity.

This process is called “sensor fusion”. There are lots of method of processing the accelerometer and gyroscope data to give a single accurate angular reading. These include Kalman Filters and Complimentary Filters. The maths can get a bit complex but it’s all based on trigonometry. You could write your own code, but there are lots of existing, tested, libraries that do this for you.

Using such a library and the appropriate sensor can give your orientation in 3D space, your Roll, Pitch and Yaw angles, or Euler Angles. This traditionally uses an additional sensor to measure yaw, as yaw cannot be measured with a g sensor, as yaw rotates around the vertical axis, and so the force of gravity doesn’t change. The easiest way to measure yaw is with a magnetometer, this measures the earth magnetic field and can calculate your orientation to “North” (magnetic north).

BalanceBot IMU - Sparkfun 9 DOF sensor stick.

BalanceBot IMU – Sparkfun 9 DOF sensor stick.

The individual sensors all work in a single plane, so a single gyroscope only measures rotational velocity in one direction. An accelerometer only measures acceleration in one direction. Depending on how much your sensor, or robot, moves, you may need three of each sensor in each orientation (x, y & z) to calculate your orientation in 3D space. For example, as the “X” axis accelerometer rotates it will no longer be vertical and will become horizontal at which point, depending on the orientation, of the sensor either the “Y” or “Z” accelerometer begin to point down. Using 3D trigonometry of the X, Y & Z accelerometer reading, you can begin to calculate the orientation in 360 degrees. This is called “three degrees of freedom” you are measuring 3 independent parameters that define its orientation. If you add three gyroscopes, and three magnetometers, you end up with 9 Degrees of Freedom, or 9DOF.

Using a 9DOF sensor and a maths library, you can calculate (almost) any orientation accurately. This combination of sensors and maths is called an Inertial Measurement Unit or IMU. There are lots of these, ranging in accuracy and cost. They can cost thousands of pounds. However there are lots of hobby level devices too for less than £100.

You have the choice of separate sensors and use your own CPU for the maths, or use an all in one device that includes its own CPU. I have an UM6-LT Orientation Sensor from CH Robotics for a different project. They are easy to interface to and do all the Kalman filtering for you on board and provide the filtered, stable Euler Angles via a serial interface. However I was using this device in a different project.

There are many 9 DoF boards containing just the sensors. FreeIMU is a popular board, cheap and compact. SparkFun produce a 9DoF Sensor Stick which I decided on, as its cheap, small and easy to interface to.

The FreeIMU site provides a GNU GPL license library for Arduino which supports not only the FreeIMU boards but also the SparkFun boards and the DIYDrones ArduIMU. This provided the sensor fusion calculations to provide a stable combined output from the raw sensors.

The SparkFun board uses an I2C interface to communicate with the Arduino. This is a bidirectional serial interface, using just two data wires, and power.

To be clear I didn’t need a 9DOF board in this simple robot, as I only needed to calculate the robots orientation in one axis (the axis that rotates around the wheels) however, by using a 9DOF board I get full 3D orientation which means it doesn’t matter which direction I mount the sensor in, plus I intend to use the magnetometer reading to capture the “heading” the robot is traveling on so I can make it travel in a straight line. It was also fun to learn.

There are two common ways to represent a 3D orientation; Euler Angles and Quaternions.

Euler Angles can be understood easily if you imagine an old fashioned gyroscope in an aeroplane. A spinning mass is suspended in space on 3 gimbal bearings, that each rotate in a perpendicular axis. As the aeroplane tilts left and right (rolls) the gimbals rotate to keep the mass level. This angle of roll is the Roll Euler Angle. Same for Pitch, the movement of the nose of the plane up and down, and Yaw the rotation of the plane caused by the movement of the rudder. These three Euler angles define the Roll, Pitch and Yaw of the object. However Euler Angles have a flaw, as one of the axis rotates 90 degrees, it aligns with one of the other axis. In fact all three axis can align. This is called Gimbal Lock.

Gimbal lock is the loss of one degree of freedom in a three-dimensional space that occurs when the axes of two of the three gimbals are driven into a parallel configuration, “locking” the system into rotation in a degenerate two-dimensional space.

This means in some situations Euler Angles cannot accurately define our orientation. However they work fine for movements that do not exceed 90 degree rotations, which is our case if fine, as we will only be “wobbling” a few degrees from vertical. (I hope)

An alternative system is called Quaternions which does not suffer from Gimbal Lock. This uses imaginary numbers to define our three dimensional orientation, and is way too complex to try and explain here. Plus I don’t really understand it enough 🙂

So, I stuck with Euler Angles.

The FreeIMU library provides a simple function to calculate the Euler Angles :

my3IMU.getYawPitchRoll(ypr);

This fills a 3 dimensional variable with three floating point numbers, which contains the Pitch, Roll and Yaw angles.

With my sensor mounted as it is, the “Roll” angle is the rotation around the wheels, this is the only value I use:

angle = ypr[IMU_ROLL];

With just this value I can balance the robot. If the angle is positive, I drive the wheels in one directions and it the angle is negative I drive the wheels in the other direction. By proportionally driving the wheel speed, relative to the angle, the robot balances. I tested it using a cube function. i.e. Speed = Angle ^ 3

If the angle is 1 degree, the speed is 1 (slow), if the angle is 2 degrees the speed is 8 (faster), if the angle is 3 degrees the speed is 27 (quite quick), etc. The more the robot falls over the faster the wheels rotate to stand it back up. When the robot is upright the speed is very slow so the robot doesn’t overshoot and start to oscillate.

Of course it does oscillate with this very simplistic control algorithm and falls over. We need a more complicated control algorithm to stop the oscillation.

A PID Controller works well. More on that next…

BalanceBot – Quadrature Decoders

I’m old school when it comes to prototyping. I tend to test bits of the initial circuit on a breadboard, but quickly move to a prototype board using wire wrap. I’m a big fan of wire wrap prototyping over breadboard. It has lots of benefits;

The wires in Breadboards have a tendency to fall out if moved or quickly become a birds nest if the circuit is complicated.

Breadboards don’t stack neatly like shields, so they are difficult to mount in the chassis.

I knew my Robot was going to fall over a lot initially, so I wanted something sturdy that could take some knocks, without falling part, but could be changed easily if I got something wrong (you always get something wrong)

I found some great prototyping boards from FreeTronics.com (ProtoShield Pro for Arduino). These have a reset button, a general purpose button and general purpose LED mounted around the edge of a grid of holes. This is great for prototyping. The boards are well designed with easy access to power rails and they’re cheap.

I buy long pin through hole sockets for wire wrap and solder just two legs of the socket (at diagonal corners) on to the prototype board, just to hold the socket in place. You then use special thin wire wrap wire and a tool that twists the wire on to the pin, to wire your circuit. There’s no soldering to wire the circuit (just a dab to hold the socket on to the board) the friction of the wire wrapped around the pins holds it tight. You can cut the wire to roughly the right length for each connection, so the board stays tidy and does not resemble a birds nest.

Quadrature Counter Shield

Quadrature Counter Shield

If you make a mistake, you can use the tool to unwrap the wire and you just replace it with the correct connection. Once the circuit is complete, the board plugs in as a normal shield, and it’s robust enough not to fall apart in use.

The LS7366 Quadrature Decoders are chips made by LSI. There is a decoder IC for the quadrature encoder on each motor. The ICs have a 32 bit counter, so they can count about 4,294 million pulses before rolling over. They have an SPI interface so it’s just 5 wires to connect them both to the Arduino (three common SPI data lines and two slave select lines). The SPI interface uses common data and clock lines from every device (slave) to communicate to the Arduino (master). The master controls who can talk on the data lines, through a unique Slave Select (SS) line connected to each device. When the slave select line is held low that device talks to the master. When the slave select line is high, the slave ignores the data lines and puts the pins in a “high Z” state, i.e. they are isolated from other signals or devices connected to the common data lines.

  • MISO – Master In Slave Out (data from the chip to the Arduino)
  • MOSI – Master Out Slave In (data from the Arduino to the chip)
  • SCK – Clock

These three lines are all connected in parallel.

  • SS – Slave Select is unique to each slave and is connected back to a separate pin on the Arduino.

In this case, I used Arduino pins 3 & 5 for the two quadrature decoder slave select lines (one for each motor). The SPI data and clock pins are pins 11, 12 & 13 on the Arduino.

LS7366 Quadrature decoder shield schematic

LS7366 Quadrature decoder shield schematic

I had a few issues with the wire wrap prototype, misreading my own initial circuit diagram, but I soon got it working.

The code is simple, but understanding the data sheet took some time. To setup SPI on the Arduino, you have to set three parameters, the Mode, the Bit Order and the Clock Frequency. They are all defined in the data sheet for your device.

The mode sets the phase of the signals for valid data, i.e. does the device response on rising or trailing edges of the clock. Wikipedia has a very useful entry on SPI : http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus

The Bit Order defines whether binary numbers are sent down the serial interface most significant bit first, or least significant bit first.

The Clock Frequency in the Arduino library is a multiple of the system clock. The maximum clock rate your device can handle will be in the data sheet. Then it’s just some simple maths to work out what multiple of the Arduino 16MHz clock you need to not exceed the maximum clock rate for your device. In this case I’m using “divide by 8” i.e. 2 MHz

There is an SPI library in the Arduino, which makes all this easy.

SPI.begin();
SPI.setDataMode(SPI_MODE0);
SPI.setBitOrder(MSBFIRST);
SPI.setClockDivider(SPI_CLOCK_DIV8);

To send data to and from the device you call the “Transfer” function while holding the correct Slave Select line low :

SPI.transfer(byte val);

This sends a byte (8 bits) out of the SPI bus. You’ll notice all SPI communications are 8 bits. Our counter is 32 bits, so I wrote some helper functions to read the 4 byte counter values. You just need to read the multiple bytes sequentially.

Here’s the helper function that reads the 32 bit counter values, which takes the slave select line as a parameter, so I can use it for either quadrature counter. It sends the read command and then reads 4 bytes in sequence, building them in to a 32 bit unsigned long.

#define  RD_CNTR B01100000
unsigned long ReadCounter(byte CSx)
{
  unsigned long result;
  byte b;
  digitalWrite(CSx,LOW);
  SPI.transfer(RD_CNTR);        // Read CNTR
  result = 0;
  b = SPI.transfer(0x00);
  result = result | b;
  result = result << 8;
  b = SPI.transfer(0x00);
  result = result | b;
  result = result << 8;
  b = SPI.transfer(0x00);
  result = result | b;
  result = result << 8;
  b = SPI.transfer(0x00);
  result = result | b;
  digitalWrite(CSx,HIGH);
  return result;
}

I don’t actually need the quadrature counters in phase one of the project to get the BalanceBot balancing. It will balance without them. However, it will probably drift from its original position as its balancing. The plan is to count the quadrature pulses as it drifts and then bring it back to its original position once it’s stable. This should mean if the robot gets knocked, it will move back to rebalance, and then slowly come back to where it was, maintaining its position.

Quadrature Counter Shield

Quadrature Counter Shield

Once I got the wire wrap version working, I converted the board to a custom PCB using Eagle PCB software and BatchPCB. More on that later…

BalanceBot – Motor Controller

I used a Pololu Dual VNH5019 Motor Driver Shield for Arduino as the motor controller. It has two VNH5019 MOSFET H-Bridge brushed DC motor controller IC’s mounted on an Arduino Shield. The motor controller can drive up to 12A continuous current per motor, 30A maximum. The motors are controlled via a PWM signal generated by the Arduino, and sent to the controller ICs, which in turn drive the MOFETS to control the speed and direction of the motors. The controllers can work up to 24v, but we are using 12v motors with a stall current of 5A, so the controller shouldn’t even break in to a sweat.

VNH5019 Shield

VNH5019 Shield

The shield comes with an Arduino library, making the programming simple. There is a function setSpeeds (int m1Speed, int m2Speed); which takes an integer speed for each motor, from +400 (full speed ahead) to -400 (full speed reverse), with 0 being stop.

There are quite a few other functions on the controller including braking and current monitoring, but I doubt we’ll use them in this project.

The only down side of using this particular shield is it takes lots of IO pins. There is a PWM signal for each motor, direction indicators, the current sensing output, enable lines and error signals. All in all the shield uses 10 IO pins of the 20 pins in total. This isn’t a show stopper, we still have enough left over (just) but there is one problem.

The shield uses pin 12 for the M2EN signal. We need pin 12 for SPI. The SPI signals are pins 11,12 & 13. There is the feature on the board to cut the track for control signals so you can move them. I had to cut the track and solder a thin wire from the jumper to the new pin. I picked pin A2 as we don’t need that and I updated the library with the new pin details.

VNH5019 Shield Schematic
VNH5019 Shield Schematic

In the library source code (arduino-1.0.x\libraries\DualVNH5019MotorShield\DualVNH5019MotorShield.cpp) just chnage the default constructor code to use the new pin :

DualVNH5019MotorShield::DualVNH5019MotorShield()
{
  //Pin map
  _INA1 = 2;
  _INB1 = 4;
  _EN1DIAG1 = 6;
  _CS1 = A0;
  _INA2 = 7;
  _INB2 = 8;
  _EN2DIAG2 = A2; // = 12;
  _CS2 = A1;
}

The controller worked fine first time. It’s powered directly from the battery. There is a jumper on the shield to power the Arduino from an on board regulator, so the motor battery powers the Arduino too. The library makes programming easy, and the motors are fast and responsive. The only thing you need to remember to do is wire one of the motors to the shield in revere. For the robot to travel forward with both motors at “+400”, one motor needs to be wired in reverse to the other as they are on either sides of the robot, clockwise is forwards for one motor, and anti-clockwise is forwards for the other. This doesn’t affect the motors at all, and it makes the code slightly easier to read.

BalanceBot – Components

The main components for BalanceBot are :

  • Two geared motors with quadrature encoders
  • A Motor controller
  • An Arduino Uno for a brain
  • A 9 DoF sensor stick to provide the IMU
  • A custom board to interface the quadrature encoder counters
  • A 12v rechargeable battery.

The motors are Pololu metal gear motors and Pololu have a nice motor controller shield that matches the requirements of the motors. The motor shield has a built in regulator to power the 5v Arduino from the motor power coming from the battery, so I could power everything from a single 12v rechargeable battery. A perfect fit. Pololu also sell mounting brackets for the motors and wheels. So that’s the mechanics covered in a single order.

The brain is an Arduino Duemilanove with an ATmega328p microcontroller but this is essentially the same as the newer Arduino Uno.

I read a lot about various IMUs and considered using an all-in-one unit with a built in CPU and code. I have an UM6-LT Orientation Sensor from CH Robotics for a different project and they are easy to interface to and do all the Kalman filtering for you on board, but I needed it for the other project, and they’re expensive, so I decided to use a raw 9 DoF sensor board and do the data fusion and filtering in code on the Arduino.

These 9 DoF boards are cool from FreeIMU but at the time I was researching this, there weren’t many online stores selling them, so I decided to use a SparkFun Sensor Stick. The FreeIMU site provides a GNU GPL license library for Arduino which supports not only the FreeIMU boards but also the SparkFun boards and the DIYDrones ArduIMU.

I was hoping to find an existing 3rd party Quadrature decoder Arduino shield, but I couldn’t one. I found some quadrature counter ICs LS7366 with an SPI interface. These count the quadrature pulses from the motors so the Arduino can read the count totals using SPI. I prototyped a shield for them using wire wrap and then manufactured a custom PCB using BatchPCB.com.

The frame is made from MicroRax which gave me the flexibility to build the frame to any dimensions to mount the motors and controller boards. Its strong and easy to construct and it also meant I could change my mind 🙂

The other components are from Maplins. A standard sealed 12v battery and some switches.

Finally, to make it look more professional I have some custom acrylic sheets laser cut for the base and top. These made mounting the PCBs and switches easier and made it look less Heath Robinson.

A sad note, Fabio Varesano (http://www.varesano.net/) the 28 year old student behind FreeIMU died suddenly over Christmas last year. Such a terrible shame for such a talented young man. He invested so much effort designing and developing FreeIMU and provided lots of support and encouragement to the robotics community. The information from his site was part of my research for this project and I’m sure he will be missed by the community. My heartfelt sympathy goes to his family and friends.