Tag Archives: PID

AVC – Competition Day

The morning of the competition arrived. After getting up early so I could be at the course as it opened to spend the final hour tuning my way points, I released when I arrived at the venue, I’d left my wallet in the hotel, and had to drive back to the hotel and get it, taking all the time I would have had to practice, driving up and down the motorway in Boulder. 😦

AVC 2013 Ground Course ((c) Sparkfun)

AVC 2013 Ground Course ((c) Sparkfun)

AVC 2013 Waypoints v1

AVC 2013 Waypoints v1

The competition began. I was in heat 8, so I had a good 45 minutes to tinker before my first run. I mounted a GoPro and double checked my way pints on Google Maps. I was kind of pleased to see that almost no one in the first few heats made it past the first corner. In fact by heat 8, I think only 2 robots had made it round the course, out of about 30.

It was time for my first run. I was actually nervous !? Even thought I knew I wouldn’t win, I wanted to make it round the course, and not look like an idiot. Also I was the only Brit in a sea of Yanks … I need to represent my country ! 🙂

The countdown began, then Go! I pressed the big red button and my bot shot off ! To my surprise and great relief in the right direction !!! and to my amazement, it made the first corner and turned 90 degrees at exactly the right waypoint, on to the second straight. My heart was pounding as my bot swerved to the 2nd way point. I had 3 waypoints on the second straight to avoid the barrels. It swerved and missed the first barrel, I was actually amazed it was working !

I turned again and aimed to miss the second barrel … but then it didn’t straighten up and started to veer in to the centre of the course … within a few seconds it had hit the boundary fencing, and toppled over. Disaster! I was out in the first round mid-way up the second straight.

Not bad all things considered. Seeing as it was the first time all that code had run at the same time, completely autonomously. Although I was disappointed, I was actually quite chuffed it had made the first corner and hit the first three waypoints. My code was working !
During the beak between rounds I debugged the code and analysed the log from the first run. The bot was working, but was veering to the left, so I adjusted the 3rd waypoint to pull the bot further in to the centre of the course.

The second round. Go! I hit the button and my bot shoots off in the right direction again, but where it had neatly turned 90 degree at the first way point on the first round, this time, it span in circles at the first way point, driving in endless circles until it hit a hay bail.

In theory that shouldn’t be possible. The code should have dropped out of the loop when the waypoint was close, plus the circle was also tight, i.e. full lock. There must be something else wrong.

I spent lunch time debugging the logs and analysing the data. To my horror, the magnetometer was completely out of whack. It would reading north to be 90 degree from where it should be. And in various walking tests didn’t even change readings as I rotated the bot. My magnetometer reading were the plague of this project.

I decided to run another calibration over lunch, it takes a while, as I have to change some connections as I can’t do it reliably over xBee, so I run the calibration over a direct serial connection.

It took about 30 mins to complete the calibration and the bot was reading north again. I had about 20 minutes left of lunch to test on the course before the final heat. I pressed the button for a final practice and the bot shot off in the right direction, first waypoint hit, second way point hit, barrel avoided. Third way point hit, this was the furthest I’d ever got on the course. The back straight was perfect. I purposely avoided the hoop, I wanted to get round, and wasn’t so bothered about points. On the third straight, it actually hit the ramp and completed a jump, without toppling. It hit the final corner turned perfectly and flew over the finish line ! I had completed the course perfectly ! Shame it was over lunch in a practice session and no one saw ! 😦 I do have some video from the GoPro to prove it through !
I decide not to touch ANYTHING and wait for the final round. More and more bots were completing the course. Each run they were getting better. It was now or never.

Finally the third round was here. Nate (The Owner of SparkFun!) was my referee for the final round. No pressure then !?

