The Balancing Robot

Now avaliable as a kit
A balancing robot kit is now avaliable via Kickstarter: Check out the blog post as well:

Hello everybody
I have for a long time wanted to build a remote controllable balancing robot aka Segway – that’s was actually the main reason why I created the PS3 Bluetooth Library both for Arduino and the FEZ Devices. It has been a long time since the sneak peak and the performance has been improved a lot since then. The original one had a FEZ Rhino as the main processor, but I discovered that it was not fast enough to read the encoders, as it is not running embedded code. Also I was already using more than 10ms per loop, which I used as a fixed time loop, so I decided to step up a notch and go for a much more powerful device: the mbed microcontroller, which is an ARM Cortex-M3 running 96MHz.

It might have been possible with just a normal Arduino (NB: I have now ported the code to Arduino, see update for the code), but I didn’t want the speed of the processor to be an issue, so I decided to go for the mbed. The robot also features an Arduino Duemilanove with a USB Host Shield on top running a sketch based on my PS3 Bluetooth Library. The mbed board actually has USB Host functionality, but I decided not to port the PS3 Bluetooth Library as my original thought were to use an Arduino Due, but as you might know it hasn’t been released yet, despite the Arduino team announced, that it would be released by the end of 2011. But as soon as it is released I think I will port the code to it instead.

Video Demonstration
Here is a short video demonstration of the robot and me explaining some of the concepts of the design and how it works:

The Hardware
Here are some pictures of the robot:

Here is a list of all the hardware I used:

I also used:

The robot itself is made of three pieces of 215x75x7.5mm MDF wood and four threaded rods. The distance between the plates is 7cm at the bottom and 7.5 at the top. In total the robot is 27cm high including the battery.
See the 3D model for more information.

3D Model
I have created a 3D model in Autodesk Inventor with true dimensions, this will hopefully inspire other people for there robot design. All files can be found at github.
The 3D model can also be viewed at the following site:

Check out these rendered images of the robot:

The Code
All the code and 3D model can be found at our github. Here is a list of hyperlinks for all the repositories:

Also check out the wiki.

I have now ported the code to Arduino. The code can be found at github:

I have thought about how I could improve the performance of the robot. First of all I could try to use an accelerometer with a smaller resolution, as the one I got is a ±3g and ±1.5g would be sufficient for my needs. Also my gyro got a resolution of ±300 deg/s and I have seen people use gyro with a resolution as low as ±50 deg/s.

Another aspect would to use belts to minimize backlash, instead of connecting the wheels directly to the motors – a bit like this one.

Also I don’t compensate for the battery level in the code – so it behaves differently depending on the battery level.

Overall I am really happy about the end result – it balances pretty well and the remote control works perfect!

It has been a really good learning experience for me and a really fun project to do, but also very time-consuming – I have spend many nights tweaking the PID values and adjusting tiny bits of the code before I accomplished the end result.

The next step would to build a full size one, but I don’t know if I will do it the near future – but hopefully some day 🙂