The referee counted down 5,4,3,2,1 – Go ! I paused slightly to let the faster bots shoot off before me, to try and minimise any potential collisions. Previous rounds had a number of collisions right off the start line, where out of control bots took smaller bots out completely. I saw one completely smashed to pieces. My bot shot off in the right direction … turned perfectly on the first courser … weaved perfectly in through the barrels, detoured slightly wide at the second corner, my heart pounded as it headed for the fence, but with a meter to go, it turned sharply and headed off past the hoop. It started to oscillate on the back straight – shame I never got the PID controller tuned I thought – the bot headed straight for the cameraman on the 3rd corner, again with a meter to spare turned sharply 90 degrees and headed down the final straight … missing the jump (shame. It made it perfectly in the practice, and I could have got some bonus points). The curb of the final corner loomed, the bot shimmied perfectly round it, turn sharply on the final straight and headed for the finish line. I held my breath as the bot bounced over the finish line perfectly, running over the SparkFun GoPro that was balanced on the start/finish line capturing the action … 2 or 3 meters after the finish line it ground to a halt. I was so happy ! I think I actually kissed it (which someone caught on camera) 3 months of late nights and constant problems culminated on a perfect run on the final round. I was elated. I’ll post a video …

What’s next ?

I want to finish the bot off, even though the competition is over. Not only to have something ready if I manage to find a way to enter again next year, but also to understand what’s wrong. I still don’t fully understand why the magnetometer is so shonky. It’s not the device, I’ve tried 3 different devices, and they all do the same strange things.

At the competition, nearly all the other bots has their magnetometer in the car, on the main PCB, not floating on a broom handle above it like mine. I need to understand what my issues are.

I want to finish the PID tuning and increase the speed. The RC chassis is very fast, and I’m running at about 1/10th speed. If I can remove the broom handle, the bot won’t topple over so easily and can turn faster. I also want to include a proportional speed control so the bot accelerates fast if it’s far away and on a straight heading, then brakes to make the turn as it gets close to the waypoint.

I also want to finish the obstacle avoidance, so if I do head for a barrel, I can veer away from it.

All in all and very pleasant way to spend a weekend. I got to talk to lot of interesting people with a similar passion, I got to spend the weekend in 90 degree sunshine and I got to indulge my passion for robotics and spend the weekend geeking out … 🙂

Thanks to everyone at SparkFun for running the event and organising it so well.

Roll on AVC 2014 … 🙂

Other posts in this series :

AVC – First test run

The mBed versions of the AVC PCBs arrived. I built the first version, with surface mount components in my oven at home. It works first time. It turns out I made one mistake on the PCB, using the wrong ratio SMD resistors with the battery divider in to the mBed, so I had to take them off and modify it with standard through hole resistors, as I couldn’t hand solder the 0602 surface mount resistors. I made the modification for the SPI connection for the IMU, and swapped a couple of sensor to different pins to give me SPI and Serial in the right places.

AVC 2013 mBed Custom PCB

AVC 2013 mBed Custom PCB

AVC 2013 mBed Custom PCB

AVC 2013 mBed Custom PCB

AVC 2013 mBed Custom PCB

AVC 2013 mBed Custom PCB

I mounted the circuit in a Tupperware box on the car to keep it water tight.

My first outside test of the complete system was very exciting, but a little disappointing. Not only did I manage to run my wife over with the car, giving her a nasty bruise, but also the car seemed to like to take the scenic route to the way point, rather than a more direct route. All down to magnetometer issues again. I was still getting erroneous and unstable readings. In fact this plagued me from start to finish on this project, and still hasn’t gone away.

The only solution I have found to improving the magnetometer readings is to re-run the magnetometer re-calibration process regularly (before each run). The numbers always come out slightly different?

The first end to end test of the system in my garden was one week before the competition. I still hadn’t written a lot of the other functions in the code. Sections of the code like; how to tell if I had reached a waypoint, loading way points from SD, obstacle avoidance using the sonar, and tuning the PID parameters. I also hadn’t actually mapped my waypoints for the route I was going to take through the course.

The PID tuning would have to wait. I was getting pretty good steering results just with the “P” terms. This would mean I was in danger of oscillations, but that was the least of my worries. I could hardly drive in a straight line at the moment …

I didn’t have time to write any sonar obstacle avoidance code. I had a plan to read the distance data from the sonar and if an object got within 2 metres, add an offset in to the steering calculation to veer slightly left or right to miss the object. However, I hadn’t quite worked out how to tell if I had “passed” it to remove the offset, as I only had a forward facing sonar. The other option was to just stop the bot if an object appeared less than 20cm in front of the bot, stop, turn a few degrees, go forwards, then continue under GPS control… I’ll get round to adding this later …

I arrived in the US for a conference about a week before the competition and spent the evenings in the hotel writing the missing code. I wrote a basic file reader to load way points and basic config data from file, plus as my xBee worked for about 2m at best, I wrote a simple logging function, which meant I had some debug data after each test run to investigate. My waypoint algorithm was relatively simple;

  1. Calculate the heading and distance to the next way point
  2. Use the PID control to steer on that heading.
  3. If the distance to the way point was less than 2m meters, we’re very close. At this point these it a danger the bot will drive in circles, as the turning radius is greater that the heading it needs to hit the way point exactly. So, once we get close, I no change the steering to steer to the waypoint, instead, continue on the heading I was on just before we got close. You can assume the heading I was on would hit the waypoint pretty close. So, if I stay on that heading, and monitor the GPS vector to the waypoint, as I pass the way point I should see the GPS vector increase to 90 degrees as I pass by the waypoint. i.e. the vector will go from straight ahead, to 10, 20, 30, 45, eventually 90 degrees. At this point I’m within 2m meters and its 45 degrees or 90 degrees to my left or right, so, that’s a pretty good assumption of hitting the way point, so I assume I’ve passed it, and move on to the next way point.
  4. Once the bot has visited all the way point it has loaded, it stops. Course complete.
Way Point Diagram

Way Point Diagram

I didn’t have time to write the obstacle avoidance code or tune the PID in time for the competition.

The day before the competition I spent the afternoon at the location planning the course and testing the code. I rewrote my magnetometer code again, this time using my own simple trigonometry rather than the library function. Plus I debugged the “hitting the way point” logic. By sunset, I’d navigated to a series of way points for the first time, but still careering off course occasionally. In addition I suddenly remembered that I hadn’t compensated for magnetic vs. true north, so added some Magnetic deviation code. This link has a great calculator for magnetic variance. It shows that Colorado has a deviation of about 8 degree, which would have been a big issue if I hadn’t remembered to compensate for it.

Other posts in this series :

AVC – Switching from Arduino to mBed

Once the initial Arduino prototype was built I started having problems with the board. I rigged it on the bench with two servos attached directly to the speed and steering outputs. The code kept crashing and the board would hang. Normally in strange ways. I would comment out lines of code, and it would be more stable, then I’d put different code in and it would crash again. It seemed to have little to do with the content of the code, and more to do with the *amount* of code…

I had this problem before with my balance bot. The code randomly crashed there too and if you run some basic compiler tools on the code, the code is very close to the RAM limit of the microcontroller.

I had been looking at mBed for a few months and had bought a board to play with and this seemed like a perfect project to learn mBed on. I could have switched to an Arduino Mega, which is what I did on my Balance Bot, but the Mega is physically so much larger, and I wanted to fit the resulting controller inside the RC car, so I bit the bullet and scrapped everything I’d done to date, and started again with mBed. There’s nothing better than a looming deadline and nothing currently working to focus the mind on learning a new platform. 🙂

I had an LPC1768 mBed which had much better specs. I would classify its advantages over Arduino as :

  • Faster – 100 MHz vs. 16 MHz. Six times more instructions per second means more power. I could read the GPS and magnetometer at a much faster frequency and baud giving me better accuracy and control.
  • More hardware serial ports – 1 UART on the Arduino vs. 3 UARTs on the mBed – I needed a fast serial port for the GPS and one for debugging via xBee.
  • Hardware I2C for the magnetometer.
  • More RAM (32 KB vs. 2KB on the Arduino) more than enough to run all the various libraries and calculations without crashing.
  • Hardware floating point – This is important for GPS as the LAT/LON coordinates are all floating point numbers. There would be lots of complex trigonometry with floating point numbers happening every second. Hardware floating point would be a huge improvement in performance.
  • The last benefit was unexpected. The mBed has an RTOS library that allows you to run multiple independent threads. This made the code design much simpler. Rather than having one big main loop, that has to do everything, I could split up the tasks in the threads. E.g. one just ingesting and parsing GPS NEMA data, one outputting debug information, one running the PID control loop, etc.