That’s all for now. Hope you like my robot. Feel free to post a comment below and I will answer as quickly as possible.

  1. kas
    April 21st, 2012 at 10:04 | #1


    I am still struggling adapting your code to my specific IMU
    Please confirm what should angleY read when
    -- lying at one side
    -- vertical
    -- lying on the other side

    My current values are 117, 90, 65 (should be 180, 90, 0 ??)
    My acc (ADXL330) sensitivity is also 330mV/g

  2. Lauszus
    April 21st, 2012 at 11:12 | #2

    Yes it should read 180 or 0 when laying down, depending on what side is facing the ground and 90 degrees vertically.

    Is that the output from your accelerometer only or output from the Kalman filter?

  3. kas
    April 21st, 2012 at 11:23 | #3

    This is the return value of getAccY()

  4. Lauszus
    April 21st, 2012 at 11:29 | #4

    Hmm, it seems strange. I looks like your sensitivity is wrong, as it is 90+-25 degrees when laying on the two sides. Maybe your calibrating in wrong, since it should read either 180 or 0 degrees depending on what side is facing down at start up.
    You calibrate it when it’s laying down right?
    Can I see your code somewhere, so I can see it and check for errors?

  5. kas
    April 22nd, 2012 at 13:29 | #5


    It’s OK now
    My AccY (your AccX) was not (yet) connected
    double R = sqrt((accXval*accXval) + (accYval*accYval) + (accZval*accZval));
    double R = sqrt((accXval*accXval) + (accZval*accZval));
    did the trick
    Lauszus did you actually check that using all 3 Acc does improve Bot beheaviour
    Using only 2 Acc definitly reduce overhead
    By the way, congratulation for your direct port access expertise 🙂

  6. Lauszus
    April 23rd, 2012 at 19:10 | #6

    You can’t simply just remove the y-value, as you will not calculate the length of the force vector. Check out this page for more information:

    I’m not sure what you mean by, that it wasn’t connected?

    No I haven’t checked it, but it seemed to make sense, as by reading all three axis, you will compensate if it for instance accelerate in the x-axis. As the y-value and z-value will still be correct.
    But of course the Kalman filter could also take care of this for you 😉

    Your welcome. Have you tried the code on your robot? Or are you still developing the the hardware? 🙂

  7. kas
    April 23rd, 2012 at 21:02 | #7


    Please don’t forget that we have different IMU’s with different Acc axis orientation
    My Yaxis is your Xaxis direction (parallel to wheels axis)
    This axis is, for the moment, not connected to the Arduino I/O
    I tested your code (up to pitch calculation) and get accurate pitch readings with only 2 Acc
    I will make further investigations for the Yaxis (your Xaxis)

    I am redevelopping the hardware and just ordered those nice big green wheels

  8. Lauszus
    April 23rd, 2012 at 21:48 | #8

    I know, but you can’t just leave out one of the axis in the calculation. You have to use some other kind of calculation.
    Alternatively you could just use sinus, if you want to use one axis:

    accXangle = asin(accXval)*RAD_TO_DEG;

    Okay nice, looking forward to see your robot 🙂

  9. Lauszus
    April 23rd, 2012 at 22:25 | #9

    I just found a better method of calculating pitch and roll with full 360 resolution using three axis.
    I will post the details tomorrow.

  10. Lauszus
    April 24th, 2012 at 22:58 | #10

    Hi, take a look at my updated code:
    I have implemented the new method on how to get full 360 degrees resolution.

  11. kas
    April 25th, 2012 at 20:14 | #11

    >> double accZval = (double)((double)analogRead(accZ) -- zeroValues[3]) / 102.3;
    >> double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval); // Calculate the >> length of the force vector
    >> // Normalize vectors
    >> accYval = accYval/R;
    >> accZval = accZval/R;
    >> // Convert to 360 degrees resolution
    >> // atan2 outputs the value of -? to ? (radians) -- see
    >> // We are then convert it to 0 to 2? and then from radians to degrees
    >> return (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG;

    Too complicated 😉
    -- you do do not need to calculate the length of the force vector
    -- you do not need to acquire AccX

    The accelerometer values do not need to be scaled into actual units, but must be zeroed and have the same scale. The four lines of code above could be just omitted, try it 🙂

    You may refer to KasBot V2 :
    int getAccAngle() {
    return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256; // in Quid: 1024/(2*PI))
    With your specific IMU, just substitute AccX by AccY

  12. Lauszus
    April 25th, 2012 at 22:55 | #12

    Okay I will try it tomorrow and let you know how it went 🙂

  13. Lauszus
    April 26th, 2012 at 16:53 | #13
  14. kas
    April 26th, 2012 at 21:11 | #14

    For what is worth, I use my own arctan2 with int values for optimisation
    please see

    int arctan2(int y, int x) { // angle in Quids
    int coeff_1 = 128;
    int coeff_2 = 3*coeff_1;
    float abs_y = abs(y)+1e-10; // prevent 0/0 condition
    float r, angle;

    if (x >= 0) {
    r = (x -- abs_y) / (x + abs_y);
    angle = coeff_1 -- coeff_1 * r;
    } else {
    r = (x + abs_y) / (abs_y -- x);
    angle = coeff_2 -- coeff_1 * r;
    if (y < 0) return int(-angle); // negate if in quad III or IV
    else return int(angle);

    You can easily check that arctan2(X,Y) and arctan2(n * X, n * Y) give same result
    I have not actually checked the code for atan2 from math.h, should beheave the same

  15. Lauszus
    April 26th, 2012 at 21:23 | #15

    Why not just use the atan2() from math.h?
    I don’t think you will get better optimisation by adding your own. I trust the writers of c++ ( more 🙂
    It will also output the angle as a float, so you will get better resolution.

    Yeah, I can see that if you just multiply or divide with the same constant, it will return the same result.

  16. kas
    April 28th, 2012 at 21:30 | #16

    Goods points, I will use the standard atan2() with floats for better resolution

  17. Luigi
    April 30th, 2012 at 01:52 | #17

    Hello Lauszus!

    I was looking your project, and I must say, great work!
    Yet, there are some sections that I could not understand.
    For instance, the scales (position scales, velocity scales, zones…).
    How did you defined them?
    What does it depend on?

    Would you kindly clarify this doubts for me? Once again, very nice project.

    Best regards!

  18. Lauszus
    April 30th, 2012 at 10:39 | #18

    Have a look at my earlier reply to kas:

    Just write me again, if you need more details 🙂


  19. Luigi
    April 30th, 2012 at 17:22 | #19


    I see… yet, by looking at the .h file, I noticed, for example, “positionScaleA = 250” and “velocityScaleMove = 40”.
    How did you achived these numbers?

  20. Lauszus
    April 30th, 2012 at 17:35 | #20

    I found them by experimenting. It was more like trial and error, but still much easier than the P, I and D values.

  21. Luigi
    May 4th, 2012 at 01:30 | #21

    As I suspected, PID gains would be troubesome to find.
    But there are other aspects that may help the robot to stand?
    Wheel size is bothering me, do you have any recommendation about it?

  22. Lauszus
    May 4th, 2012 at 11:47 | #22

    You can always try to experiment by putting more weight on top or adjusting the hight.
    I recommend the same wheels as I bought!

  23. Luigi
    May 4th, 2012 at 15:03 | #23

    My robot is about 1m tall, and the gravity center is on top (put a big battery there).
    The wheel is smaller than yours, though.

  24. Lauszus
    May 4th, 2012 at 15:09 | #24

    Hmm, I think you will need some much larger wheels then?
    1m is pretty tall, what kind of motor do you have?

  25. Luigi
    May 4th, 2012 at 17:09 | #25

    This one:
    Tested it last week, it is strong enough to carry the robot, also it is fast.
    Robot is tall, but not heavy.
    So just adjusting PID gains may not be enough for it to stand with a wheel smaller than yours, despite the weigth being on the very top?

  26. Lauszus
    May 5th, 2012 at 00:43 | #26

    I really don’t know. It’s a bit hard for me to tell, when I can’t see the robot myself, sorry.
    You can always try with the wheels you have for now, and then try new ones if you are not happy with the performance.

  27. Luigi
    May 5th, 2012 at 04:32 | #27

    All right! I’ll do some experiments.
    Thanks for the tips, you’ve been very helpful to me!

  28. Lauszus
    May 5th, 2012 at 15:14 | #28

    Your welcome 😉 Please write again if you need further assistance!

  29. batista1987
    May 5th, 2012 at 15:42 | #29

    Hi guys,
    I want to know if anyone has tried to identify the pattern of those engines (the 12 volt 350 rpm Pololu 29:1), so as to obtain the mathematical model of the engine, and then simulate it in matlab simulink.

  30. Lauszus
    May 5th, 2012 at 15:52 | #30

    I havn’t, as I got no experience with matlab or any other simulation software 🙂
    It would also be nice if someone could simulate the 50:1 motors ( and compare them.

  31. kas
    May 7th, 2012 at 11:23 | #31


    >>Yes it should read 180 or 0 when laying down, depending
    >>on what side is facing the ground and 90 degrees vertically.

    I am confused, pitch (Kalman output) reads 90°, 180°, 270° (180° when standing)
    please confirm this is OK
    Everything looks fine up to PID calculation
    for the moment, my motors go full speed, whatever happens
    Please post a striped down version with no PS3 RC control,
    just code allowing still balancing
    I tried to do it, and can’t make it work


  32. Lauszus
    May 7th, 2012 at 13:06 | #32

    Mine read 180 degrees too, after I changed the pitch calculation to use atan2(), so yes my targetAngle is 180 degrees, see

    Have you confirmed that your PWM actually works correctly?
    To the following code work:

    while(1) {
       for(uint8_t i = 0; i < 100; i++) {
          moveMotor(left, forward, i);
          moveMotor(right, forward, i);
       for(uint8_t i = 100; i > 0; i--) {
          moveMotor(left, forward, i);
          moveMotor(right, forward, i);

    I that work, I will post a version without the PS3 functionality.

  33. kas
    May 7th, 2012 at 14:33 | #33

    This above code works well, after adding a delay(50) in each loop 😉
    motors are OK
    I also checked that rightCounter and leftCounter actually count

  34. Lauszus
    May 8th, 2012 at 00:26 | #34

    Argh, forgot to do that 🙂
    Okay. I will take a look at it tomorrow.

  35. Lauszus
    May 8th, 2012 at 10:45 | #35

    You can just uncomment the following line: and it will work the same way as there is not a PS3 controller connected.

  36. kas
    May 8th, 2012 at 15:09 | #36

    That’s what I did; I also commented those lines
    //USB Usb;
    //PS3BT BT(&Usb,0x00,0x15,0x83,0x3D,0x0A,0x57);

    I will make further investigations (inverse angle direction, change PID sign, inverse motors connections). Must be something really basic.

    Please try commenting receivePS3() on your own boot and let me know how you bot beheave; does it really stand still, any oscillation back and forth ??

  37. Lauszus
    May 8th, 2012 at 21:35 | #37

    It sounds like it’s something basic as you wrote -- check your connections and try to inverse your motor connections on your motorcontroller.

    I stands perfectly still. The stripped down version can be found at the following link:

  38. kas
    May 9th, 2012 at 18:00 | #38

    Thanks, I will let you know the outcome

  39. kas
    May 9th, 2012 at 19:48 | #39

    I am getting close 🙂
    Printing the PID values in the serial monitor, I found that iTerm gets very high (>3000) in seconds
    speedRaw is 100 and PWM motor 255 (full speed)
    If I omit the I component (PD only), I get better results, but no balance yet

    FWIW, in my own code, iTerm is clipped to avoid huge values:
    integrated_error += error;
    integrated_error = constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);

    I made a further stripping to your PID function, removing offset, turning and wheelPosition:

    void PID(double restAngle, double offset, double turning) {
    /* Update PID values */
    double error = (restAngle -- pitch);
    double pTerm = Kp * error;
    iTerm += Ki * error;
    double dTerm = Kd * (error -- lastError);
    lastError = error;
    double PIDValue = pTerm + iTerm + dTerm;

    /* Steer robot sideways */
    double PIDLeft;
    double PIDRight;

    PIDLeft = PIDValue;
    PIDRight = PIDValue;

    /* Set PWM Values */
    if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft);
    moveMotor(left, backward, PIDLeft * -1);
    if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
    moveMotor(right, backward, PIDRight * -1);

    Compared to your original code, lines have been removed, but no addition and no lines modifications
    Please try this modified function and let me know the outcome
    The bot should balance and possibly drift forward or backward (no encoders)

  40. Lauszus
    May 9th, 2012 at 20:17 | #40

    Why are you saying that 255 is full speed? Are you using my function or just analogWrite()? Because if you use my function -- setPWM(), then full speed will depend on ICR1. If you are running 16MHz, ICR1 will be 400, so 400 is top speed, but if you are running at 8Mhz, 200 will be full speed.

    You don’t have to constrain iTerm, you just have to find the right constant. Try lowering Ki.

    When using the stripped down PID loop the robot balance softly -- no oscillation. It doesn’t stay at the same place at all. To make it balance without the encoders I think you have to raise the PID values, so don’t worry if the balance isn’t perfect -- the encoders helps a lot!
    Before I implemented the encoders I also had to fine tune the targetAngle everytime the I used the robot -- I did this using either my PS3 Controller or the Processing application I made.

  41. kas
    May 10th, 2012 at 06:07 | #41

    I will try again with encoders and lower PID parameters
    Our bots are pretty similar (except the wheels) PID parameters should be close

    Do you still have the wheels I have for the moment ?
    Please describe the specific behaviour for the two wheels type ?
    Can you balance with the previous wheels and actual PID parameters ?
    Do you have adequate PID parametrers for old wheels in a previous code version ?

    If I manually move the bot forward, rightCounter I get positive values, leftCounter get negative
    I suspect they should be same sign, otherwise “wheelPosition = readLeftEncoder() + readRightEncoder();” would remain constant. Please confirm

    >>Why are you saying that 255 is full speed?
    sorry, it was just an analogy with analogWrite(), I do use your function ? setPWM() 😉

  42. Lauszus
    May 10th, 2012 at 09:14 | #42

    Yes I still have those wheels, but I only used it for a early prototype, so I have no PID values for them. But as I explained in the video, I have reinforced the hubs, so I cant’ just take them off to try the other wheels. Sorry!
    The performance were instantly better with the Banebots wheels. I recommend you buy them -- there is bit of waiting, but it’s worth waiting for!

    Sorry, I forgot to write that in the comment. When the robot is moving forward both encoders decrease and when the robot is moving backward then the encoders increase. I know is pretty counterintuitive, but I had them wired up that way, so I just compensated for that in the code.
    It’s pretty simple to change it in your code, just change the interrupt routines:
    But yes to are right, the should have the same sign 🙂

    And another thing to notice. The angle is less when moving forward and bigger when moving backward. For instance to make it travel forward, the targetAngle is set to 85 degrees -- you might have to change the orientation of your IMU, so it reflects my setup.

  43. kas
    May 13th, 2012 at 08:49 | #43

    I received my green wheels direct from Banebot online shop
    I ordered the 2 wide 3/4″ hubs. Unfortunatly, for this model, “2 wide” means really “2 wide” (2 X 20mm)
    I contacted them but them but those bastards did even care responding
    I ended up modifying the parts with my lathe
    For those interested, beware !!!
    before ordering the parts make sure you have a friend owning a lathe 🙂

    I will fit those wheels and let you know the outcome

  44. Lauszus
    May 13th, 2012 at 14:29 | #44

    That’s to bad. The information at there website can be really confusing.
    Looking forward to see the outcome 🙂

  45. Luigi
    June 3rd, 2012 at 01:10 | #45


    Hello all!
    You’ve been doing a great work.
    I need some help with the Kalman filter.
    I’m using the same code as yours, is the same, yet mine filter is too slow.
    When spinning the IMU quickly, the angle overshoots and takes a while to settle down.
    When reading only ACC angle, that is instant.
    What can cause this behavior?

    Thanks in advance!

  46. Lauszus
    June 3rd, 2012 at 15:57 | #46

    You should try to experiment with the three noise covariances:
    Q_angle // Process noise covariance for the accelerometer -- Sw
    Q_gyro // Process noise covariance for the gyro -- Sw
    R_angle // Measurement noise covariance -- Sv

    By lowering “R_angle” you trust the new angle more. So you should find just the right value, where it’s fast enough, but at the same time doesn’t get affected by vibrations. Alternatively try decreasing “Q_angle” as this will “trust” the accelerometer more!

    For more info see the following pages: and

    These three noise covariances are the key to make it faster and more correct 🙂

  47. Luigi
    June 3rd, 2012 at 23:58 | #47


    Thanks for the answer!
    I will play around with these parameters a bit, then I’ll post what I have found.

  48. Luigi
    June 8th, 2012 at 01:26 | #48

    Now the Kalman seems correct (the gyro was inverted ¬¬).
    Yet another issue rose: when I connect the 12V (motor supply) to the circuit, the PID value goes to 255, regardless of the angle I am currently at.
    Therefore, the motor spins to only one direction, even if it is the wrong one.

    Using serial monitor, this is very noticiable.
    It seems to work fine when only the Arduino is powered on, but when I connect the 12V, it presents this behavior.
    What can possibly be affecting the program?

    For tests sake, this is what I got:
    >Kas’s program, V2.
    >Arduino Duemilanove
    >My drive is the same as yours, “Dual VNH2SP30 Motor Driver Carrier MD03A”
    >The same motors as yours.
    >My IMU is more accurate, acc is 1.5g.

    I do not know what to think… how can a supply connection be affecting the program?
    Is there a catch to connect the drive perhaps?

    Thanks in advance!

    Best regards,


Comment pages
1 2 3 4 13 2196
  1. May 14th, 2012 at 15:14 | #1
  2. May 23rd, 2012 at 20:02 | #2