The design changed from Arduino to mBed over night and I planned a new board. Although this has only taken a couple of blog pages to explain, it took about 6 weeks in elapsed time. Lots of late night debugging, scratching my head, reading on the interweb and chatting to people on forums.

I designed another PCB for the mBed solution and using the cheap PCB fabs I calculated it would arrive a couple of weeks before the competition. I had no choice but to commit to this design. There would be no time to reinvent the solution a third time. In fact if it didn’t go well, I’d be soldering it on the plane, if I wasn’t lucky … 🙂

AVC 2013 mBed Custom PCB v2.1 layout

AVC 2013 mBed Custom PCB v2.1 layout

I added a battery meter to the final PCB design as a last minute addition. I had seen this video on the EEVblog describing the LM3914 LED driver. The worked example on the video was exactly what I needed to monitor the charge of my LiPo’s, so I added the circuit and LEDs to the final PCB. This would give me an easy visual reading of the battery condition before each run, reducing the risk of the run failing due to a low battery. In retrospect, I perhaps should have done this a different way. The LED driver was actually more complex and more expensive that driving the LEDs from the mBed directly. I have enough IO pins and I’m already taking an ADC reading of the battery voltage, but it seemed like a good idea at the time.

I sent the PCB design off which would give me a few weeks to convert the code.

I should point out here why I like having custom PCBs, instead of wire wrap or breadboards. I do jump to custom PCB very quickly in my prototyping phase. The simple answer is reliability. In the past I have run projects from a breadboard. But as always happens you drop the project, or it has to travel somewhere in a box, and suddenly two of the wires have popped out of the bread board. Typically you now waste hours tracing every connection to work out where they went and which ones have popped out. Plus with moving robots, the momentum of the bot tends to throw the boards around unless they are stuck down well, giving the same problem with wires coming lose. I like having a custom PCB. They are very cheap and easy to make (once you’ve used Eagle a few times) and they make the circuit an order of magnitude more reliable, much smaller and less messy. In addition they are just as easy to change if something is wrong, you can cut a trace and solder in a jumper wire in. Plus things like xBee, which has 2mm pitch legs, aren’t breadboard friendly, so you end up with breakout boards, again that come lose. Custom PCBs are just easy, cheap, and more reliable.

I started by porting the existing Arduino GPS and magnetometer libraries to mBed, which meant the bulk of my code would just migrate to the new platform. The mBed platform is fantastic. Its “proper” C++, not the cut down libraries of Arduino. I got things like printf back 🙂 which are huge resource hogs on Arduino.

There were other benefits too. There is a simple SD Card library, so I could store my waypoints on an SD card, which would mean making small edits during the competition wouldn’t mean I needed to recompile the code and upload it to the mBed, I could just edit the text file in an editor and re-read them.

I structured my code slightly differently to make use of the multithreading benefits of mBed too. I had three threads;

  • The first was dedicated to just ingesting the GPS NEMA strings and converting them in to numbers that were stored in a global variable structure. I ran the GPS at 4 Hz to give me good accuracy.
  • The second thread was dedicated to providing debug data. It ran every second, (or every 2 seconds) and dumped all the current readings of all the sensors and internal variables over the xBee. These would then be read by the desktop app and displayed in a user friendly way.
  • The final thread was the main loop. This ran the PID control and used the data from the GPS structure and magnetometer to feed the PID, and then set the steering servo with the output of the PID.

I got a prototype working on an mBed development board while I waited for the custom PCB to arrive. I used wire wrap as usual, with some jumper leads to plug in to the mBed. The prototype worked OK and I started to get a basic system working.

AVC 2013 mBed wire wrap prototype

AVC 2013 mBed wire wrap prototype


AVC 2013 mBed wire wrap prototype

AVC 2013 mBed wire wrap prototype

Here’s the test video the full system working on the bench for the first time. You can see the third channel switch the control between RC and mBed. During mBed control, the compass on the mBed controls the steering servo, and the wheels track the position of the compass. During RC control, the transmitter has control and the mBed does nothing.

Other posts in this series :

AVC – How difficult can it be ?

Ever since the SparkFun AVC started a few years ago, I’ve wanted to enter. But living in the UK made it difficult to justify the expense of flying to Colorado for a robot competition. This year, purely coincidentally I was in the US for a conference the week before the competition. The opportunity was there to enter so I decided to build a robot and enter the SparkFun AVC 2013.

The AVC on paper sounds simple 🙂 … Build an autonomous vehicle that can navigate an obstacle course without human intervention. The course is timed and you get bonus points for achieving certain tasks (going under a hoop, going over a ramp, etc.).

I decided to enter the ground competition, as the aerial challenge sounded way more complex. I’ve never built an aerial robot, so I thought I stick to something I had experience in. I decided to base my robot on a radio controller car chassis. I had an HPi Racing Savage Flux XS, which was perfect. It’s a 4×4 mini monster truck, that’s fast and simple. Just two controls, steering and speed. In addition, the radio it comes with has three channels, so I could use the 3rd channel as a control to switch between radio control and autonomous control, and quickly switch back to radio control during testing in the event it went berserk and headed for a tree.

My bot has a GPS receiver to provide its current position, and a magnetometer to provide its current heading. A microcontroller controls the speed and steering servos and a Radio Control switch can switch between autonomous control and manual control using the third channel. In addition the course has obstacles, so I need some basic obstacle avoidance. I used a small sonar to provide the distance to obstacles directly in front.

I needed a way to monitor and debug the car while it was hurtling round my garden during testing. I decide to add an xBee so I could get basic debug information from the car in real time, and (as I did with the balance bot) write a simple PC desktop app to display them in a user friendly way.

AVC 2013 High level system diagram

AVC 2013 High level system diagram

SparkFun provide the course details with all the GPS coordinates. The course is roughly rectangular with four sides, a start finish straight, a side with 4 barrels as obstacles, a side with a hoop to go under for bonus points and a side with a small ramp to jump, again for bonus points. The course is about 200 feet square, with the course being about 20 feet wide. With basic GPS accuracy of 2-4m this made the challenge perfectly achievable … easy huh !? If only …

AVC 2013 Ground Course ((c) Sparkfun)

AVC 2013 Ground Course ((c) Sparkfun)

Initial components

I started with components I already had. An EM-406A GPS module and a Pololu 3D magnetometer breakout board. I bought a MaxSonar sonar from CoolComponents and an RC switch from Pololu to give me a failsafe control.

The initial plan was to use an Arduino, as I have lots of them and lots of experience programming them. As mentioned before, I like old school wire wrap for prototyping, so I prototyped a basic Arduino shield to tie the 3 sensors together.

The initial testing went well. I used a third party GPS library and magnetometer library to ingest the data and I could get a basic heading to a GPS waypoint, and get a bearing from the magnetometer on the bench. The PWM output worked controlling two test servos, so confidently I sketched a custom PCB to shrink the design and make it more feasible to fit in the RC car, so I could start field testing in the garden.

AVC 2013 Arduino Custom PCB v1.0 board

AVC 2013 Arduino Custom PCB v1.0 board

AVC 2013 Arduino Custom PCB with components

AVC 2013 Arduino Custom PCB with components

Once the PCB arrived I had the basic software running. The premise of the control system is to navigate to waypoints, so the GPS coordinates as used to calculate a bearing from the current position to the next way point. This is the heading the car needs to steer. However, the car is on rough terrain and will never steer in a straight line. The course the car is taking is provided by the magnetometer, which gives the bearing the car is pointing. To reach the waypoint the difference between the car’s heading and the GPS bearing need to be kept as close to zero. One of the best ways of doing this is to use a PID controller.

I had a basic control loop written, for the Arduino, which looks something like this pseudo code:

While(CurrentWaypoint < TotalWaypoints) {
  MagneticHeading = GetMagnetometerHeading();
  GPSLocation = GetGPSLocation();
  GPSHeading = GetGPSHeadingToWayPoint(GPSLocation,WayPoint);
  PID_input = MagneticHeading – GPSHeading;
  If (DistanceToWaypoint < 2m) then CurrentWaypoint++;

This should steer the car in a straight line from wherever it is, to a defined GPS waypoint. Once it gets within 2m of the way point, it moves on to the next waypoint. Once all the waypoints have been reached, it ends.

This is when the problems began…

Other posts in this series :

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 – 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 :


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…