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:
[nggallery id=1]
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:
[nggallery id=2]

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. March 5th, 2012 at 03:25 | #1

    Excellent work! I’ve been working in a similar robot since july of 2010, but after a few setbacks I starter working on it again last year. I also decided to use mbed due to the speed. I previously used the PIC18F46K20. you can see some photos of my robot here

  2. Lauszus
    March 5th, 2012 at 10:12 | #2

    So far it looks pretty good. But I think your wheels are a bit to small. Originally I used these wheels from Pololu: They worked okay, but I got much better performance by buying these wheels: The reason why, is because the robot needs torque to stay balanced and not speed and those big wheels help doing that. Of course it depends on your motors, but those wheels worked better for me.
    What motors are you using right now? πŸ™‚

    – Lauszus

  3. March 5th, 2012 at 18:46 | #3

    @Lauszus Yes, I originally planned to use bigger wheels too, but a few distributors ships to my country, and if they ship stuff, the prices are very high. Now robotshops does not have them in stock, so I’ll have to wait a little more. I’m using these motors: they do not have encoders, but I think I will test how they perform before buying new motors with encoders. =D

  4. Lauszus
    March 5th, 2012 at 21:02 | #4

    It’s the same here in Denmark, that’s why I chose a european distributor. I also had to wait more than two weeks before they were even shipped.
    Okay. I really recommended getting some encoders as they provided the stop functionality and also made the robot stay in one place all the time.

  5. Lucky
    March 8th, 2012 at 05:52 | #5

    I bet balancing needs power. While balancing when not moving around may have its advantages, Why not make a couple of automatic “bike stands” that engage when desired speed is 0?

  6. Lauszus
    March 8th, 2012 at 09:40 | #6

    I see what you meen, but don’t you take all the fun out of it if you just put some wheels on it. Then you could just have made a four wheeled robot to begin with.
    Anyways it can actually run for several hours with a battery fully charged.

  7. Kas
    March 8th, 2012 at 10:26 | #7


    Beautifull implementation, congratulation
    Your bot balances pretty well

    I would like to test the Banebots wheels, please confirm you ordered “2 wheels wide” hubs
    as per
    why not “One wheel wide” hub ??

    Keep up the good job

  8. Lauszus
    March 8th, 2012 at 11:28 | #8

    Thank you.
    Yes that was exactly the hubs i ordered, see the hardware section above for all the information about the hardware that I used.
    The reason why I choose the “2 Wide” version, is pretty simple. Because the wheels are 2 wide (they are in the “series 800”). BaneBots got there own system, so it’s possible to mix up different kinds of wheels – you could for instance mix up three types of wheels on a 3 wide hub.
    Take a look the this page: at “NOTE”. I also though it was very confusing, regarding to which hubs fitted which wheels, so I actually emailed robotshop asking if they were the right hubs for those wheels.

    I got to ask you. Are you the same Kas that created “Balancing robot for dummies”: You post on the Arduino forum, where the main reason why I choose the motors, brackets and motorcontrollers. I have wanted to thank you, but didn’t know if you’re still active in the balancing robot community πŸ™‚

  9. Cosmin
    March 8th, 2012 at 22:20 | #9

    nice work

    I tried to test the library but I get stuck at:
    Bluetooth Dongle Connected
    CSR Initialized
    HCI Reset complete
    Local Bluetooth Address: 00:09:DD:50:07:7D
    Wait For Incoming Connection Request

    the 4 leds on the DualShock 3 flash constantly but it does not connect
    what could be the problem?


  10. Lauszus
    March 8th, 2012 at 23:06 | #10

    You have to set the bluetooth address into the PS3 Controller first. Try to first to plug in the dongle, then unplug it and then plug in the PS3 Controller. It should then work fine. Or simply just change the following: to your bluetooth address and the you don’t have to plug in the dongle before you plug in the PS3 controller.
    See the wiki for more details:


  11. Kas
    March 9th, 2012 at 08:42 | #11

    Thanks for the clarification, the RobotShop informations are reallly confusing
    I will order those wheels right away

    I now realize that my PID/encoder/motor code is far from perfect
    Did you implement the Arduino version of your code ??
    I would love to test it on my bot

    Kas, the genuine one πŸ™‚

  12. Lauszus
    March 9th, 2012 at 10:28 | #12

    Your welcome – yes it is very confusing.
    I don’t know if I had the right approach, but it seemed more intuitive to adjust the target angle instead.
    What do you mean by the Arduino version of my code? πŸ™‚ I only used the Arduino for the bluetooth functionality. It’s the mbed that drives the whole thing. But I think you could easily port it to an Arduino, as I only use about 1ms when not receiving any serial data.

    Thank you again for you great guide. It were a big inspiration to me.


  13. Kas
    March 9th, 2012 at 18:09 | #13

    My pleasure

    Well… I mean your mbed code ported to Arduino πŸ˜‰
    I am especially interested by the void PID(double restAngle, double offset) function
    Would you mind explaining this part of your code ??


  14. Cosmin
    March 9th, 2012 at 22:29 | #14

    finally it works,
    I had 2 problems
    put the mac addres of the dongle in the dualshock3, I used

    the usb shield and the dongle use more power than the arduino supply from the usb
    so needed to connect an external power supply.


  15. March 10th, 2012 at 07:15 | #15

    That is a awesome robot, I have been thinking about getting into walker robotics myself. Any pro/con to wheels that I should take in note?

  16. Lauszus
    March 10th, 2012 at 12:04 | #16

    Okay. Glad you succeeded. I have never had any issues setting the bluetooth address.

  17. Lauszus
    March 10th, 2012 at 12:07 | #17

    @Mads Barnkob
    Thank you.
    I recommend those wheels that I bought, as they are really good quality. Remember that they should be a bit heavy, as it’s torque that keeps it balancing not speed, so don’t go for any tiny wheels. Originally I bought these wheels from Pololu:, but they where not as good as the BaneBots one.

  18. Lauszus
    March 10th, 2012 at 12:26 | #18

    Okay I will give it a try:

    The restAngle is a constant (90 degrees), I only adjusted it before I implemented the encoders, as they keep adjusting the angle, so it remains in the same position.
    The forward and backward commands including the targetOffset, are sent from the Arduino via serial. See this line:
    But if you just set the offset to for instance 5 degrees, it will eventually fall over, so to avoid this, I adjust the offset depending on the wheel velocity. This can also act as an break, as if the wheels are going forward and you set the backward command, the target offset is actually getting bigger than the values being sent.

    The stop function is a bit different. At first there is three zones – this will slow it down again, when reaching the targetPosition. It adjust the restAngle, so it moves to the right position. At last I have also implemented the wheel velocity. It’s main purpose is to keep it in the same spot. In short the positionError force it to go to a certain position and the wheelVelocity keeps it there.
    At last I limit the restAngle angle, so it doesn’t overreact when the error is great. Think of it as the constrain function for the Arduino.

    The next step is just a normal PID controller. The pTerm looks at the error, if it’s for instance 5 degrees it will multiply it by a constant, to make the error less. The problem is that this will make the robot start to oscillate – this is what dTerm prevents: it looks at the difference between the last error and the error. So if the last error were 5 degrees, but the new error is 3 degrees, the diference is -2, so it will actually slow the robot down, when it’s reaching the correct angle. At last the iTerm will force it not to be satisfied until the error is zero – this will also helps the robot from drifting, as if the error is for instance just 0.5 degrees, it would start to drift, if the iTerm values wasn’t there.

    At last I split up the PIDValue, and set and offset to it, if the Arduino sends an turn or rotate command. It will keeps balancing if you just set the exact same offset to the difference motors, as I explained in the video.

    Hope that answered your question fully πŸ™‚ If not just send me another comment.

  19. Kas
    March 11th, 2012 at 08:21 | #19

    Thanks for the clarification, I was not too sure about velocityScaleMove and zone XX

    Now, my roadmap:
    – Bot reengineering with IMU placed near the wheel axis and new “green” wheels
    – Look for possible noise with a scope (tantalium capacity across motor leads)
    – Add a PID per motor

    A PID per motor should correct individual behaviour and allow control based on velocity, not PWM.
    It should also give higher torque at low speed

  20. Lauszus
    March 12th, 2012 at 00:39 | #20

    Nice. Do you got a blog or anything like that, so I can follow your progress? πŸ™‚

  21. zmiguelb
    March 12th, 2012 at 04:42 | #21

    Hi Lauszus,
    You blog is quite amazing, I stumbled upon it because I am building a similar robot to yours and although I have been able to implement everything so far I can’t seem to hit the correct PID values.

    Would it be possible for you to provide me with some insight how you calculated the PID terms.
    I’ve been through a lot of websites and most of them explain PID but not how to get there without a model (expect the Ziegler method).

    Any help would be appreciated πŸ˜€

  22. Lauszus
    March 12th, 2012 at 09:15 | #22

    I didn’t calculate the values – it was more like trial and error. But I followed this guide: see page 42:

    1. Select typical operating setting for desired speed, turn off integral and derivative part, and then increase Kp to max or until oscillation occurs.
    2. If system oscillates, divide Kp by 2.
    3. Increase Kd and observe behaviour when changing desired speed by about 5% and choose a value of Kd that gives a fast damped response.
    4. Slowly increase Ki until oscillation starts. Then divide Ki by 2 or 3.

    I more or less followed that, but I then fine tuned it myself. I recommend using a wireless application, so you can set them wirelessly, as mentioned in the video, as you have to spend a lot of time finding those values.

  23. zmiguelb
    March 12th, 2012 at 13:20 | #23

    Thanks for the fast reply Lauszus.
    I will try that approach as soon as I get home. (Crossing finger hoping it will work πŸ˜‰ )

  24. Kas
    March 12th, 2012 at 18:54 | #24

    No blog, will keep you informed

  25. Lauszus
    March 12th, 2012 at 18:55 | #25

    Okay, looking forward to see your robot in action πŸ˜€

  26. March 19th, 2012 at 19:45 | #26

    Hi Kristian,
    Cool, fun to see that there are more nerds and potential engineers around in Holstebro.
    It’s a really fine robot and some good choices as well.
    Niels Bo

  27. March 20th, 2012 at 21:34 | #27

    Are you still following the Arduino Forum Lauszus?

  28. Lauszus
    March 20th, 2012 at 21:56 | #28

    @Niels Bo
    Thank you Niels, nice to hear that from an engineer πŸ™‚

  29. Lauszus
    March 20th, 2012 at 21:57 | #29

    Yes I’m stil following it, but didn’t have the time to check out the forum until now πŸ™‚
    Your robot looks really nice (,60170.msg732345.html#msg732345), I have one comment though: on the picture it looks like you have put the IMU on second level of the platform – you should put it just at the axis instead πŸ™‚

  30. March 25th, 2012 at 10:04 | #30


    You’ve done a great job, the robot’s turning is very impressive and when it stops it stands absolutely still. Have you tried climbing down small steps with it? And does it go up a slope? If so, what’s the maximum inclination? What happens when you stop it on a slope?


  31. Lauszus
    March 25th, 2012 at 16:00 | #31

    Thank you, I’m also very pleased with it πŸ™‚
    It can handle steps pretty good and slopes to, but I will not stay at the same position on a slope. You should lower positionScaleC and velocityScaleStop to make it do that, but it will also be more “aggressive” and make it vibrate more when standing still. See this line in the code for more info:
    I don’t know the maximum inclination, I will let you know if I ever measure it.

  32. March 29th, 2012 at 19:48 | #32

    Nice robot! I think you gave me the inspiration to continue with my both also. Now I half to start working on X-bot v3. (

  33. Lauszus
    March 29th, 2012 at 21:26 | #33

    It’s always nice to inspire people, I got inspiration from πŸ™‚

  34. Lucas Ezequiel BLanco
    March 30th, 2012 at 23:48 | #34

    Very good work! THE FOLLOWING I ask, as you have done to adapt the controlardor 5v engine to 3.3 mbed plate. Thank you very much for your respusta and sorry for the translation because I do not speak English and I do it with google translator travez

  35. Lauszus
    March 30th, 2012 at 23:57 | #35

    @Lucas Ezequiel BLanco
    The motors are not running 5V, if that what you asked about, they run at 12V, but the motorcontroller will take care of this.
    I haven’t done anything to convert the logic from the mbed, they work just fine with a logic level of 3.3V.


  36. Patrick
    April 12th, 2012 at 18:13 | #36


    I just bought the exact same motors, wheels and hubs as your robot but I found that the hubs are a bit too large for the motor shaft (lenght wise).

    How did you solve this problem?


  37. Lauszus
    April 12th, 2012 at 19:03 | #37

    I didn’t. I just but mounted them as they were.


  38. Patrick
    April 16th, 2012 at 01:40 | #38

    Thank you,

    I realized that if I tightened them enough they wouldn’t move!

    Thanks again

  39. kas
    April 17th, 2012 at 20:17 | #39

    Hi Lauszus
    I just saw that you ported your code to the Arduino platform
    This is great for the Arduino community
    Did you allready physically test this code on your bot ??
    Do you get the same outstanding beheaviour as with the mbed implementation ??

  40. Lauszus
    April 17th, 2012 at 22:14 | #40

    Yes it’s running just next to me πŸ™‚ The performance is just as good as on the mbed.

    I’m actually really impressed with the performance of the atmega328p – every loop takes approximately 3ms, just as good as the mbed running 96MHz. But the serial receiving also takes a lot of time on the mbed, but the Arduino still have to run the USB Host communicating with the PS3 Controller.

  41. kas
    April 19th, 2012 at 17:52 | #41


    I looked at your sensor aquisition code and need some clarification:

    >> // (gyroAdc-gyroZero)/Sensitivity (In quids) – Sensitivity = 0.00333/3.3*1023=1.0323
    why 1023 ? 10 bit is 1024 values (0 to 1023) please correct me if I am wrong

    >>double R = sqrt((accXval*accXval) + (accYval*accYval) + (accZval*accZval));
    Why do you use 3 Acc’s ?
    One Acc axis should be parallel to the wheels axis and remain horizontal, with no gravity modification

    >>double accXval = (double)((double)analogRead(accX) – zeroValues[1]) / 102.3;
    seems that the 102.3 coeff has no impact on the calculation and can be ommitted

    >>if(inverted) accYval–;
    please elaborate on this one

    // -1g when lying at one of the sides
    Do you perform callbration when the bot is lying ?


  42. kas
    April 19th, 2012 at 19:57 | #42

    An additional one πŸ™‚

    >>define leftA PIND0 // PD0 – pin 0
    >>define leftB PIND1 // PD1 – pin 1
    Pin 0 and 1 are used for Arduino serial communication
    Any possible interference with code loading ??

  43. Lauszus
    April 19th, 2012 at 22:12 | #43

    Try to do the math. You want to convert a voltage into quids, so if it is 3.3V that it equal to 1023 – so the equation looks like this: 3.3V/3.3V*1023=1023

    I calculate the length of the force vector, see You can also just use two or only one axis to calculate the angle, but all three will give you more precision.

    You can also just calculate the angle using sinus:

    accYangle = asin(accYval)*RAD_TO_DEG;

    But yes I see your point, that the x-value are not changing that much, as it’s not affected by any gravitational force. But it will still be affected by the acceleration of the robot.

    Yes I calibrate the robot when it’s laying down, but the “zeroValue” for the y-axis is really not the “zeroValue”, as it’s either +1g or -1g depending at what side is laying down.

    You can easily use pin 0 and 1 – it works just fine!

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


    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

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

    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?

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

    This is the return value of getAccY()

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

    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?

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


    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 πŸ™‚

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

    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? πŸ™‚

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


    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

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

    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 πŸ™‚

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

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

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

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

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

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

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

    Okay I will try it tomorrow and let you know how it went πŸ™‚

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

    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

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

    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.

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

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

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

    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!

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

    Have a look at my earlier reply to kas:

    Just write me again, if you need more details πŸ™‚


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


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

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

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

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

    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?

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

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

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

    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.

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

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

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

    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?

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

    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.

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

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

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

    Your welcome πŸ˜‰ Please write again if you need further assistance!

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

    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.

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

    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.

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


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


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

    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.

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

    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

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

    Argh, forgot to do that πŸ™‚
    Okay. I will take a look at it tomorrow.

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

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

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

    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 ??

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

    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:

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

    Thanks, I will let you know the outcome

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

    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)

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

    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.

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

    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() πŸ˜‰

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

    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.

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

    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

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

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

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


    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!

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

    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 πŸ™‚

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


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

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

    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,


  92. Lauszus
    June 8th, 2012 at 14:08 | #92

    I would check my connections to the motor driver – see this image for the pinout!

    How did you drive the motors before? Did you just connect 12V to the motordriver and then plug in the usb cable to your Arduino to power it with 5V?
    Remember that you just have to connect your battery to the screw terminals and connect one of the pins labeled VIN on the pin header to VIN on the Arduino.

    You also don’t have to connect the DIAG/EN pins to anything, as they are connected to a pullup resistor. See the schematic for more information.

    Best Regards
    Kristian Lauszus

  93. Luigi
    June 8th, 2012 at 18:24 | #93

    I did what you suggested. Now Arduino and Drive are supplied with the same 12V from the battery.
    Vin pins are connected to each other.
    Yet the problem persists!

    I’ve ran a test whitout the motors, and through the leds present on the driver, I can see the supposed direction.
    When I lean the robot to one side, leds light up red.
    To the other side, they light up green.

    However, when I connect the motors (even just one!), they spin to only one direction (just green leds), same as before.

    What could be now?

    Thanks in advance,


  94. Lauszus
    June 8th, 2012 at 18:31 | #94

    Have you connected your motors the right way? You should connect the red and black wire to OUT1A/OUT1B or OUT2A/OUT2B.


  95. Luigi
    June 8th, 2012 at 19:01 | #95

    Yes, motors are wired to OUT screw terminals.


  96. Luigi
    June 8th, 2012 at 20:14 | #96

    I’ve ran some tests, and this is what I have:

    12V (battery) supplying both Arduino and motor drive;
    Arduino supplying 5V to the motor drive;
    Arduino supplying 3.3V to the IMU;
    Grounds (Arduino/IMU/Drive/Battery) are all common;

    I did this:
    Monitored some variables, and when a motor is connected to the drive (in the OUT terminals), the accelerometer angle goes crazy. Without any motor connected, acc is fine.

    Have you seen anything like this before?


  97. Lauszus
    June 8th, 2012 at 20:19 | #97

    It’s possible because of the noise from the motors. Are you using a digital IMU?
    Try to add some large decoupling capacitors add the VIN input of the Arduino, to reduce noise.


  98. Luigi
    June 8th, 2012 at 20:35 | #98

    No, the IMU is analog.
    Actually, is made of a acclerometer and a gyroscope separated.
    How does the noise affects the sensor (supply 3.3V?)?
    Large capacitors? Large enough to be eletrolytic or ceramic will do?


  99. Lauszus
    June 8th, 2012 at 21:18 | #99

    It shouldn’t affect it then, unless the noise is really great!
    I would put a 100uF and a 100nF at the supply.


  100. Claudio
    June 9th, 2012 at 02:08 | #100

    Hi, congrats to you, very nice project, very nice indeed. I’m basing a lot of my balancing robot project on yours. I would like mine to be as stable as yours.
    I have a few questions.

    How did you connect the wheel to the motor shaft? This motor have a D shaped shaft and the wheels have either a HEX or round connection.

    How important is the motor encoder to the balancing part? My understanding is that your primarily use it to control the robot using the joystick, is that correct?

    How would you go about compensating for the battery level in your code?

    Thank you very much. I’m sure when I start the project (I live in Brazil and the delivery time is enormous!) I’ll have quite a few more questions.


  101. Luigi
    June 9th, 2012 at 05:37 | #101

    Problem solved. It was noise, but not electrical noise.
    It was mechanical noise – vibration.

    I simply detached the motor from the frame and it worked (I still do not believe this).
    The motors were put back in the frame with some foam, now the system appears to be working.

    Next step will be PID tuning!

    Thanks for the help,

  102. kas
    June 9th, 2012 at 16:29 | #102

    Hi Luigi,

    Those are indeed mechanical vibrations transmitted by the motor to the IMU
    been there, done that …
    Try to mechanically decouple the IMU from the frame, and/or move it to another location, but the best place is at the motor axis… where the vibration is generated πŸ™‚

    Good luck

  103. Lauszus
    June 9th, 2012 at 17:58 | #103

    I have placed my IMU at the motor axis, as kas mentions, and I have never had any problems with vibrations! I don’t use any foam or anything to reduce any vibrations from the motors πŸ™‚

  104. Lauszus
    June 9th, 2012 at 18:44 | #104

    To connect the wheels to the shaft, you have to use a hub. Since my motor shaft were 6mm and the HEX connection of the wheels were 0,5 inch in diameter, so I ordered these hubs for it to work:

    The encoders a really important if you want to make i balance as good as mine. It will help the robot stay in the same place, but also make it react to pushes from any object, so if you can afford it, I really recommend encoders as well!

    I would just change the PID constant depending on the battery level. I have tried that before, but it works fine without it.

    The delivery time can also be very very long here in Denmark too, unless you pay a lot to get it shipped by DHL or something like that, but then you also have to pay for it.


  105. Claudio
    June 9th, 2012 at 20:58 | #105


    Thank you for the reply, Lauszus.

    One other thing, if I can’t find a motor with similar characteristics to the one you used, should I pick one with higher or lower RPM? Yours have 350 RPM, it is better to one with 500 RPM (lower Gear Ratio) or one with 200 RPM (higher Gear Ratio).

    It is possible to install a motor encoder later on? To your type of motor particularly.


  106. Lauszus
    June 9th, 2012 at 21:10 | #106

    It really depends on the torque of the motors too. The torque is actually the most important thing. You only need the higher RPM to drive is faster, but if you are not planning on remote control, then it really doesn’t matter – you should of course no pick motor with 50 RPM or so.

    Yes it is possible to add a motor encoder later, there a many different types. Try searching at different sites. For instance have a look at Alibaba, as they are general really cheap, and you don’t need that high precision encoders for a balancing robot. For instance have a look at his seller:


  107. Claudio
    June 10th, 2012 at 01:17 | #107


    Nice, so this one should work fine, right?

    Since I do not want to pay large amounts for delivery, it will be at least 30 day for me to receive my goodies, and I really don’t want to have to order again. That’s why I’m testing your patience with my questions. =D

    Thank you for the reply.

  108. Claudio
    June 10th, 2012 at 05:19 | #108


    Lauszus, have you tried using only the arduino as the microcontroller? My idea is only using a arduino UNO or MEGA to do all the calculations for the Kalman Filter and PID (using the output from the encoders) and motor driver. Also, I would like to connect my android phone via bluetooth and “control” the robot with the phones gyro.

    Do you thing it is possible or it is a bit much for the atmega? I do not know the mbed microcontroller, and never used another microcontroller other than arduino.


  109. Lauszus
    June 10th, 2012 at 11:17 | #109

    I think the motors would be okay. They look exactly like the one I got, it’s just a different gear ration. But why not just order from Pololu? It’s the same price and located in US too?

    I am using an Arduino right now! Read the post again: and have a look at Github:

    It would be theoretical possible, but you will have to figure out to do that yourself. I have never seen anything like that. But try to search fro RFCOMM – that what I would use. I’m thinking about writing RFCOMM library for the USB Host Shield in the future.


  110. Claudio
    June 10th, 2012 at 15:03 | #110


    At Pololu the delivery is at least US$ 20,00 higher. And I have to use FedEx (it is the best choice). And using FedEx I’m sure to pay the duties of my county regarding international shipments. If I use USPS ther’s a chance I don’t have to pay the duties, which is 60% (!!!) of the price plus delivery fees.

    I’m not in a hurry and every dollar count.

    I saw that you had ported to Arduino but I didn’t know that you had implemented the hole thing. Good to know.

    Using Android and Arduino over Bluetooth it is not a problem. There’s a app and arduno library called Amarino that implement this communication very powerfully. It can send the info of lots of sensors on the phone. Since my priority for this project is implementing the Kalman Filter (or a Particle Filtes, I’m not sure yet) and PID control I think I’ll just use Amarino for the communication.

    Another thing, do you have the input from the encoders on the PID control? I don’t want just to copy your code, I really want to create my own, that’s why I’m asking and not just checking the code.

    Thank you for the reply, Lauszus!

  111. Lauszus
    June 10th, 2012 at 15:24 | #111

    Okay, that makes sense. I also try to order from inside the EU as much as possible, so I don’t have to pay any extra taxes.

    Oh yeah, that’s true. I thought you were talking about the USB Host Shield. Amarino just uses a serial bluetooth module with build in support for RFCOMM, I thought you would like to use the USB Host Shield.

    What is a particle filter? Don’t you mean a complimentary filter or something like that?

    Yes I use the input from the encoders in my PID controller, see the following lines:

    It’s not copying, just because you get inspiration form my code. I recommend you try to read and understand my code, before you try to create your own – or it will be like walking – or balancing πŸ™‚ – in blind. I have read a numerous of different codes for balancing robots, including kas’s code (see:, before I created my own.


  112. Luigi
    June 10th, 2012 at 17:53 | #112

    Maybe it is because I use an aluminium frame, while yours is made of wood?


  113. Lauszus
    June 10th, 2012 at 19:11 | #113

    That’s would make perfectly sense πŸ™‚ I will have that in mind if I ever have to create a new one.

  114. Claudio
    June 11th, 2012 at 03:28 | #114


    That’s very true, Lauszus. Kinda silly of me trying to reinvent the wheel.

    One thing I really want is to implement the “full Kalman Filter”. It really works very very well when we set the Kalman Gain to a fix value, but since I study Mechatronic Engineer and the Kalman Filter is one of the most used filter in the industry, I would like to learn it a bit more in depth. I do not think I will be able to see any difference with the recursive changes in the Kalman gain, but will be a nice learning experience.

    Particle Filter it is a different approach with the same goal. It uses the same methodology of measure and update as the KF but it uses particles, initially randomly distributed on the “world”. Every iteration of the algorithm the particles get closer and closer to the “real” position. Kinda hard to explain like this, but it is something like that.

    Since I never saw anyone use this method to control the balancing robot (maybe for a reason, I doesn’t really work) and I would like to try it. As a learning experience also. I know it is possible since the math behind is the same, I just don’t know if its a good solution.

    They use particle filters to control the Google car.

    This is a course by the leading creator of the Google car. Unit 3 of the course teaches particle filters. I started with this course. I highly recommend it.

    Cheers and thanks for the links!

  115. Lauszus
    June 11th, 2012 at 17:55 | #115

    Okay. I have also written a repport just before new year regarding the basics of Kalman filters (I’m in the last year of high school) and it was differently one of the most interesting reports I have ever written. It was like a whole new world opened to me πŸ™‚
    I am thinking about rewriting the Kalman code for my balancing robot, to see if I can’t get it better than it currently is.

    Ahh okay, sorry for my ignorance. I thought it was a typo or something like that πŸ™‚ It sounds a bit like entropy ( or am I wrong?
    Please keep me updated if you ever get it working! I would love to test out and compare it to the current Kalman filter.

    Argh, I would love to have a course like that, I can’t wait to go to university!


  116. Claudio
    June 11th, 2012 at 20:11 | #116


    Lauszus, the course is open to anyone. And I’m pretty sure that you have all the prerequisites for the class.

    You really are in the right track. I could not imagine building one of these when I was in high school.

    What kind of modifications do you think of doing on the code? Do you have anything in mind?

    Have you ever thought about building a QuadCopter, aka QadriRotor? That’s my main goal. I wanted to start with the balancing robot to begin with a easier (in comparison, of course) task.

  117. Claudio
    June 11th, 2012 at 23:15 | #117


    Lauszu, how long can you use the robot with one fully charged battery?

  118. Lauszus
    June 11th, 2012 at 23:28 | #118

    Okay, I didn’t realize that. I will go to university after this summer and hopefully do project like that!

    The kalman filter looks like it’s not totally right. But the main pourpose is to find the optimal noise covariances.

    Not right now, but Thomas is designing one at the moment. Take a look at his post:

    I can use it for nearly two hours or so!

  119. Claudio
    June 12th, 2012 at 16:35 | #119


    Lauszus, how do you check for the battery voltage?

  120. Lauszus
    June 12th, 2012 at 19:01 | #120

    Right now I don’t. I just change the battery when it the speed of the robot is to slow πŸ™‚

  121. Claudio
    June 15th, 2012 at 13:39 | #121


    Lauszus, what do you power using the 5V voltage regulator (7805)? Why not use the 5V from the Arduino? To much current?


  122. Lauszus
    June 15th, 2012 at 14:18 | #122

    I only used a 7805 before, when I used the mbed. Now I just connect 12V to VIN on the Arduino, and it hasn’t been a problem at all!

  123. Claudio
    June 15th, 2012 at 23:50 | #123


    Nice, good to know. Thanks!

  124. Claudio
    June 15th, 2012 at 23:56 | #124


    You are powering the motors through the Arduino Vin? Is it possible to post your current pins and wires disposition?

  125. Lauszus
    June 17th, 2012 at 16:04 | #125

    I power the motors directly from the 12V battery connected via the screw terminals on the motordriver. If you have a look at the schematic, you can see that the pin 1 and 5 on the JP2 connector is connected directly to the battery, so I have connected one of them to VIN on the Arduino.

    The Arduino 5V rail powers the encoders, buzzer and the motorcontroller logic.
    The 3.3V rail powers the IMU and is also connected to the AREF pin.
    It’s pretty basic so you should be able to figure it out by that description.

  126. Luigi
    June 28th, 2012 at 16:34 | #126


    Hello all!
    I’ve fixed the mechanical issues (vibrations) by using viscoelastic materials as dampers.
    Yet another issue arose – PID tuning.
    I know that it would be pretty difficult, do you have any hints?
    I am suspicious about the loop time (using this section from yours/kas’s code as parameter).
    Mine “last loop useful time” is about 4ms, while “last loop time” is about 10ms.

    Does these values are good for PID?
    I have noticed that yours “last loop useful time” is about 2ms.

    Obs.: I am using Arduino Mega instead of Duemilanove.

    Thanks in advance,

  127. batista1987
    June 30th, 2012 at 22:54 | #127

    hi luigi,
    i think that you must measure the time necessary for read the value from your imu and encoder for any loop, and than you can set a loop time that allow to read all value write the duty cicle to the motor. but for a better solution you must consider the best users @kas and @lauszus

  128. kas
  129. Lauszus
    July 1st, 2012 at 13:25 | #129

    I don’t think I understand your problem? Do you wan’t to remove the time fixed loop and then put then use the delta time in the PID controller?

  130. Lauszus
    July 1st, 2012 at 13:30 | #130

    Very interesting.
    I would like to see how well fast it can go backward and forward, as I don’t think they will be as fast as normal brushed motors, or what do you think? πŸ™‚

  131. kas
    July 1st, 2012 at 16:23 | #131

    No idea

    At least that’s what he said:
    >>…It goes quite fast, thanks to the slightly over-spec’ed steppers…

    More info here:

    I am tempted to use stepper motors (no backslash, no encoders…)

  132. Lauszus
    July 1st, 2012 at 16:32 | #132

    Hmm, please let me know if you do so. I would like how if they can be used as an alternative – they are also half the price compared to the motors I used.

  133. kas
    July 1st, 2012 at 20:00 | #133

    Just collected today, on a rainy Sunday πŸ™‚

    Another one with steppers:

    and a ballBot:

    So many projects…

  134. Luigi
    July 2nd, 2012 at 19:53 | #134

    Hello all!

    It is balancing, finally!
    My last post was because the execution time of my loop is double of yours, I thought it was a problem… but it was’nt, PID tuning was.

    Just one weird thing though.
    Without encoder data, the robot is much more stable.
    It should be perfectly still when encoder informantion is added, but now I am better without it.
    Any hints?

    Using the encoder like you are doing is called “feed forward” control.

    Best regards,


  135. Lauszus
    July 2nd, 2012 at 19:59 | #135

    Nice, do you got a video of it? πŸ™‚
    How did you implement the encoders? Using my method or Kas’s?
    Thanks, I didn’t know that it was called that. I just thought it made the most sense to implement the way I did.


  136. kas
    July 27th, 2012 at 17:02 | #136

    I ordered and received a MPU-6050 breakout board (15$ delivered):
    This is a 3 Gyro + 3 Acc + data fusion in a single chip,
    I2C digital output and adjustable Gyro/Acc sensitivity
    Looks promising πŸ™‚

    I will let you know the outcome

  137. Lauszus
    July 27th, 2012 at 17:44 | #137

    Nice, we have thought about buying the MPU-6050 as well! It looks really interesting – the new Nexus 7 even uses it: πŸ™‚
    Looking forward to hear from you again.
    I will try your code as I promised next week! I have spent most of my time implementing the RFCOMM protocol to the USB Host library and doing some freelancer jobs.
    See the RFCOMM example sketch: and the source code: regarding RFCOMM.
    We will write a blog post tomorrow with more information.

  138. kas
    July 28th, 2012 at 07:15 | #138

    I though you heard about MPU-6050 allready πŸ˜‰
    Let me know the outcome for my code

  139. Lauszus
    July 30th, 2012 at 17:02 | #139

    Yes. We have actually talked about using it with the balancing robot instead of the current sensors πŸ™‚
    I also just orderede the MPU-6050 to check it out myself.

  140. Claudio Donaté
    August 4th, 2012 at 01:44 | #140

    Hi Lauszus. Let me ask you something. Would it be difficult to adapt your Processing code to use the Bluetooth from my notebook, conecting to a RN-42 Bluetooth module ( I do not have a xbee module. Thanks!

  141. Lauszus
    August 4th, 2012 at 02:47 | #141

    @Claudio Donaté
    No it would be exactly the same! I also recommend a bluetooth module like that in the end of the video if you don’t own a Xbee module already πŸ™‚

  142. Claudio Donaté
    August 4th, 2012 at 13:01 | #142

    I tried using the bluetooth, checked the COM port, the baud rate and click connect but the program crashes. On your code you have lots of printf, how do I see the output? I’m using the compiled version.
    My note is paired with the RN-42, but it is not connected, the LED remains red and never turn green.
    I looked the code over and I thought it would pretty similar using with a BT module. Nice to know will work.

  143. Lauszus
    August 5th, 2012 at 01:34 | #143

    @Claudio Donaté
    Are you even sure you have set the module up correctly? Make sure you can make a normal connection to it using a terminal application. I recommend PuTTY if you are running Windows and CoolTerm if you are running Mac OS X.

  144. Ryan
    August 5th, 2012 at 20:26 | #144

    I got here from the “Balancing Robot for Dummies” thread on the Arduino site. Are you also the primary contributor to that thread? What you have done was my ultimate goal before I found your robot and I am using many of the same components. I have a prototype almost balancing but I just can’t get over the final hump. It won’t balance. I was hoping to get a dialog going with you. Perhaps you can tell me what I could provide you with that would better allow you to critique my robot and code.

  145. August 5th, 2012 at 23:28 | #145

    No I just answered some peoples questions now and then πŸ™‚
    I’m always glad to help other people out!
    What code are you basing yours upon? Mine: or somebody else? Or are you rewriting it from scratch?

  146. Ryan
    August 6th, 2012 at 17:52 | #146

    I am essentially rewriting from scratch although “your” Kalman filter was offering better performance than mine so I “borrowed” it. My CPU is a Stellaris LM3S9B96 Cortex M3. I have spent hours tuning PID parameters and while I’ve gotten it close several times I just can’t get it to balance. It seems it’s either too aggressive and subsequently unstable or not aggressive enough and just falls over. I had an old Traxster II chassis laying around that I pulled the tracks off and replaced with Pololu wheels and I believe the same motors as you have. The platform is not symmetric about the pitch axis but I empirically determined the corresponding angle at which it should balance so I don’t think that should be a problem. I also have the IMU mounted at the top of the platform which it seems nobody does but there just so happened to already be mounting holes up there. It also made more sense to me at the time. What kind of resolution do you have on your motor controller. I bought a Trex from Pololu which seemed neat at the time but it only offers 7 bits of resolution. However, it’s actually a stack of two boards so I could pull the smarts off it and PWM the board my self and possibly get 16 bits of resolution.

    How can I help you help me? I could send you my code but it involves quite a bit more files than an Arduino project. I’m using the IAR compiler and it was also originally written to run on the RTOS which comes preloaded in ROM. It currently just runs in a while loop but I plan to move it back over to the RTOS when I get it balances.

  147. August 7th, 2012 at 14:20 | #147

    I think the hardware is just as important as the software – or even more, if your hardware is not proper designed it will never be as good. So I think you should make it as symmetric as you possible can!

    You should put the IMU at the wheel axis to reduce noise/vibrations – the things is if you put in on top it will be shaken much more! So put your IMU at the wheel axis.

    My resolution is determined by my pwm output and since I’m using a 16 bit timer on the Atmega328 it’s 16bit. Another aspect is that you should use a high frequency as possible – my motorcontroller supports up to 20k, so I set the pwm frequency accordingly: and here is PWMVALUE defined here:

    Just upload you code to github or something like that and I will have a look.


  148. Ryan
    August 7th, 2012 at 17:04 | #148

    I’m working on different hardware. I’m trying to find someone who will CNC some aluminum plates which will stack in a similar configuration as yours. The Trex motor driver I am using does support a 20kHz PWM however the communication interface requires you send it 7bit speed commands so you only have the option of 128 different speed settings. It also supports analog and PWM but the PWM are meant to come from a RC receiver so I am pretty sure they are measuring the pulse width with a timer and then generating a new PWM with the timer value. I’m not sure what the resolution on their ADC or timers are so I don’t know that either method will buy me any resolution. I will upload my entire IAR project directory and you can tell me what you see or let me know if you need any help navigating my code. My email is ryan(dot)lush(at)gmail(dot)com

  149. August 7th, 2012 at 17:37 | #149

    I don’t know for sure that you need a higher resolution, as I have never tried it, but I think it would be better. At seconds thought I only have 401 values to choose from since the TOP value is 400. So 128 not too bad.
    I just had a look at the user guide for the motor controller and it seems like you can just put in a pwm signal to one of the analog input: It has a 10bit ADC which will take care of the conversion. So I think you should try that instead, so you will get up to 10bit resolution on the input of the motor driver.

    Okay. Just tell me when you have uploaded the code and I will have a look πŸ™‚

  150. Ryan
    August 7th, 2012 at 17:50 | #150

    I can actually get up to 16bits if I pull the processor module off the TREX and interface with the ST driver my self but I’m also thinking what I have should be enough. Those motors aren’t very high quality.

    Here is my code:

    The bulk of that starts on line 322. Most of that file is used to set up the RTOS and my touch screen which isn’t important right now.

    The rest of the important stuff is further up in the repo in kalman.c, adxl345.c, l3g4200d.c and pid.c. Ignore pid_task.c, it’s not important.

    Thanks a lot for your help. I’ve got dozens of development boards, fpga boards, XBee radios and on and on and on. Maybe I can gift you something cool for your time.

  151. August 7th, 2012 at 18:22 | #151

    What kind of motors do you use?

    It looks like you haven’t uploaded the pid.c file? I would really like to see it, since I think this is the most important one.

    I would also recommend writing or reusing my Processing program: as it will safe you a lot of time finding the PID values.

    Nice – I would love to get a nice bit of kit πŸ™‚

  152. Ryan
    August 7th, 2012 at 18:42 | #152

    Yeah I noticed I had forgot that right after the first commit but I added it about an hour ago.

    I think I am using the same motors you have. Pololu gear head motors, 350RPM, 64 counts per revolution.

  153. Ryan
    August 7th, 2012 at 19:18 | #153

    I have a few lines of code in there that allow me to send a command over the UART to change the parameters but I haven’t taken the time to write something to actually send the commands. Right now I’m using RealTerm to manually edit the text string and then send the whole thing with a click. What is processing? I’ve heard that name several times since I started this en devour

  154. August 8th, 2012 at 00:48 | #154

    Okay, how big is your robot? Many people make the mistake and make it way too high.

    I just had a quick look. It seems fine, but I have one question. What is the point of this line:

    Processing is the IDE Arduino is based on, but despite that they have nothing in common. Processing is a IDE to make cross platform application in Java. See the homepage for more information:

    It really easy to use and get started with and it supports multiple platforms, so that’s why I choose to use it. I would recommend using a application like I that, it will save you a lot of time!

  155. Ryan
    August 8th, 2012 at 01:44 | #155

    Yeah I checked out Processing. Its similarities to Arduino turned me off initially. I was playing with AVR’s years before Arduino came along so its been hard to get rid of my prejudices. The libraries are getting pretty impressive though and I routinely steal code but nothing beats Emacs, Makefiles and a through understanding of the registers. But I kind of figured Arduino was based upon it and not the other way around due to the PDE extension so I’m cutting it some slack. I still plan to use C# or Python instead of Processing though πŸ˜›

    My robot is only about 10-12″ high, I haven’t measured it. Google Traxster II and you should see an image of my platform without having to click another link. I just moved the IMU down along the axis so I’m going to try it now. Fingers crossed!

    So line 35… The integrator bothered me before I even started. When you are trying to set a desired RPM with a PID controller the integrator makes sense. If your error is not yet zero keep adding more “oomph” and when we get there we will stop adding to the integrator but what’s there needs to persist. This is different to me. As soon as we are balancing we need the integrator to drop out, along with everything else right? If the bot is perfectly balanced we shouldn’t need any steady state signal to the motors, they should be off. If you are 20 degrees off center that integrator is going to blow up fast, or at least hit your predetermined rail. How do you get rid of it when you are back to zero? So what you see on line 35 is part of my solution. Only add up the past ERROR_ARRAY_SIZE errors. So I have a ring buffer in which I add each new error and subtract the oldest error. The integrator drops out as soon as I get ERROR_ARRAY_SIZE 0 errors.

    You done much with the Basys board? I used the ADC Pmod to make a 2 channel oscilloscope with it when I was in college. One of my favorite projects ever! I upgraded to a Nexys but my old boss lost it. Now I have an Atlys but I haven’t found time to play with it. This Stellaris chip has sucked me in. It’s incredible.

  156. Ryan
    August 8th, 2012 at 13:35 | #156

    I added a few photos to the repo

  157. Ryan
    August 9th, 2012 at 20:53 | #157

    I found a student willing to machine parts for me for cheap. I’m having the same problems you did with my aluminum Pololu hubs. How serious were you in your video when you mentioned having new ones machined. Maybe I could get several made if you are interested.

  158. August 9th, 2012 at 21:04 | #158

    I’m also starting to use the registers more and more. But it’s a great platform if you want something out to many people. Like the USB Host library, which I have contributed to:

    And regarding your iTerm in your PID controller. It seems alright, but I think that it’s not necessary if you tune the parameters right.

    I have never tried the Basys board, but I would like to try it, if you recommend it!

    I have seen the photos and the hardware seems fine. But I’m still a bit worried about the symmetric aspect. Can you make it balance just turned off on a carpet or something like that?

    I would be very interrested in some reinforced hubs! What is material is he going to use?

  159. Claudio Donaté
    August 28th, 2012 at 15:18 | #159


    Hello Lauszus! I’m here to bother you again.

    I’m having quite a hard time finding the correct values for the kalman filter. Was it hard for you to?
    I do not know if I’m being to hard on the filter and it won’t get any better than it already is, but I thing, when I vibrate the IMU the Kalman filter let’s to much noise through.

    If I limit the noise it gets very laggy and have a huge overshot.

    Can you tell me how did you manage to tune yours? And how did you decide that it was good enough.

    If it would help I can post pics from the output of my filter.

    I’m using the BMA180 accelerometer with 1.5g resolution and the ITG3200 with 250 deg/sec.


  160. September 2nd, 2012 at 18:52 | #160

    @Claudio Donaté
    I never tried to tune the filter myself. I just use the standard values.
    I’m currently writing a blogpost, it will be finished this week, so stay tuned and let me know if that help you solve your problem πŸ˜‰

  161. Ryan
    September 8th, 2012 at 20:56 | #161

    Sorry, I got distracted for a while. I didn’t realize this wasn’t solely your blog. There is at least one tutorial on the Basys on here and I assumed it was yours. Yes I can get it to balance. My robot still has a CG, as does every other object in the universe with mass, and as long as the CG is over the wheels, my robot balances. I can actually get it to balance indefinitely without power. It’s just not volumetrically symmetric about the CG.

    I pretty much gave up on the balancing aspect for a while and put all my efforts into a C# HMI but now that I can adjust what ever I want from the HMI and watch an artificial horizon and log all the parameters, it’s back to trying to balance. It’s driving me nuts. I just can’t figure out where I’m going wrong.

  162. September 9th, 2012 at 13:53 | #162

    Hmm, I don’t think the asymmetric aspect of your robot is the problem, as it could easily be solved by adjusting the target angle.

    I think it’s a very good idea to have a good HMI, as I also mentioned in the video.
    Are you only trying to balance using the angle or also the encoders? I would suggest just trying to balance using the angle and then implement the encoders when you are satisfied with the balancing of the robot.

  163. Ryan
    September 9th, 2012 at 22:27 | #163

    I have a good HMI that does everything I need to do. My solution to symmetry is the one you mentioned. I balance the robot by hand and then set that as my target angle. What were your final P.I.D. parameters? I don’t see those in your code. Did you try different loop times? You use a fixed loop time of 10ms right? I just let the thing run wide open and feed the Kalman filter and PID controller the previous loop time. My UART handler blocks so the whole loop takes about 1.8ms. I tried 10ms wondering if slowing it down would help but it didn’t.

    No I’m not using the encoders. I did try to use the encoders with a 2nd and 3rd PID controller so I could set the motors more accurately but it got too complicated. The lash is a pain in the ass though. Sometimes one motor has already gone through an entire revolution before the other one starts up.

  164. September 9th, 2012 at 22:40 | #164

    My PID value can be found in the header file.

    The reason why I wanted to use a time fixed loop was because you can’t use too small a interval if you want to read the encoders, as the reading will be too noisy – but this can of course be solved by a timer running every 100ms (I think I will do like that in the next version), so the PID and Kalman filter can run as fast as possible – but you still don’t want to exceed the maximum update rate of your IMU.

    I think that is the right approach, you can always implement it when it’s balancing.
    Hmm, what motors are you using? I have never experienced a problem like that.

  165. Jetguy
    September 10th, 2012 at 14:43 | #165

    I’m having a bit of trouble with the mbed code. Since the 6DOF analog IMU is no longer available, I am attempting to use the 5DOF. The only real difference in the boards is the gyro (they both used the same accelerometer ADXL335). The original gyro in the code is 3.33mv sensitivity and the new gyro (IDG500) is 2mv.
    // (gyroAdc-gyroZero)/Sensitivity (In quids) – Sensitivity = 0.00333/3.3=0.001009091
    adjusted for new gyro IDG500
    // (gyroAdc-gyroZero)/Sensitivity (In quids) – Sensitivity = 0.00200/3.3=0.000606061
    Obviously, that number goes into the next line actually doing the calc.
    So here is the other problem. The motors run constantly once the program boots. Turning on debug statements doesn’t tell me anything good: It always says the angles are 180 no matter what. No gyro plugged in, all 4 analog input grounded gives the same 180 and pulling the pins high (3.3V) as well. Using a sample analog read program, I can see the values change so the analog is working. What bothers me here is that the motors run, even when clearly they shouldn’t since the bot should assume laying down. I’ve tried every possible combination of flipping the IMU board and every time always 180 for both values. Clearly there is something very wrong as even with the battery voltage read not connected, it should be assuming low battery and not working either. Other obvious gotchas would be motors wired incorrect polarity and thus the encoders would read the wrong way (seen this in other balance bot codes).
    I want to thank you for the code which is well commented and the pins are very clear, but I tried to use identical hardware (motors, wheels, motor controller, battery, mbed, etc) and something is very, very wrong. I also get the impression the software is not making it through the loop as I never get to any of the other debug statements. Again, it’s seemed obvious to me, the battery read could be a stop, so I commented out those lines(also tested by putting 3.3V to pin15), but it’s driving me insane.
    One more question. How exactly do I connect the USB debug port? I’m not using a shield but assume that’s the USB host pins, so how does that become a serial connection (115200) at the PC? (I didn’t plug anything in not knowing) I was using a FTDI cable to read the xbee output at P13, P14 and just changed any debug line to xbee.

    Sorry if these are amateur questions, I’m new to mbed, so I assume this is likely a wiring or other gotcha issue. I’ve got more than 20hours trying to troubshoot this on my own, and just cannot figure this out. Problem one is that the motors should not run at all if pin15 is not in the right range. Problem2 is that I shouldn’t always report 180. Problem 3 is I should be reaching other debug statements? Problem 4 is I should be able to debug the angle calc but never get there?
    Sorry, just frustrated since I at least tried to use the exact same setup as you have documented. I know the IMU is working as I can read the pins with a DMM, everything is wired per the pinouts. The motors don’t spin until your code is loaded.

  166. September 10th, 2012 at 14:59 | #166

    I think you need to implement it one step at a time – this will make it much easier for you.

    First of all try to see if you can calculate the angle correctly using the IMU. My guide at the Arduino forum will be a huge help for you:,58048.0.html. Also take a look at the code I provided:

    To see the serial debug messages simply plug in the usb cable to the mbed (the same port you use to program the device), then open up a serial monitor at 115200 baud and then you will be able to see the messages. See this page for more information:
    The thing is that the mbed shows up both as a MSD and a serial port and this port can be used as well, see:

  167. Jetguy
    September 10th, 2012 at 19:53 | #167

    Thanks for those tips. I have read those Arduino pages and read through them but was having trouble trying to reverse engineer the mbed code. Point being, the 6DOF is no longer available and I’ve tried searching high and low for the LPR530AL gyro in either a carrier board or as part of another IMU. The 5DOF uses the same accelerometer so this should be trivial to fix the rate and have it work—–key word is should. My concern was the other issue that the motors are spinning full on even when it’s assumed laying down. I see the bounding limit for un-recoverable position, so why would it command the motors to spin regardless of IMU orientation? Even wrong with wrong rates, it should stop at some point and/or reverse direction as the IMU is moved.

    Anyway, thanks for the links and I’ll keep messing with this. There is some tiny detail somewhere in the code or wiring causing this issue, and being that the wiring is very straightforward, no good reason why the same hardware shouldn’t at least try to act right. Even stupid stuff at the IMU like wrong axis wiring or cross correlation has been checked. Steady state, the IMU should read the right angle even if wrong and stop the motors. And yes, the motor encoders are working too. X-bot v2 Arduino firmware does work on an Uno and the same hardware. Just want this thing remote controlled without having to reinvent the code. It’s just driving me crazy that I went to the trouble to get an mbed, you have good straightforward code and a well documented project, and I still have a very non-working situation after hours of messing with this. I’m not even talkign about balancing or remote control, just basic motor on off. Unlikely and I haven’t checked, is there any difference in the code on github for mbed VS the code in the mbed site?

  168. September 10th, 2012 at 20:16 | #168

    I doesn’t matter if you use a 6DOF or a 5DOF, as long as you modify the code. I would take one step at a time – start with trying to calculate the correct angle using the links I provided in my last reply.

    I really can’t say what is causing your motor problem. There can be thousands of different explanations.

    But you should really have a look at the newest version of the code written for the Atmega328:, as it futures some big improvements.
    It should be pretty easy to port to the mbed.

    You should get the code from github, as it will be the newest version, but if you have a Arduino then I recommend trying the Arduino version of the code: which is the one I currently use.

  169. Jetguy
    September 10th, 2012 at 21:03 | #169

    One thing I dislike about the Arduino code is no way to debug. All the pins are used including Serial (0,1). It doesn’t work for me yet so no way to troubleshoot without ripping out motor control. I was thinking of rewriting the direction control portion to only use 2 pins (A4, A5) or 0,1 writing them high or low and use a hex inverter to invert a second output for the A,B of the motor control. Yes, it’s one more chip, but a chance to see what’s going on under the hood is critical. I’d also like to add an LCD (serial backpack) to see under the hood so freeing up pins is a must.
    Another peeve of mine is that the TQFP 328P package VS the DIP has 2 more analogs (6,7) which would be nice in this case that aren’t normally broken out to a shield.

  170. September 10th, 2012 at 21:12 | #170

    I totally agree with you – it’s impossible to make the robot without a serial port as it will take to long time to reupload the sketch with the new PID values and so on.

    You can use a Arduino Pro mini if you want the extra pins. In fact you could also use the eight GPOUT pins on the MAX3421E if you want to. See this page for more information:

  171. Ryan
    September 10th, 2012 at 23:22 | #171

    Who made up the word “Quids?” Someone has too much time on their hands.

  172. September 10th, 2012 at 23:28 | #172

    Haha, I really don’t know and I don’t know if it’s even a correct term?
    But I have seen people use it and tend to use it myself, as people often know what the meaning of “quids” is πŸ™‚

  173. Jetguy
    September 11th, 2012 at 06:22 | #173

    Ok, so several hours of troublshooting both Mbed and Arduino versions still results in that they do not work with the current shipping 5DOF. This code does
    So now it’s a matter of merging them.

  174. September 11th, 2012 at 08:41 | #174

    Okay, good to hear that you are finally getting some progress πŸ™‚

  175. Jetguy
    September 11th, 2012 at 13:14 | #175

    I don’t know if I call ripping my hair out progress? I’ve come to one of 2 conclusions. Sparkfun says the current 5DOF uses a 2.8 Volt regulator instead of direct 3.3V. The chips data sheets say 3.3-2.7 volts, and say that voltage shouldn’t affect the reading. I’m using a TO220 LDV33 3.3 Volt regulator, properly filtered for analog reference. I stand behind the idea that in theory, I should be able to change the sensitivity # in your code to adjust for the gyro and it *should* work. The other thing I have seen and suspect. On the 5DOF the x and Y axis are swapped between gyro and accelerometer, where on your older 6DOF, they line up because I see this in the pin comments for the Mbed since you read all 3 axis of the accelerometer and a single axis of the gyro. Obviously, in my testing, I’m smart enough to swap the axis of the gyro to use X instead of Y. I don’t think that in itself is the problem (but a good note for someone else trying this). I think though it could be a sign issue. In other words, maybe the 6 DOF sensor both read up in one direction and down in the other or maybe are inverted, and this new 5 DOF is opposite correlation. Thus it could be a sign switch (multiply gyro axis by -1)? Where is the best place to test this? In the line where the sensitivity number resides?

  176. September 11th, 2012 at 19:48 | #176

    What I do to know that the orientation and sign of the gyro and accelerometer reading lign up is by outputting the unfiltered gyroangle, see:

    Just one question: are you using the 4.5 output or the normal output from the gyro?

  177. Jetguy
    September 11th, 2012 at 20:58 | #177

    I’m using normal output (2.0mV/°/s)
    X-/Y-Out Pins: 500°/s full scale range
    2.0mV/°/s sensitivity
    X/Y4.5Out Pins: 110°/s full scale range
    9.1mV/°/s sensitivity

    This comment here about the 6DOF you used SEN10010: (from Sparkfun Comments)
    “How come this board doesnΓ’??t align the x and y axis on each sensor? From the readings, you could program the micro controller accordingly, but it seems better if the X and Y were aligned.”

    A. Babb | about a year ago 1 ItΓ’??s one of several different coordinate schemes:
    This board has the X and Y angle set up so that a positive X angle tilt will result in a positive linear acceleration (and displacement) along the X axis, and a positive Y angle tilt will result in a positive acceration along the Y axis.
    It seems like this was more intended for quadrotors and helicopters than for airplanes and other craft.

    I’ve been trying to cut down the Mbed code to just the sensor read and angle calculation to get to the bottom of whats going on with this sensor. Since the Kasbot V2 and V1 code was designed around the 5DOF, it works great out of the box. I took last night and today and began comparison line by line between working kasbot code sensor read and kalman and your code (Arduino and Mbed). I also read the latest article on kalman here (but a bit much for me to digest). Point being, I see one line difference that is present in your code and it’s just an order issue-not actual change.
    In Kasbot, “y=newangle-angle;” comes before “S=P_00 + R_angle;” but in your code, that line is further down after K_1=P_10/S;” So which is right and how does that affect the math or at all?
    Since the way you guys actually initialize and then calculate the actual final numbers are radically different, I’m having trouble trying to even become close at troubleshooting or comparing there. This is after changing the gyro sensitivity number in my first post.

    Again, I can run Kasbot only changing a few of the motor control pins around and it works, then run your code and cannot get past the laying down issue even if I spend 10 minutes of changing orientation and hitting reset. No combination ever gets any kind of decent reading for angle. Since the Arduino code uses serial, no way to debug without serious code slashing. Mbed at least has a good way to check, but trying to strip the code down and reverse engineer the readings as to what is wrong is not fruitful yet. I actually scoured the internet trying to get a 6DOF analog IMU or even just a LPR530AL breakout or even a bare chip. I guess as a last ditch effort, I can try the other analog gyro I have on hand and keep the ADXL335 (on the 5DOF). I have this LPY510AL and would have to adjust the sensitivity again and play the orientation game, but maybe that’s what it takes to match the 6DOF.

  178. September 11th, 2012 at 22:35 | #178

    I know that the they don’t align, that’s why I put a minus infront of the gyro value:

    It doesn’t make any difference if you calculate y before S. See this code:

    I really think that you need to solve your problem with your IMU first before running my code, it has clearly something to do with it.
    Please try to download the code I send you: and modify it to your needs! If you don’t try the things I suggest, it’s pretty hard for me to help you out.
    You don’t need to buy a new IMU, the one you have is good enough.

  179. Jetguy
    September 12th, 2012 at 04:30 | #179

    Ok, so running the program and adjusting the lines for x and y sensitivity:
    Stock: Sensitivity – in quids – Sensitivity = 0.00333/3.3*1023=1.0323
    5DOF: Sensitivity – in quids -0.00200/3.3=0.000606061*1023=0.620000403

    double gyroXrate = -((analogRead(gX)-zeroValue[0])/0.6200);
    gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter

    double gyroYrate = -((analogRead(gY)-zeroValue[1])/0.6200);
    gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

    Here’s what I found:
    The x axis (blue) line for gyro-x, drops like a rock to the bottom of the screen.
    The other blue line for the y axis responds fine and follows red, blue green and yellow as I move in Y. So, your thinking bad gyro output, but no, checking with a DMM, verify wiring order etc, both x and y gyro read about 1.34 volts steady at level and then swing when the gyro is moved in the corresponding axis. So then I think, maybe it’s the Arduino and I have several and swap-same results. Then I swap X and Y wiring and now the we read it and of course it still graphs the same, but moving the gyro in X shows up on the screen Y and correlaes so the hardware is checking out fine. I have to go into the example and figure out what exactly is different for X gyro in the code. This is similiar response I am seeing in the Mbed and Arduino balance sketches.Hopefully this will get me closer as it appears to be something in the x code.

  180. Jetguy
    September 12th, 2012 at 05:36 | #180

    Further swapping and messing around. I swaped the unused A2 from Z to the X gro in the code to make sure it wasn’t something with the A0. No change, then I said Ok, unplug Y so we see the blue line for X and connecting to either pin of the gyro does work, it’s when both pins are connected, then I get that X always drops off the screen but Y axis graphs and then of correlations work as well.
    Final tests, if I leave one of the gyro axis unplugged, that axis will graph but if both gyro axis are plugged in, Y always graphs and X never graphs??? In other words, I can get X to graph all three lines exactly, only if Y gyro is left unconnected. The meter tells me the sensor is putting out the changes-it’s not being read. I have 2 of these 5DOF IMUS so it’s not a hardware issue. I don’t think the graphing is off because the yellow and red lines exactly match on the y axis indicating sensor fusion is happening but X is off (no blue line and red, yellow do not exactly track). Again, unplugging Y, hit reset and x graphs correctly, so i can only assume, since i see the blue line go down after reset and then x basically is not seeing the gyro, it’s something in the initial calibration? It’s just odd as reading the code, I don’t see anything different? Further, it works if when one axis is disconnected ???

  181. Jetguy
    September 12th, 2012 at 06:49 | #181

    Ok, tested the LPY510Al and got the same thing. It seems something is crossed in the code if both gyro axis are plugged in at the same time. One reads, the other doesn’t. They both will read individually (so basically both sides or channels work), but combined, it’s one axis or the other.

  182. September 13th, 2012 at 07:43 | #182

    Nice job Lauszus. I don’t doubt that bigger wheels made your robot better, but it actually provides less torque and more speed. Since you have a bigger wheel the perimeter is larger, so one full rotation would produce further travel. You trade in torque for speed, that’s the way I like to see it at least :p.

    Regarding the use of a PS3 controller, what requirements do i need on my bluetooth module? Can I buy a bluesmirf, as those in sparkfun and use one of those?

    Thank you, keep up the good work.

  183. September 13th, 2012 at 10:24 | #183

    You forget that the BaneBot wheels has a significant more mass, so they will differently provide more torque to the robot.

    You can’t use a bluesmirf module, it’s only for serial communication. You will need a proper Bluetooth dongle, the good thing is that you can get them for something like 1$ on ebay. See this list for more information:

  184. Jetguy
    September 13th, 2012 at 19:05 | #184

    Ok, So some progress.
    The test code did show me that it does work at least in one axis. I still need to fix either the Mbed or Arduino balance code, but shows the theory that changing the sensitivity (something I’ve already done) should have been the only change required to the balance bot code and have a working bot. Being the Arduino has no way to debug (easily) I’m working with the Mbed since this balance bot project is the entire reason I bought the Mbed in the first place.
    Anyway, that said, page 1 of the comments and stripping the Mbed code to only the sensor read and angle calculations has at least led to some progess. Of note:
    “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?” In the response to the question from Kas (great respect for everyone’s projects and documentation).
    This clue helped me to understand some basic functions that aren’t obvious VS some of the other balance bot codes out there. Your code assumes the IMU board is at 90 to the surface when the bot is laying down and then either changes the reading to be either 180 or 0 so that when the IMU is then parallel to the ground, it reads 90. Most code I’ve touched or used including the Kas codes (X-bot) assume standing up at boot (IMU flat) or reset. Again, page one comments (not normaly seen by default unless you navigate to them) helped me understand what your code is looking for. Next, I see the comments about advanced laying down code in the Arduino version (Github comments on the commit) and if both sensitivity is off and you don’t have it in position to calibrate, you’ll never get anywhere since there is no serial debug. Again, after reading the comments from Kas, I’m not the only person to have issues trying to port this to another sensor since the 6DOF analog (and the LPR530AL gyro) are no longer made or available but those questions and answers helped put me on the right track.

    Maybe putting them in the documentation on the page would help rather than them just being in the comments? I’m just saying the usage instructions in that the bot assumes laying down to calculate 180 could be a very specific mechanical design feature as well when people ad sponges or other soft material at the top of the bot during initial balance testing, this would move the 90 assumed balance point because the bot didn’t start in the correct position, calibration is off.
    I want to thank you again for your code, documentation, help, and the project in general by helping a novice like me get past these little gotchas. I still don’t yet have it working, but at least know what I’m looking for and have a path forward of testing and calibration.

    Also, one of my earlier comments that the motors kept running-now I know why, I removed the trip points for over anfgle recovery, but because calibration was so far off (bot was standing vertical at boot) it always thought it was nearly 180 or 0 and thus so far out, a full tilt of the sensor never brought it into a position that could even be sensed as 90 or less causing a stop or reversal of the motors. It seems to me your intended start directions should be, lay bot down, plug in battery or hit reset, then stand vertical. I also asume since the bot may not balance exactly at 90 (hardware dependent) the rest angle is what you adjust in the code? It’s those little assumptions that have been causing me the trouble, so I’m stating what might be painfully obvious to others in hopes that somebody doesn’t pull their hair out.
    Again, thanks for the help and the hard work that went into this project.

  185. Jetguy
    September 13th, 2012 at 22:15 | #185

    Additional notes on switching the code to 5DOF SEN-11072 and (Mbed code):
    The IDG500 has a sensitivity (rated) 2mv/degree/sec
    double gyroRate = -(( – zeroValues[0]) / 0.000606061);
    What, I’m seeing is very specific to mounting so the IMU should be ICs up, pins towards the back of the bot and mounting holes forward.
    I also get (on mine YMMV) that calibration at laying down(board is 90 to earth, pins down, chips twards the top of the bot) and that moving to dead flat chips up should be 90 but ends up as around 100 or so degrees. Also, going too far forward (pins up) takes it past what should be 0 degrees and thus the angle actually goes back up. This would indicate to me that the sensitivity is slightly off, coupled with the fact the Mbed code only covers 180 degrees (I think I saw in comments Arduino code has 360 degrees patched). It shouldn’t be too hard to get this sorted, but it was a large learning curve to figure out exactly what was going on. Also wondering if I get better accuracy going to 4.5x gyro ouput and adjusting the sensitivity to match?

  186. September 13th, 2012 at 22:22 | #186

    The IC of the IMU should be face the ground and yes you will get better accuracy by using the 4.5x output.

  187. hector_ka
    September 15th, 2012 at 06:02 | #187
  188. Ryan
    September 15th, 2012 at 23:01 | #188

    I’m at a real loss here. I don’t get it. I don’t know if I just haven’t found the right parameters yet or I’m doing something fundamentally wrong. If I were to balance it by hand on the carpet and then let go so it can slowly fall over, it’s like it doesn’t even try to balance. It just falls over. It should be so much more snappy than that. But at the same time, the controller is hardly stable. If I hold on to it and allow it to do what it wants the oscillations tend to blow up. The D term is either not strong enough to damp them or it’s unstable but there is like no in between. I can’t believe it’s just a matter of more tuning. I’m doing something wrong but I don’t know what it is.

    You have Pololu 350RPM motors don’t you? I can’t figure out why the motors don’t start at the same time either. I guess I need to put my scope on those when I get home and see what’s up. I figured it was just because they were cheap motors but if you don’t have a problem with them then it’s just another thing I’m doing wrong.

    Any other tips? Maybe I can take a video and see if that will help.

    Quids is definitely not an engineering term by the way. You had a question mark on your statement so I figured I would reply πŸ˜‰

  189. Jetguy
    September 16th, 2012 at 15:31 | #189


    I know it doesn’t help but this project is kicking my butt too. I tried to go back and read your comments to figure out what IMU you are using Ryan. Have you put your code up someplace?
    Here’s my frustration, I bought exactly the same hardware as Lauszus has documented except the only thing is the gyro LR530AL is no longer made or available anywhere ( I even begged Sparkfun who wouldn’t sell me theirs for any price as it’s soldered into a project). So that said, the obvious thing is to simply adjust the setting in the code for the gyro rate on the new 5DOF analog and it should work in under 5 minutes but neither the Mbed, nor the Arduino code works for me. I mean I’ve watched the videos of Lauszus driving around hundreds of times so in theory, he posted running code, but after days and weeks now of troubleshooting line by line, I don’t yet have a bot which even attempts balance. Latest debugging of both sets of code, I am now reading correct values from the IMU but now, the motors are not integrating the encoders and only seem to stick full on. I’ve even displayed the velocity count which is going up or down based on motor direction, verfied the wiring based on the pictures (a painful tracing process across the pictures and all those jumpers). I’ve done the motor polarity swaps to make sure it’s not that, and even crossed left/right controls and encoders to make sure that somehow the pin comments are right. I even see the PS3 commands come across (print input), but motor control is nearly non-existant. Basically, all I can do is get the motors to reverse direction if I swing to nearly laying down in either direction but I’m expecting them to stop when both the pitch and acceleromter values are reading 90 and now I can see that the IMU is reading 90. What really bothers me now is that debugging, the major functions are working (IMU read, angle calcs, encoder counts, even PS3 commands) but no way can I even set this thing down. In fact, once I got past the IMU working and the fact the bot must be laying down on reset, this should be working. I used MS word to compare the github code vs the code on the embed site and it’s exact line for line. Maybe I have the wrong expectation but if the exact motor, encoder, speed control and wiring is used and then the IMU calc is verified to read correct angles this thing should at least proprtionally control the motor speed and direction based on error angle but that’s not what it doing. Thus the only conclusion one can make is that the posted code is not working code or even though the IMU and angles calcs are working, there is something funky with the IMU.. Again, basics, the only real error could be motor direction vs encoder direction and as stated, I’ve checked this every possible way. I’m trying not to be a jerk as I am happy for all the help thus far, but am interested in any other code where someone has adapted this to another IMU and actually got it to work. I think the worst part is, I’ve tried other code bases from Kas that did work that don’t have remote control. Lauszus swears his code works and I want to believe it, but the same hardware, verfied wiring, and then even debugging and it’s clearly not even trying to run the motor proportional to angle error. I’ll try and post the code and a video showing both the debug results and the resulting motor actions, but since both branches do it, this seems to be a fundamental error. If it’s wiring I don’t see how. I’m basically begging for help or just a verfication that either there is some trick I’ve missed, or that someone else got this to work. It’s really anoying to bench test this and troubleshoot when it should be plug and run.I’ll post a youtube to maybe help point me in the right direction. Again thanks for the help and sorry that I’m doubting, I just don’t get it at this point why it’s not working.

  190. September 16th, 2012 at 17:04 | #190

    If you have all the wiring correctly, then it’s just a matter of tuning the parameters.
    It sounds a bit weird that the motors don’t start at the same time, what motors are you using?
    Yes I’m using the motors from Pololu with a 350rpm, as mentioned in the blog post.
    I would really like to see a video of your robot, so I can see what might be wrong.

    Are you seriously claiming that I would post non-working code? That is just ridiculous, it would make any sense to spend so much time on making the video, doing the 3D model and so on, if I would just post non-working code.
    I think a video would be a good idea too.

  191. Jetguy
    September 16th, 2012 at 20:20 | #191

    I’m sorry and I was tired. I’ll admit it when I made a mistake. In order to free up pins on the Arduino code in order to have a serial debug, I used a Hex inverter, swapped a few lines in the code so that I only used A4 and A5 to write high or low for direction change, but I now see you write both pins high or low to stop in the normal code, thus my hack would never work. I’ve gone back to the original arduino sketch after having at least verified from the debug drill the gyro and accelerometer both give proper readings. I get 180 when laying on one side, 90 vertical and 0 laying down the other way. This part is consistent and reliable. Heck, even if I don’t change the sensitivity, it just means the angle is off and it takes more tilt of the sensor to reach the magic 90. Also, I found that in the box stock Arduino code downloaded from github as a zip, the bluetooth initialization string would not work with the latest version of the library. To fix it, I just copied the string from the PS3 example in the new library.
    From github Arduino balance bot cpp (your original posted code)
    PS3BT PS3(&Usb,0x00,0x15,0x83,0x3D,0x0A,0x57); // Also remember to uncomment DEBUG in “PS3BT.cpp” to save space
    Edited line for my specific dongle
    PS3BT PS3(&Btd,0x0E,0xFD,0x59,0x15,0x19,0x0E); //white kensington
    Notice the library is expecting “&Btd” rather than “&Usb”
    That’s problem #1 to just get the code to compile, and yes, a minor change and something the user has to set up for their dongle anyway. Again, not that big of a deal but was my first trip up.
    And finally, after much, much experimentation, I’ve found the error. It’s simple and something I had tested, but not understanding exactly how your code deals with it a major problem. Only by isolating and driving one motor and encoder at a time, was I able to determine the issue. Since the motors face opposite directions, both the motor and the encoder encoder are wired reverse polarity for one side but thats not listed in the pin comments and since there is no wiring diagram, no clue to anyone this might be an issue. Again, I want to state I knew this could be a problem but way harder to drill down than I previously thought. The why behind that is that the code is using both encoders and when one side is wrong or both sides are wrong, the error condition is that the motors seem out of control.
    Basically, the gotcha is the encoder pins really need labeled better as this is the root cause.
    When the code says this:
    /* Encoders */
    #define leftEncoder1 2
    #define leftEncoder2 4
    #define rightEncoder1 3
    #define rightEncoder2 5
    A person generally assumes (being that you and several others are sharing some code and wiring) Yellow is the assumed first pin and white the second pin. basically I assumed from the above, pin 2 and 3 are yellow and 4 and 5 are white, but in reality even with other quadrature encoders you MUST reverse one side. Then you further must reverse the motor wiring on the same side the encoder is reversed. What makes this dificult to test is the fact there is no debug to serial print anything, further, the fact the system also wants to return the bot to not just balance, but same count so it hold physical position makes testing the combinations nearly impossible. Only by isolating to one motor, then reversing the motor leads until I could get it to show fine control back and forth again due to the count and position stabilty code interacting. I then isolated the second motor and got it’s combination right and then, with fingers crossed, tested both motors and now have the correct wiring.

    I’m just asking, please update the comments in the code to reflect what really is the correct wiring, at least for the encoders as this was the root cause of my problem.
    Maybe just change the order which would also clue somebody in on the switch:

    /* Encoders */
    #define leftEncoder1 2 // left encoder Yellow
    #define leftEncoder2 4 // left encoder White
    #define rightEncoder2 5 // Right encoder Yellow
    #define rightEncoder1 3 // right encoder White

    Those words might have saved me hours and someone else is sure to get tripped up on this. I expected to have to change the motor wiring so that from the driver standpoint, one motor is opposite, but the encoder also being backwards but not labeled makes troubleshooting a major pain. There are 16 different combinations, but also, a very narrow window of oportunity after pressing reset to be actually able to find the right one, because the longer it’s not at 90 and the mtoors and encoders count up, it won’t react as you are expecting. Only by isolating and testing can you ensure you have this correct.

    The flipside is, I now see how good the code works, it’s just getting there without a wiring diagram. Sorry for all my comments but I hope this single one helps somebody else.
    I owe you a sincere thanks.

  192. September 16th, 2012 at 22:07 | #192

    Thanks for your feedback. I’m not at home at the moment, but I will add comments about the color of the wires to the code when I get home πŸ™‚

  193. Jetguy
    September 16th, 2012 at 22:46 | #193

    Ok, so I will post a video soon but back to calibration, I’ve found another question area in the CPP file:
    if(zeroValues[1] > 500) { // Check which side is lying down – 1g is equal to 0.33V or 102.3 quids (0.33/3.3*1023=102.3)
    zeroValues[1] -= 102.3; // -1g when lying at one of the sides
    angle = 90;
    // It starts at 90 degress and 270 when facing the other way }
    else { zeroValues[1] += 102.3; // +1g when lying at the other side
    angle = 270;

    This key part of the code sets the initial calibration and thus the actual balance position (we assume 90) but in order for 90 to actually be 90 this must be right. The original sensor on your 6 DOF is the ADXL335 and that’s the exact same sensor on the 5DOf (ha ha Sparkfun just discontiued it too).

    Anyway, how is the best way to go about modifying it to reach a balance point from the start point? Should I modify target angle in balancingrobot.h ?
    const double targetAngle = 180;

    or is it smarter to modify
    1g is equal to 0.33V or 102.3 quids (0.33/3.3*1023=102.3) (specifcially 102.3) in the appropriate blocks above in the CPP?
    I’m thinking fixing CPP is better because much of your code is based on the actual sensed angle being 100% correct (steering and start/stops are critical in my mind). Just trying to figure out a strategy since there is not an easy way with the Arduino code to determine what that start value should be? The bot is balancing leaning forward maybe just a few degrees, but even putting the bot so it leans further past flat on reset and then raising doesn’t fix it from a purely mechanical standpoint. Again just curious what is the best way to get this balance point correct?

  194. Ryan
    September 17th, 2012 at 00:20 | #194

    You are crazy if you think I’m going to read all that… You lost me at “Here’s my frustration”

    But if you’re that curious about my IMU… I used something I found on eBay for $10 consisting of an adxl345 and an l3g4200d along with a barometric pressure sensor and a magnetometer.

    You shouldn’t have any trouble getting any IMU to work as long as you have at least a single axis gyro and a 2 axis accelerometer. Get your sensors talking first. Just read the raw data and spit it out the UART. Once you are happy with the data plug it into your fusion algorithm of choice. Work your way from the bottom up.

    Maybe Lauszus meant for his code to be plug and play, to have people treat this as an afternoon project, but I doubt it… If you don’t like the way he did something, do it differently.

  195. September 17th, 2012 at 00:39 | #195

    The reason why the angle is set at the startup is so the Kalman filter don’t have to spend time calculating the angle from for instance 0. This will speed up the process if you need correct angle short after startup as with a balancing robot.
    The thing is that my angle at rest is 180 degrees, so the angle is 180±90 when it’s laying down. That’s why I set the angle like so.
    Remember that you are actually not finding the zero value for the y-axis (or x-axis, it depends on how the IMU is orientated), but the ±1g value, so to compensate for that i add or subtract 1g (equal to 102.3 in my case) from the value.
    What you need to do is only change this value, so it represent the sensitivity of your accelerometer.

    That’s what I have been trying to say from the beginning. Make small steps – first get your IMU working, then the motors, then encoders etc. It was never meant to be plug and play.

  196. Ryan
    September 17th, 2012 at 01:50 | #196

    Yeah I know, he doesn’t seem to be hearing it though. I figured I would back you up.

    I just made a video and I’m uploading it to Youtube now. I’ll post a link in a few minutes. I currently start up with kP=kI=kD=0 and then tune from there because I have never come close to anything I thought was… close… Eventually I will write them to ROM or EEPROM as I tune but that’s another story. Point is this video is not the results of hours of tuning but this is still about what it looks like after hours of tuning. Your comments will almost certainly include “you need to adjust your parameters” but I’m hoping you can see something a little more fundamental. For instance, I had the sign wrong on the D term for days and couldn’t see it. I’m hoping something like that will be somewhat obvious to you.

    I can also log the angle, PWM duty cyle and P, I and D term contributions to a CSV file if you would be willing to look at that. I made some Excel plots but could never find anything obvious. I would be willing to send you an XBee or an IMU or something else I have laying around in exchange for that much of your time. Just a glance is appreciated as well though.

    Looks like my video is done…

  197. Ryan
    September 17th, 2012 at 02:01 | #197

    I have the same motors as you do BTW. Pololu, 350RPM with 64 pulses per revolution of the input shaft. The code I uploaded a few weeks ago is pretty much what I am running today. Except I hacked up that Trex motor controller so I am just PWM those ST H-bridges my self. This gives me 16bit resolution as opposed to the 7 I was getting before.

  198. Jetguy
    September 17th, 2012 at 05:17 | #198

    Well, unlike you, I’m actually finding a lot of good notes in other posts so maybe reading them and mine might help your problem. I don’t really know how else yo take your comment earlier towards me, I’m actually trying to help.

    I watched your video and think you have the motors and encoders reversed. In other words, the if you lean forward, the motor should move forward and rol the bot straight then stop as the unit becomes level. The other easy way is to to rotate the IMU board 180 around the Z axis with the same net effect. Whatch your video and see that you induce some lean, the reaction seems to be opposite of balance, and then if you are using the same motor control code, the stay in the same place code is oscilating larger each swing indicating you are inserting positive feedback into the loop. Positive feedback increases until the sytem hits the limits and is the root cause of my problem when the encoder wiring was wrong. Again the primary fault seems to be direction is reversed and encoder direction might be a second issue but is normally more pronounced in my experience.
    I would trouble shoot one wheel at a time and you should be able to resolve the issue.

  199. Ryan
    September 17th, 2012 at 13:23 | #199

    You could not be more wrong. You’re telling me to check things I’m not even using.

    Please don’t read my posts or watch my videos or waste my time with your inaccurate feedback. I will grant you the same respect.

  200. Ryan
    September 17th, 2012 at 15:23 | #200

    Sorry. Its early here. I shouldn’t take such offense to your comments. I do though and I don’t think I’m the first person you have insulted.

    If you hold your robot out straight in front of you and lean it 30 degrees to the right, it’s very easy to figure out the wheels need to turn clockwise in order to drive the wheels back under the top of the robot. For you to try to explain that to me is insulting. Give a 5 year old a broom and tell him to balance it on his hand. I promise you he gets that without even thinking about it. I get that as well and it was the very first thing I did. I didn’t expect it to balance out of the box and then work my way back. I started by calculating the angle correctly and then I made sure the wheels turned in the correct direction to maintain that angle. If I got that backwards it wouldn’t oscillate. As soon as I leaned it the slightest bit in one direction it would take off in the other direction and never oscillate. The oscillations you see are the result of an underdamped system, not positive feedback.

    Again… sorry for the sharp tongue but what you wrote is wrong and insulting.

  201. Jetguy
    September 17th, 2012 at 19:19 | #201

    Please accept my appology, I was honestly trying to help.

  202. Ryan
    September 17th, 2012 at 19:35 | #202

    I think I need anger management. I gave an old man the finger yesterday when he wouldn’t let me ride a roller coaster because my arm is in a cast. You can comment on my comments all you want and I’ll pay a little more attention to yours as well.

  203. Jetguy
    September 17th, 2012 at 21:07 | #203

    Honestly, I think this whole balance bot stressed us both out. It’s complicated and there are many, many points of interaction between the various blocks of the system. Troubleshooting may not catch the interaction between blocks and thus the system still doesn’t work as intended. I think both of us ran into that at some point.

    Just a question but is this your code?

    I have the same gryo and accelerometer you are using ( I have a pile of IMUs and sensor breakouts) and since I figured out the Arduino code and now have a working bot (other than the dreaded aluminum hub failure from the torque).

    Point being I am willing to try out that code and promise to not bug you but maybe a second set of hands trying it might reveal something?

    As to the hub failure, I think the drill through and hardened pin is about the only fix I can come up with. (Can’t remember if I saw that fix here or someplace else). I would get some red loctite (the hardest one) and coat the shaft and the screws now before you get too far in. Red can only be removed with heat, but that’s really what we want, and we could heat the hub later if we had to remove it. I’m wishing I had done this before now. Clean the shaft and the hub with acetone and then apply the loctite and tighten and it should hold. On mine, the setscrew seems to have backed out (wasn’t using locktite) even after seriously tightening them and while I am able to tighten it back, I see why they end up stripping and then the pin is the only way to secure it after the hub is stripped. Again, others have warned, I thought I had it tight enough, but the torque is quite impressive, so much so, the aluminum gives, then it gets loose and rocks and only gets worse from there.

  204. September 17th, 2012 at 23:41 | #204

    I had several looks at your video, but it pretty hard to tell anything by the video. Could you perhaps tune the PID loop as best as you can and then upload a new video? I would maybe be able to see what’s might be wrong then.
    Also if you upload the newest version of your code I will take a deeper look.

    I just pushed a new commit to github, with comments about the wiring of the motor driver and encoders:
    But I found out that I actually mistyped the names of the encoders (!! So what’s called “leftEncoder1” is actually “rightEncoder1”. I will make a update to the code tomorrow which fixes the problem and has support for the new version of the PS3BT library as well.

    I was the one who drilled though the hub and put in a hardened pin, I mention it in the video:
    I would also wish I had done something like that in the beginning, because now it’s nearly impossible to get the hubs off the shaft as small pieces of metal is stuck between the hubs and the shaft making it stick to it like is has been superglued, but only in the direction away from the shaft – you can still rotate the hub along the shaft if the hardened pin is not there.

    Btw I’m really glad that both of you solved the problem and took your part of the responsible πŸ™‚ I will try to help both of you out as best as I can!

  205. Ryan
    September 18th, 2012 at 01:49 | #205

    Yes that’s my code but it’s for a Stellaris LM3S9B96 Cortex M3. It would take a fair amount of work to port the whole shebang to something else. Not an impossible amount of work but probably a day or two for a programmer experienced in both architectures. You would be best to just try my P.I.D. controller. If you want to look at my code though main() starts in safertos_demo.c and the P.I.D. controller is in pid.c, not pid_task.c. Eventually I will hand this code back over to the RTOS but for simplicity I have disabled it and just run in a loop right now. That’s the reason for the naming conventions though.

    I considered Loctite but you are only delaying the inevitable. Steel is so much harder than aluminum, no amount of Loctite is going to protect it. It’s not worth hacking up my hardware only to destroy it in a month as opposed to a day. I am just going to machine my own hubs from steel when I find the motivation. I have a platform I designed in Pro/E and some mechanical engineering students at my former school willing to do the work when I’m ready to pull the trigger.

  206. Ryan
    September 18th, 2012 at 03:31 | #206

    I can’t remember how to use git. I’ll have to figure it out tomorrow. My code hasn’t much changed though. I started working on other things like a C# HMI and logging.

    How do I delete everything thats in the repo and start fresh. Nothing I can find seems to work.

  207. Ryan
    September 18th, 2012 at 04:03 | #207

    Took me far too long to figure this out…

    It will be a while before I can really tune it. I’ve been out of town on business working 70 hour weeks for almost 3 weeks now. I just so happened to throw my robot in my suitcase before I left.

  208. September 18th, 2012 at 12:52 | #208

    I really think it would be nice if BaneBots also sold steel versions of the hub.
    I use the Github for Mac ( for most things, it only rarely I use the terminal. There is a equivalent program if you are on Windows:
    Okay. Please let me know when you got time to shoot another video and I will have a look.

  209. September 18th, 2012 at 13:01 | #209

    Didn’t you forget to use “delta_t” in the Iterm: You do it in the Dterm:, so I thought it might be a mistake. See:

  210. Ryan
    September 18th, 2012 at 13:13 | #210

    No I didn’t forget, I never even considered it. You’re right though I should have.

  211. Ryan
    September 19th, 2012 at 03:08 | #211

    That made a big difference. It’s mostly stable in my hands and trying really hard to balance but if I let go its not responsive enough catch its self. It just can’t quite keep up with a fall so it shoots across the room if I let it and crashes. I’m not sure whether to fix that with P or I but neither one seems to make a difference. I’ve doubled them both and it still does that.

  212. Jetguy
    September 19th, 2012 at 04:27 | #212

    Funny, I’m at the exact same point. I started with the default values, then started bringing up P (now at 9.9 from stock 7). I only increased I to 2.4(from 2), much higher and it gets way too unstable, lower values aren’t enough to really try and balance. D term seems to have minimal effect in either direction from the stock value of 8. Anyway, really close, just need some more practice.

    Also, I kept playing with adjusting the 1 G values but that never “fixed” the angle from calibration but changing the target angle in the .H file worked better for me to calibrate the balance point from laying down. Mine is 170.87 rather than the stock 180 but other than it not being fully able to catch itself after getting some speed (I would think I term would take care of that), but one difference, and I think it is significant is that I’m using a 3 cell 5.5 Ah LiPo rated at 11.1 volts and the example system is using 12 volts NiMh which is heavier, but also nearly a volt higher. That extra voltage makes a difference to the motor response (mine would be weaker and thus the higher gains required, or at least that’s my theory) I also played with lowering the battery shelf to the 27cm spec, since I made my rods are a few centimeters longer than the example, but not much ~31cm total height. Taller seemed better and that’s what everyone says about these anyway (within reason). I might try runnning with a long wire from one of my 30 amp 12 volt supplies, leave the battery attached for weight and return to the stock PIDs and see what that does. Again, I know the system is voltage dependent (many have said this), and that could be the magic between working and not.

    Sorry, my notes may not help you much, I saw you were using NiCd or NiMh batteries that were 10 cells so 12 volts. Anyway, I hope you get it working, mine is so close but it’s just not quite self balancing.

    Also, I need to be smarter, as my Bluetooth dongle nearly got snapped off in a crash (sounds like exactly the crash you described). I’m now using a short USB extension cable to relocate it to a safe spot under the top shelf. The PS3 control is fun, again, just need to fine tune it and I would be right there, cruzing around, remote controlled.

  213. Ryan
    September 19th, 2012 at 15:09 | #213

    You realize my system is completely different than what you are working on right? Different hardware, different software, different platform. Right now stable values for kP and kI are in the 100’s on my platform.

  214. Jetguy
    September 19th, 2012 at 19:05 | #214

    Sorry, I’ll try to address any direct answers or questions only to you separate from anything else.

    I do fully understand you are testing your hardware and your code and thus the numbers will not be the same. My intent was we are both in the tuning phase. I’m hoping that tuning PIDS is more or less the same process and trying to compare some notes even though our numbers may be magnitudes away from each other. In other words, does the suggestion of doubling the value, checking the response and then halving the numbers work for you? Are you finding any trends between the numbers (P is double I or something)? Again, I know your code is different and just trying to share some lessons learned.

    Additionally, I’ve read every comment in the blog so far and I don’t think a lot of people directly tried to use the code from Lauszus (at least they didn’t post that they did) and thus when I post, I try to give relavent data about my experience with his code since this is his blog. I know my numbers don’t directly relate to yours but maybe there is a trend, maybe not.

    Rather than make two separate replies, I combined information, the fact we are both tuning and what the effects where. In my case, I think voltage does affect my PIDs but for you, maybe not since you are using 12V VS my 11.1V. That said, I think you are using AA sized batteries (based on what I could tell from the video and pics at github) and wonder how much sag if any is present in the system VS the sub C cells used by Lauszus. It may not be significant but a point worth discussing.

    Again, sorry if I confused things or upset you, I’m just trying to share.

  215. Ryan
    September 19th, 2012 at 20:24 | #215

    It’s OK. I just wanted to make sure you understood that.

    The battery thing is far more complicated than that. You really don’t need to worry about it. It affects everyone in a multitude of ways. There is no reason you would be affected by it with 11.1V batteries and I won’t at 12V. Those are relative numbers. Fully charged, your lithium batteries should total well over 12V and fully discharged my batteries will be well under 11.1V.

  216. September 19th, 2012 at 23:23 | #216

    Have you seen my previous reply on how I tuned the filter:

    I don’t the 11.1V voltage batteries are going to make a huge different, as Ryan also says – you have to understand that my battery actually start at just above 13V and then drops down to 9V or something before I charge it and it works pretty good anyway.
    Btw I updated the code at github, so it now fixes the problem with the swapped encoders and has also the information regarding the connections to the motor driver and encoders connections. It also uses my updated Kalman filter library and the newest version of the PS3BT library.

  217. Jetguy
    September 20th, 2012 at 01:34 | #217

    Thanks for the updated code!!! Trying it now. One tip for anyone who was using the previous version, the motors and encoders (both left and right) seem to be exactly reversed so definitely double check your wiring per the new notes in the posted code. In other words, I had mine wired working with the old code, flashed the latest from github and needed to change the wiring to match the new code. No big deal, I’m probably the only one who was using it anyway, and the new code works great!!! Very, very smooth is the best way I can describe the motor control.
    The only changes required to work with the 5DOF analog was to set my gyro sensitivity, slightly adjust the target angle in the .h file (could just be my bot), change the BT MAC to my dongle and it balanced right away.
    Thanks again!!!!

  218. Ryan
    September 20th, 2012 at 03:32 | #218


    My friend recorded the last one but I had to do this one my self. Best I can do with a broken wrist and a cast.

    I always assumed my robot would be top heavy as it should be due to the battery but its not. It’s almost perfectly balanced. This kind of explains why the wheels can never catch up with the top as it falls. The top just doesn’t have enough inertia to remain still while the wheels catch up. What’s the weight distribution like on your robot Lauszus?

    Have you ever measured the current through your motors?

  219. September 20th, 2012 at 12:02 | #219

    Glad you finally got it working. I kind of regret that I didn’t include the pinout in the beginning.
    But the wires for the motors were not reversed! That must be the reason why you couldn’t get it working! I simply mistyped the left- and right encoded when I originally ported the code.
    You are not the only one out there who is using the code, I know several people who is using it as well.

    You should differently put some weight in the top then. The batteries of my robot weight a great deal compared to the rest of the robot.
    No I have never measured the current through the motors.

  220. Jose_Luis
    September 20th, 2012 at 16:28 | #220

    Good interesting Lauszus your little robot .. I’m also doing a balancing robot and have had many difficulties regarding the use of hadware necessary because in my country do not ahy a shop specializing in robotics .. Well here I present a Avanze of my thesis work .. is similar to yours ..

    At the moment I’m not using the encoder .. only as the accelerometer sensor ADXL345, gyroscope L3G4200D, arduino nano, these engines, these wheels for road, and this power amp, I’m still in test mode as will be done later changing the engines and the use of a remote control .. …..
    I look forward to your support in some questions that I submitted later ….. thanks

    best regards from PERU

    PD. sorry but use the google translator. XD

  221. September 20th, 2012 at 21:42 | #221

    I can’t get the hardware here in Denmark too, that’s why I order everything online!
    Your robot balance pretty well. It seems like you only need to implement the encoders for it to balance perfectly.

  222. Ryan
    September 20th, 2012 at 22:14 | #222

    There aren’t any shops around the corner here in the U.S.A. either. I think everyone buys their equipment online.

  223. Ryan
    September 24th, 2012 at 03:05 | #223

    Did you ever change anything with your Kalman filter? If I guide the bot it does what it should. The wheels do their best to stay under the top. But they don’t do it fast enough. If I let go its probably leaning ten degrees past center before the motors go to work and by then it’s too late. I can’t find any combination of parameters to stop this. I am transmitting the angle to my laptop, watching the lean on an artificial horizon and I can’t tell if I can see any lag or not. I don’t know if the motors are slow to respond to commands or the filter is slow to track angle. I’m to lazy to start writing more code to tell me right now. This shouldn’t be that complicated.

  224. September 24th, 2012 at 15:13 | #224

    This is the part where I found an error in the original code:

    You could try to implement a complimentary filter instead like so: I more quick to tuned that and then afterwards you can implement the Kalman filter if you want too. The truth is that it actually ends up behaving as a complimentary filter, see this comment at my other blog post:

    Have you got some kind of visualisation of the angle? I find it very hard to actually see it it behaves as intended if I just look at the numbers.

    That’s why I often use a Processing code I wrote a while back:, so I can see that the filter is working as intended.
    I can see that you have done the same thing. Try to rapidly turn the IMU to for instance 90 degrees and then back to 180, if you look at you/or mine graph, it should be pretty obvious if the filter is responding fast enough.

  225. Ryan
    September 27th, 2012 at 17:44 | #225

    I did start to implement the complimentary filter but I got distracted and haven’t gotten back to it since. I do have a visual representation of the angle. My C# HMI has an artificial horizon like you would see in an airplane. I honestly can’t tell if it’s responding fast enough. It doesn’t appear to be off by that much if at all but I can’t think of any other reason it wouldn’t respond fast enough. Proportional correction is instantaneous (enough) that it should catch the bot given sufficient gain but if I let go and it starts to fall one way or the other it always falls that way. It falls too far before the controller kicks in.

  226. Ryan
    September 30th, 2012 at 17:04 | #226

    I look at everyone’s code and it’s so simple. My code is no more complicated but this effing bot doesn’t even try to balance. It has the general motion down but if I let go it will just fall to 10 deg and then take off in that direction. The wheels can’t catch up with the upper half. I’m about to throw it out the window.

  227. September 30th, 2012 at 21:51 | #227

    What if you rotate the IMU very fast 90 degrees? Does it respond immediately?

    Have you tried to scope the PWM pin and see if it actually behaves as intended?

  228. Ryan
    September 30th, 2012 at 23:06 | #228

    It appears to lag quite a bit. Just eyeballing it, I would say it’s still at 20deg by the time I get to 90deg and it takes 500ms or so to catch up. This is with a complementary filter now. Same results with a Kalman filter. Not sure what is going on here. PWM looks fine on my scope.

  229. September 30th, 2012 at 23:36 | #229

    Okay. What IMU are you using?

  230. Ryan
    October 1st, 2012 at 00:12 | #230

    I bought it on eBay and I had to ask the seller for documentation so everything I have is in a zip file. If you want to see a schematic I’ll have to email it to you. It’s very simple though. All I’m using on it is the ADXL345 and the L3G4200D. I’m reading it over I2C and I don’t see any special hardware for filtering. I know some senors allow you to enable filtering with external caps but none of that is present. So it should be enough just to look at the data sheets for those two devices.

    The only thing I can think right now is the L3G4200D may be writing to a FIFO faster than I’m reading so I’m reading old data. The data sheet is not clear on how that works but I believe I have the FIFO disabled.

  231. Ryan
    October 1st, 2012 at 00:59 | #231

    Can we talk via instant messenger or something? I work 60-70 hours a week and only get to mess with this every 2nd or 3rd weekend so I promise not to bother you much but I feel like I’m just missing something simple and second set of eyes and some consistent communication could get this thing balancing. Maybe you could watch it on skype or something.

    I’m almost certain the problem is my angle readings lag reality.

  232. Ryan
    October 1st, 2012 at 01:07 | #232

    I just committed my most recent code. Not many changes but a few.

  233. October 1st, 2012 at 01:52 | #233

    Okay. Have you tried to do something like this first:
    If you swapped the orientation it might look like it’s working, but the response is very slow, as you describe.

    Of course, but not today. It’s almost 3am here in Denmark πŸ˜‰

    Okay. I will have a look at your code one of the following days.

  234. Ryan
    October 1st, 2012 at 02:42 | #234


    You were right. I had my pitch and yaw axes swapped. It’s not balancing yet but I can tell its much quicker. I think it should balance tonight.

  235. October 1st, 2012 at 17:58 | #235

    You are not the first one to make that mistake πŸ˜‰
    Did you get it to balance?

  236. Ryan
    October 1st, 2012 at 21:02 | #236

    No but it wants to. It can catch its self now when it starts to lean but the controller still diverges eventually. I may still be missing something. Perhaps I’m not getting the true “0 angle.” I would still like to get on Skype with you when we both have some time but for now I have to go back to writing software that pays the bills.

  237. October 2nd, 2012 at 11:55 | #237

    You can add me at Skype. My account name is just “Lauszus”.

  238. Ryan
    October 5th, 2012 at 21:22 | #238

    How did yours look before you started using the encoders? Mine wants to balance but as soon as Iintroduce a disturbance it will do 2 or 3 times back and forth and then just drift across the room. I thought it would be better than that even before the encoders.

    Sorry, I have a hard time following your code so let me bounce this off you. You introduce a false offset angle for the bot to correct in order to correct your encoder offset? Thats what it looked like you were doing to me last night when I looked.

  239. October 5th, 2012 at 23:59 | #239

    I had to tune the targetAngle every time I turned the robot on – I used the up and down buttons on the PS3 controller for that.
    I didn’t handle pushes and so on that well to, so your robots behavior is quite normal.

    Yes I adjust the targetAngle depending on the input from the encoders. Check out these lines:

  240. Ryan
    October 6th, 2012 at 05:12 | #240

    Here is your English lesson for the day.

    The word you’re looking for is “brake”

    If you don’t slow down with the BRAKE, you might hit a tree and BREAK your neck.

  241. Ryan
    October 6th, 2012 at 05:38 | #241

    I can see integrating the encoders is a bit more difficult. How long did it take you to tune the offset angle generated by a given position error?

  242. October 6th, 2012 at 14:33 | #242

    Haha, you are totally right, I will fix it in the next commit πŸ˜‰

    It was actually much faster to implement the encoders.
    Is it able to balance forever without any disturbance? Here is a old video of my robot before I implemented the encoder and remote control:

  243. Ryan
    October 6th, 2012 at 22:45 | #243

    No mine is nowhere that good. Those are the same wheels I’m using, I believe we have the same motors and I think we have the same motor controller. However I think my issues may be more mechanical now. I’m not sure if I have enough of a moment arm to rotate the bot. My battery is nowhere near as large as yours and it’s mounted much closer to the wheels. Its mounted at the furthest point possible but still not far enough. Before I fixed the swapped pitch/yaw axes I had the battery on a big mast but I pulled it off so it would fit in my suitcase while I traveled. When can we get on Skype.

    I told you I would send you something if you could help me. You did almost nothing but helped me tremendously. What can I send you? I have a huge assortment of parts. A dozen or so XBee’s, processors, op-amps, FET’s, power supplies. What do you want?

  244. October 7th, 2012 at 16:07 | #244

    I’m starting to think that’s your problem is hardware related too.
    We can get on Skype tonight if you want to πŸ™‚

    Hmm, do you by any change have any atmega644? I need them for an upcoming job. I need them both in a DIP and TQFP package. Or just send me something you think I will find interesting πŸ™‚

  245. Ryan
    October 7th, 2012 at 22:35 | #245

    Let me see if I can convince Atmel to send me some samples. They aren’t very good about it but I think if I use my work email address they might just send them to me.

    What time does a Skype chat work for you? I’m in the EDT zone right now.

  246. October 7th, 2012 at 22:44 | #246

    I have tried that before using my business email as well, but they didn’t even answer me, but fell free to try it πŸ™‚
    Now is not a good time, it’s 11.42pm right now (I’m in GMT+1), but perhaps tomorrow?

  247. Ryan
    October 7th, 2012 at 22:53 | #247

    I used the 324p for almost everything I did while I was in college. My professor had developed an Arduino like board about 6 years ago which became a spring board for almost everything I did. But my first job out of college I used an XMEGA and it blew the doors off the ATmega. I don’t know what your requirements are but you are open to other options, take a look at the XMEGA. Atmel makes a development board called the XPlain and it’s only like $30 USD.

    I’m on Skype now.

  248. October 8th, 2012 at 13:23 | #248

    The problem is that it needs to be compatible with the Arduino IDE. I base it on the Sanguino which uses a atmega644p. The github repository can be found here: I already made the necessary changes for it to work with a atmega644, but I can’t test it without atmega644.

    I also use other microcontroller than the atmega series, I just find it better if I want the code to get out to a lot of people πŸ™‚

  249. Ryan
    October 8th, 2012 at 15:42 | #249

    I got a sample tracking number from Atmel this morning. They are sending me

    2 ATMEGA644P-20AUR


    2 ATMEGA644P-20PU

    which I will send on to you if you give me an address to mail them to.

    P.S. your Captcha is effing impossible.

  250. October 8th, 2012 at 15:47 | #250

    Nice thank you! πŸ™‚
    I will send my address by email.
    Haha, it’s just a plugin, but I will have a look at it.

  251. Ryan
    October 9th, 2012 at 00:54 | #251

    I can see now that horizontal translations GREATLY affect my pitch angle. WTF. Something still isn’t right.

  252. October 9th, 2012 at 10:30 | #252

    What do you mean by horizontal translations? Do you mean when the robot is lying down?

  253. October 9th, 2012 at 10:35 | #253

    I just googled “horizontal translations”, but I’m still unsure what you mean?

  254. Ryan
    October 9th, 2012 at 17:56 | #254

    A translation is just a movement from point A to point B as opposed to a rotation. Horizontal meaning along the ground. But really its any translation. So a rapid movement from point A to point B (like when the bot starts to take off) induces a force on the Z axis of my accelerometer and causes the bot to think it’s rotating when its not.

    Why would my Kalman filter not be taking care of that? It should. I’m glad I know what’s wrong now but I’m frustrated I can’t fix it.

  255. October 9th, 2012 at 18:05 | #255

    Ahh okay. Try to graph the data as we talked about and then just try to tune a simple complimentary filter, as it’s much faster to tune. That should be pretty easy.

  256. Ryan
    October 9th, 2012 at 18:34 | #256

    Tune as in change the .999 and .001 in the following?

    comp_ang = (0.999*(comp_ang+(gyro_y/32767.0*250.0*delta_t)))+(0.001*accel_pitch_ang);

    even the 999/1 isn’t cutting it. How far do I have to go? You didn’t have to go that far.

  257. October 9th, 2012 at 18:36 | #257

    Hmm, have you tried a different IMU? Maybe there is something wrong with the accelerometer?

  258. Ryan
    October 9th, 2012 at 18:49 | #258

    I do have another one with me I could play with. But the raw values look OK. What’s your resolution set to?

  259. October 9th, 2012 at 18:51 | #259

    For my robot I use analog sensors, that is fixed at ±3g for the accelerometer and ±300 deg/s for the gyro.

  260. Ryan
    October 10th, 2012 at 03:14 | #260

    Mine are +/- 250 deg/s and +/- 2g. I don’t understand why it seems like my Kalman filter is almost ignoring my gyro. The complementary filter I posted 4 posts ago wasn’t working because I needed to cast my gyro_y measurement to a float. Once I did that it started working and I am able to balance indefinitely. Its not pretty though. I can’t get the motors to agree no matter what I do. They never turn on at the same time and its not consistent. I can’t just set one to 90% of the other because it needs to be 110% 5 seconds later. At one point I was driving each motor with a separate PID controller on top of the one that controls my angle. So I can set them to 100 cnts/s and then they turn consistently but tuning 3 PID controllers at once if effing impossible. Translations still induce an offset in the calculated angle as well. At least I am making progress though.

  261. October 11th, 2012 at 16:50 | #261

    Okay. I think it looks pretty good now, maybe you could try to implement the encoders now?

  262. Ryan
    October 12th, 2012 at 20:43 | #262

    I want to figure out the Kalman filter first. The whole point of this thing is to ignore the unwanted dynamics and noise and its not doing that. I’ve been laying it on a tray that slides easily back and forth across the floor or a table. So the sensor remains at 0 deg pitch while I slide it back and forth and the acceleration on the x axis of my accelerometer will see up to about .25G which causes the filter to to read +/- 10-15 deg. This is why my bot still takes off. The complimentary filter isn’t much better but it is a little better. But like I told you before the point of this exercise for me was to learn about attitude estimation so I can’t really move forward until I understand it better.

    This stuff is difficult though! I had a hard time with signals and systems and later on DSP when I was in college and this is no different. I have the background to understand it but it just hasn’t clicked yet. Your write up on the Kalman filter is the best I have ever seen though. Really impressive stuff. I haven’t read the paper you said you wrote in HS but just the fact that you were studying the Kalman filter in HS blows my mind. I’m glad to have you in my technical network.

    I got back into town last night and your processors were in my mailbox. I will try to get them out to you as soon as possible. I’ll look around and see if there is anything else you might have interest in. Oh I know, I’ll send you some through hole Schmart boards. They are always handy to have laying around.

  263. October 13th, 2012 at 12:52 | #263

    Okay I understand. Your test bench sounds pretty neat πŸ˜‰

    I don’t think you will learn that much from the paper I wrote in High School, as most of it is actually about statistics and bell kurves and so on – it then introduces the Kalman filter and then a little about ADC’s.
    In the end I use the Kalman filter to estimate the voltage of a DC signal modulated by known Gaussian white noise.

    Awesome. I’m really looking forward to receiving the chips so I can play around with them πŸ™‚ Also thanks a lot for the Schmart boards – I have heard about them, but never used them myself.

  264. Ryan
    October 13th, 2012 at 17:54 | #264

    My “test bench” huh? Last week I was staying in a hotel for a business trip so the test bench was the tray the cups and ice bucket were laying on. This week I’m home without a tray so the test bench is a copy of Circuit Cellar magazine on a carpeted floor.

    Am I wrong to think the Kalman filter should be smart enough to ignore the horizontal accelerations? At least those brief transient ones?

  265. October 13th, 2012 at 18:56 | #265

    Haha okay πŸ™‚

    Hmm it might be, since the variance of the noise is not the same as when it’s actually balancing. So if you tune the filter to that situation it might actually not work when it’s actually balancing? Just a thought I have.

  266. flyrobot
    November 7th, 2012 at 18:22 | #266

    Hi Lauszus,

    I just read all the post and all the video’s. its the most stable balance robot i ever seen. 3 years ago i made similar to this robot but it never balanced well. Now im busy playing with multirotor such as, multiwii, MK, now autoquad.
    After i read this i interested to continue my balance robot, but with more modern digital sensor, such as MPU6050 and the price time by time become lower and lower. You can check this out : with multiwii code, its fly super stable.
    I think this is the cheapest sensor and ATmega328 in one pcb, even they give us ftdi. I have one and i plan to use it for this balancing robot.

    Im not coder but im familiar with arduino. So i hope you still keep maintain this project.



  267. November 7th, 2012 at 20:01 | #267

    Hi John.
    As you might have noticed we are also working within the Quadcopter area and we have actually recently received the “MultiWii 328P” from HobbyKing which works fairly well.

    And I agree with you, it will definitely be possible to use such a board to make a balancing robot.
    Unfortunately I don’t think you will be able to add the same amount of features as we have, such as PS3 Joystick remote control or via Bluetooth and your Android phone.

    But for a regular PID balancing algorithm and even also control over an Xbee module or similar, it will indeed be possible.

    Best Regards
    Thomas Jespersen

  268. flyrobot
    November 8th, 2012 at 01:41 | #268

    Hi Thomas,

    it is not possible to use joystick remote control and android phone, is it because not fast enough using ATmega 328 or its because arduino doesn’t has the library for PS3 joystick yet or porting from mbed to arduino is not complete yet ?
    What quadcopter do you working now ?


    Best Regards,


  269. November 8th, 2012 at 01:46 | #269

    It is indeed supported, but you will need a USB Host Shield:

    I’m a developer on the library. Take a look at github for the source code:

    Also take a look at this Android app I use to control the robot: and this branch of the code:

  270. flyrobot
    November 8th, 2012 at 04:15 | #270

    Hi Lauszus,

    Many thanks. Its really very very interesting project. I will continue my balance robot (never stable) I was inspired by this
    Hopefully i can built like that, i was thinking it can help many people such as disability people.

    Best regards,


  271. flyrobot
    November 8th, 2012 at 05:10 | #271

    Hi Lauszus,

    I just thinking why not the robot can be controlled by normal RC remote control, now its very cheap and have many channel and the robot can be controlled even farther upto 1 km or more.
    The most popular and cheapest is turnigy 9x.
    And the most active hacked

    Best regards,


  272. November 8th, 2012 at 18:17 | #272

    That would be awesome. I have also thought about building a unicycle πŸ™‚

    Yes RC control would be nice as well and actually pretty easy to implement. I might look into that in the future!

  273. flyrobot
    November 9th, 2012 at 05:52 | #273

    Once im ready with this small segway, i will continue with unicycle.
    Now i have several IMU from Fabio varesano and chinese made.
    You probably can see the multiwii code, i have multiwii,, dji wookong, Ardupilot, now autoquad (probably the best one so far). From all i have, i love multiwii very much since its open hardware, i can use any sensor any board (arduino) and some people porting it to STM32 base hw And it fly super stable. Also it can configure as from tricopter upto octocopter and helicopter and normal rc plane.
    I hope You see the possibility to add self balancing robot code it to be able multiwii can be configured as balancing Robot.

    Best regards,


  274. flyrobot
    November 9th, 2012 at 06:18 | #274

    With rc remote such as Turnigy 9x some creative people hack it tobe able to see the telemetry data, such as battery info, total power etc. without any external telemetry hardware into receiver part. its very usefull feature.

    If you merge your code into multiwii, i think you do not need much time to add those features. They are opensource as well. It just my thought.

    Best regards,

  275. November 9th, 2012 at 19:21 | #275

    Yes I can see that it’s possible. Let me know if you got any questions and so on and I will try to help you out! πŸ™‚

    Thanks for the feedback. I might add that later if I got the basic remote control working with a rc remote.

  276. November 10th, 2012 at 15:02 | #276

    Dear Flyrobot.
    What I meant with it not being possible is that it isn’t possible to add the USB Host functionality to the MultiWii board, as the proper pins isn’t brought out.
    It would for sure be possible to use the MultiWii board you have and then with an RC receiver and transmitter to control the Segway – no problem there πŸ™‚

    I’m actually working on two Quadcopters; one where I built my own frame and another one where I’m using the FPV Manuals QAV500 frame. Secondly I am also trying to invent our own IMU and controller board, including the stabilizing firmware.

    Would you mind giving me a link to the telemetry hack your talking about for the Turnigy 9x transmitter?


    Regards Thomas

  277. flyrobot
    November 11th, 2012 at 02:11 | #277

    Dear Thomas,

    And you check this :

    With Best Regards,


  278. flyrobot
    November 11th, 2012 at 02:13 | #278

    Dear Thomas,

    Thanks for the info. This the link :

    – for 9x forum :
    – the most active hacked firmware from eraz :
    – this is the code :
    – multiwii telemetry using 9x with frysky module :
    – multiwii telemetry using Graupner Hott Tx :
    – my Graupner Hott Tx for tuning multiwii PID :

    Best Regards,


  279. November 13th, 2012 at 21:52 | #279

    To get the Telemetry working I understand you would have to make a mod inside the 9x where you solder the TX/RX lines to the chip. Is this true?

    Next you would of course also have to change the transmitter and receiver module to a Telemetry supported FrSky module.
    Can this FrSky DJT be used:
    Or only this one:

    Regards Thomas

  280. flyrobot
    November 14th, 2012 at 06:40 | #280

    Hi Thomas,

    Yes, you have to change it to frsky since it has much better receiver and again super cheap. This combination both 9x and frsky is very popular. Becarefull the first one receiver module is not support telemetry. This receiver support telemetry

    You’d better using the frsky receiver that has ppm sum (cppm) capability. It will simplied the cable. You look into multiwii code.



  281. Project 11
    November 14th, 2012 at 16:38 | #281

    Hey, me and some colleagues at my university are building an omni-directional motorcycle, so in principle it is the same design as yours.
    We would appreciate any advice on the matter if you are willing to help?

  282. November 14th, 2012 at 23:52 | #282

    @Project 11
    Yes of course πŸ™‚ What do you need help with?

  283. Project 11
    November 15th, 2012 at 11:49 | #283

    What are your views on using an inverted pendulum to balance and omni-directional motorbike?
    We have explored many avenues and think this is the best idea.
    Or do you have any resources/books you would recommend to read for control systems?

  284. November 15th, 2012 at 12:05 | #284

    @Project 11
    How else would you do it? Normally people use a heavy spinning disk which prevent it from falling to the sides. The princip is a lot like a balancing robot, but more difficult since it can fall to the sides as well.
    Sorry I don’t have any books or resources I can recommend.

  285. project 11
    November 20th, 2012 at 15:36 | #285

    Thanks for the idea for the spinning disk, we will look into it for more information πŸ™‚

  286. flyrobot
    November 21st, 2012 at 16:24 | #286

    Hi Lauszus,

    Do you have any plan in short time to use RC remote on this robot with arduino?
    I really exciting if you could merge your code onto multiwii code. They are ready with not just RC transmitter (even use ppm sum, only 1 cable), GPS and magnetometer/compass (for return to home, next is “way point”) its really cool if the robot can do Return to home.
    Even some creative people using android (with voice guide) for telemetry and follow me (your robot follow the person bring the android phone).
    I hope i can do it but im not coder and have no spare time.



  287. November 21st, 2012 at 20:16 | #287

    No unfortunately, as I don’t own a RC remote.
    Yes that would be awesome! But as I neither own a multiwii or the RC remote, I don’t think I will work on it in the near future.

    You voice control comment inspired me. I just created a simple Android app that use the voice recognition service:

    I might work on implementing that to my project πŸ™‚

  288. flyrobot
    November 24th, 2012 at 06:31 | #288

    Hi Lauszus,

    can we chat using skype?


  289. November 24th, 2012 at 14:16 | #289

    Yes, my Skype name is just: “Lauszus” πŸ™‚

  290. Jakub Blokesz
    November 27th, 2012 at 00:12 | #290

    Hello Lauszus,
    Your bot is super stable:) Really nice.
    I`m writing because Ineed Your help. I`m building the balancing robot too. Here is a video of my robot:
    I don`t have encoders yet. The robot is balancing fine (I think so:p), but when I`m trying to push him he has a big problem to come back to the stable position. Only a small deviation and the robot is falling over. What should I do? Should I try to adjust PID values? Or maybe it is impossible to make a really stable robot like Yours without encoders. I don`t understand what do really encoders do?
    I hope that You`ll answer me.
    Thanks a lot.
    Greetings, Jakub Blokesz

  291. November 27th, 2012 at 00:24 | #291

    @Jakub Blokesz
    It looks like it needs a little more PID tuning, but it will help a lot if you implement the encoders.

    The reason why you implement the encoders is because it helps the robot to know when the robot is actually moving in either direction, so it will act accordingly before the speed gets too high, so it can’t catch up with it anymore!

    If you need a more technical explanation on how encoders work, then this wiki article is a great resource:

  292. Jakub Blokesz
    November 27th, 2012 at 00:58 | #292

    Thank You for Your quick answer. I`ll try to tune more PID. But could You say what results should I expect? The robot will be more stable during balancing or will be more resistant on my push? And maybe do You know which PID constant should I change.

  293. November 27th, 2012 at 01:05 | #293

    @Jakub Blokesz
    Implementing the encoder will both help it to balance, but also make it more resistant to pushes.
    No I can’t say that from the video, but it looks alright. You could try to implement remote control, but to get I really recommend implementing encoders!

    Here you can see the robot before I implemented the encoders: You can’t see it on the video, but if I push it, then it would immediately start to travel in one direction and then likely fall over.

  294. Jakub Blokesz
    November 27th, 2012 at 22:55 | #294

    Thank You for quick answer:) I am going to use AS5040 Magnetic Rotary encoders. I`ve got the enkoder but I`m waiting for magnets. Tomorrow I should receive magnets and as quick as possible I`ll try to implement them. I hope I can ask if I would have some problems with them.

  295. November 28th, 2012 at 15:48 | #295

    @Jakub Blokesz
    Okay. Of course, just ask, I’ll be happy to help πŸ™‚

  296. Jakub Blokesz
    December 12th, 2012 at 16:22 | #296

    The mechanical part of my work is done. I fitted the robot with the encoders. Now I`m trying to create appropriate software for the encoders. I want to use Your algorithm but unfortunately I don`t understand how Your algorithm works. I see You use external interrupts to read the encoders. First question why do you use only rising edge? Both edges would give 4 times better resolution. Then in the main loop You compute every 100ms the wheel position and wheel velocity. And what’s next? I don`t understand how are you using these two variables in the PID function. If You could describe it for me I would be grateful. On the others sites (for example this: )
    there is another algorithm for the encoders but it is different to Yours. He is using second regulator(PD) and in Your code I can`t see a second regulator. Thanks a lot and wait for Your answer.
    Jakub Blokesz

  297. December 12th, 2012 at 16:50 | #297

    @Jakub Blokesz
    I only read on the rising edge, as I decided that I didn’t need that much resolution. Right now I get (64*29)/4=464 CPR, and if I used interrupt on change, then I would get: (64*29)/2=928 CPR, but then I had to compare the two pins as I didn’t know what the state of the pin connected to the interrupt was, but it’s fairly easy as well. Just take a look at the NXTShield library I wrote:

    To get the maximum of resolution, then you would need to connect both pin from each encoder to an interrupt, but since the atmega328 only has 2 interrupts, it is not possible.

    The reason why I only read the encoders every 100ms is to filter out the noise you would get if you simply just read the encoders every loop.

    The reading from the encoders is used inside the PID loop, take a look at these lines:

    And it’s also used to scale down turning at high speed: and

  298. Jakub Blokesz
    December 16th, 2012 at 15:17 | #298

    I am using Your algorithm to use encoders. But I don’t know how to set constants values for example: zoneA,zoneB, … Could you say how to adjust them? and second question do you think that 256 impulses per wheel rotate is ok? I have the encoders on the wheels.
    Thank You.
    Jakub Blokesz

  299. December 16th, 2012 at 15:40 | #299

    @Jakub Blokesz
    I have 464 cpr myself, so I think 256 cpr will just as fine – the deal is that it doesn’t have that high a resolution, as long as i can simply tell if it’s going forward or backward.

    You simply set them here: Try to divide all by about 2, as the ratio between my encoders resolution and yours is approximately 2.

  300. Jakub Blokesz
    December 16th, 2012 at 23:20 | #300

    The encoders are working. The robot is more stable than without them. But to obtain that I had to set constant values much smaller than Yours. These are my values:
    zoneA = 1800;
    zoneB = 900;
    positionScaleA = 53;
    positionScaleB = 100;
    positionScaleC = 212;
    velocityScaleStop = 6
    I only don`t know why the robot doesn`t always come back to the previous position. I think that only when I push the robot harder it comes back to the previous position and when I push him just a little bit he doesn`t come back.
    Could You say how strong can You push Your robot that he doesn`t fall down(max safe tilt angle)?
    I want to adjust constant values wirelessly. This is more comfortable way. Later I want to add
    wireless steering and I don`t know if I can use only one bluetooth module to set values and to steering? I`m asking because You used two modules.
    Thank You a lot for Your help.
    Jakub Blokesz

  301. December 16th, 2012 at 23:45 | #301

    @Jakub Blokesz
    I can push mine pretty hard, but it all depends on what hardware you have.
    I think the reason why yours doesn’t come back is because you velocityScaleStop value is too small, try to increase the value and see if it helps.

    To set the values I use this Processing Application or this Android app:

    No I never used two modules, at the beginning I used Xbee modules, but right now I use the SPP library I wrote:

    I have actually made three different versions of the of the code depending on how I want to control it. The SPP version:, PS3 version: and the Wii version:

  302. Jakub Blokesz
    December 27th, 2012 at 22:33 | #302

    I think the encoders are working correctly. But I decided to go back and try to find better PID values because the robot with encoders didn`t balance so good like Yours. And I was right. At the beginning I had a positive values of D term. But if I set negative D term the robot moves smoothly, much better then with positive values. But I can`t reach such a nice result like Yours. I`m trying to set PID without encoders. I want to reach the same result like this on Your video:
    If You could explain how did You find Your values? Maybe some steps which I should do to tune PID. I tried to tune it whole day but without any result. The best result i reached with: Kp=7; Ki=3.15; Kd=-9;
    But without encoders the oscillation are bigger then Yours.
    Thank You a lot for Your help and for so quick responses:)

  303. December 28th, 2012 at 22:14 | #303

    @Jakub Blokesz
    How do you set them? Do you set them wirelessly? I really recommend that as it’s much faster to do compared to re-uploading the program all the time.

    See this reply for more information on how I found the PID values:

  304. Jakub Blokesz
    December 28th, 2012 at 23:17 | #304

    I set them through Serial port but using a cable. I bought BTM-222 and tomorrow I`m going to use it do tune PID wirelessly. Thanks for the instruction how to tune it. I`ll try tomorrow with it. Maybe do You know why my construction has to have negative D values. I compared my code with Yours but I don`t see any differences which can give this result.
    Maybe I have problems with tune PID because I`m using L298 H-Bridge which is much worse then Yours.

  305. December 29th, 2012 at 00:22 | #305

    @Jakub Blokesz
    You’re welcome.

    Are you using the same motors as me: They have a stall current of 5A, but the L298 can only deliver a output current of 2A per channel! See:

  306. Jakub Blokesz
    December 29th, 2012 at 01:22 | #306

    Yes, I`m using the same motors only without built-in encoders. I know about L298 but I thought when the motors will take more than 2A the IC will spoil, not limit the motors. Your H-Bridge is very nice, but it costs very much:( But if it helps I`ll try to buy it.
    I see one more thing. Why do You use micros() function for timing? I have time in miliseconds. Microseconds gives better results? I ask because I`m not using arduino so I can`t quickly use another function-micros().

  307. December 29th, 2012 at 13:00 | #307

    @Jakub Blokesz
    Yes I know. It’s a expensive motor driver. I don’t know if it will help, but it’s not a good idea to use a undersized motordriver.
    Because you get a higher resolution by using micros() instead of millis(), by doing so I can ensure the loop rate with a higher accuracy.
    What MCU are you using then?

  308. Jakub Blokesz
    December 30th, 2012 at 01:13 | #308

    I am using ATMega32 but it isn`t Arduino. But it is not so easy to obtain timing in microseconds. If You see micros function, it isn`t so easy like millis(). But I`ll try to change this time to microseconds.
    Now I can tune PID values via Bluetooth:) So maybe it will help me to set better values.
    The problem is that if I set I term to high it`s more stable but the oscillations are bigger and faster. If I set I term to small the oscillations are smaller but the robot isn`t stable and it can fall over himself. This all is without encoders. With encoders is better but not so good as on Your video:)

  309. December 30th, 2012 at 16:12 | #309

    @Jakub Blokesz
    You can easily just copy the micros() from the Arduino source. First you need to set up timer 0 like so:
    Then you need the micros() function:

    Then you have to define timer0_overflow_count:

    And sbi and cbi:

    And finally clockCyclesPerMicrosecond():

    Okay it sounds like you are making progress though. What wheels are you using? πŸ™‚

  310. Jakub Blokesz
    December 30th, 2012 at 23:57 | #310

    Thank You for the instruction. I made a timing in microseconds:) But unfortunately it doesn`t help.
    I use these wheels:
    I`m not sure if I`ve got a right place for an accelerometer(MMA7361). I haven`t it in the gear shaft. It is about 3 centimeters above the shaft. And I read that if it isn`t in the gear shaft, a tilt angle can be incorrect. Maybe do You know if it`s true?

  311. December 31st, 2012 at 00:00 | #311

    @Jakub Blokesz
    Too bad.. Okay, I still recommend the BaneBots wheels, but those will be fine for now – and the BaneBots wheels are also ridicules expensive!

    My experience is that as long as you just don’t put it on the top then it’s okay. Put the ideal place is just at the gear shaft πŸ™‚

  312. Kaelin Ellis
    December 31st, 2012 at 01:12 | #312

    How did you end up doing the controls for your robot? Initially I was thinking about using Transfer functions with the input being the wheel angle and the output being the robot’s tilt angle. It seems though that the system has multiple inputs (wheel one angle, wheel two angle), and multiple outputs (body tilt angle, yaw angle), which lends itself to a state space approach. I was hoping you could elaborate on the equations of motion you used, and the level of detail you went with them.

  313. January 2nd, 2013 at 03:05 | #313

    @Kaelin Ellis
    To control the robot back and forward I set a offset angle, see:
    And to turn it I set the turning values like so:

    One important thing I also mentioned in the video is that I limit the value depending on how fast the wheels are turning, see:

    Another thing I do is that I set a new target position when I want it to stop, see: and the video:

  314. Dustan
    January 14th, 2013 at 18:52 | #314

    Hello Im looking for some help i am not entirely a noob at arduino but close enough. I have bought a MPU6050 acc/gyro no mag I2C board and i have read the entire 25 pages on the arduino site and trying to get something working for me for nearly 2 weeks now. I can read the sensor using your code, I think. I have your Kalman filter arduino code that Im trying to incorporate into another arduino code written for an analog 5DOF IMU meant for a segway clone and i cant for the life of me figure out what to subistitute for what. I know its probably easy for someone else to learn arduino and code writing but im really struggling with this more advanced stuff that im not used to. This is my first non basic arduino project and its kickin my butt so if theres any way i could send you the two codes I have to combine could you help? I know I said “help” and i hope you dont think i just want you to do this for me but I really dont know what to do? Sorry for the long and poorly written message and thanks for anything you can do to help me in advanced

  315. January 14th, 2013 at 18:59 | #315
  316. Dustan
    January 16th, 2013 at 07:00 | #316

    Hello Thanks for replying. Yes i have seen and downloaded that code but it says sketch too big so i commented out the things you said and it is still 32020kb and my arduino IDE says it can have 30720kb?? I dont need bluetooth or usb control although would be cool. Would i just comment out all bluetooth and usb stuff and the code still work?? Also replacing thos functions is where im having problems im not sure how or what to replace them with for my MPU6050 its very confusing to me. I tried using getMotion6 raw values but thay dont work with the segway code i have maybe they would in your code if it wasnt too big to fit on my arduino?? I uploaded the code for the segwayand your kalman filter that i was gonna try and combine. But if your “balancingRobotArduino” code can be made smaller and adapted for MPU6050 then i would use that. Maybe you could take a look at the code i posted and tell me if i should use it and make it read the MPU6050 or use yours and the MPU6050. the first file i forgot to name but it is the segway code. Here is where i put the code at

  317. Dustan
    January 16th, 2013 at 07:24 | #317

    Sorry i forgot to mention im not using motors that have encoders like you did my hardware is a Sabertooth 2×25 motor driver which i bought here:
    A MPU6050 acc/gyro i bought on ebay eaxctly like this one:
    and some electric wheelchair motors here:

    Hopefully these links work


  318. January 16th, 2013 at 16:30 | #318

    Yes just comment out all the Bluetooth stuff: You only need that to control the robot. There will be plenty of flash and ram available if you remove all the USB and Bluetooth libraries.

    Have you succesfully tried this example I created:

    I would recommend using my code if you want me to help you out.

    Your hardware seems alright. But I would recommend taking some smaller steps at the time. First make a small program that will simply turn the motors on and off. Then maybe enable some kind of control. Then get your IMU working and then you can try to put everything together. My experience is that people sometimes rush everything together because they are too focused on making the end product, but you will spend much more time debugging if you don’t have the basics sorted out.

  319. Dustan
    January 16th, 2013 at 18:37 | #319

    I commented out the Bluetooth stuff and it looks like i have plenty of room now. Yes i have that example and it is the one i was trying to implement into the segway code i just didnt know which values to use, I assume I would use “kalAngleX” and “kalAngleY” somewhere, but dont I need an accelerometer value also perhaps “accZ”? I just dont know what values i need to provide to the code, as in does it need raw value of acc and the filtered kalman x and y or do i not need anything but the filtered x and y?? I can run the example you show and i get several readings that seem to be correct when i move the sensor and I also can make my motors turn I just dont know how to make them move with the sensor. In this link you provided earlierof your balancingrobot lines 427-462 would i just change the all of the analogreads with the raw values of the acc/gyro and map them from 0-1023 to mimic analog inputs? Im sorry for not understanding and all the stupid questions @Lauszus

  320. January 16th, 2013 at 18:53 | #320

    I just created this gist for you. Check it out:

    You have to understand that the Kalman filter is a library in it self I created. You can find the source code here:

    And read more about it at my blog post:

  321. Dustan
    January 16th, 2013 at 19:35 | #321

    Thank you for the gist! I already have the library, Deciphering it is my problem lol. I have also visited your blog But i think maybe you went to a far better High School than I as i do not “have some basic knowledge about matrices like multiplication of matrices and transposing of matrices” nor do i recall anything of the sort even mentioned in high school lol. but anyway i still tried to follow the links and read along. Thanks for you help i will study the gist you provided and try to figure out how to us it with my motor controller

  322. January 16th, 2013 at 19:39 | #322

    Okay. I haven’t learnt about matrices or anything like that in High School. I just decided to look into it and it’s really not that hard.

    Okay good. Let me know if you got any further questions and I will be happy to help you out πŸ™‚

  323. Dustan
    January 18th, 2013 at 05:34 | #323

    Hello again I tried to implement your code to the code I already had because it was already set up to use a Sabertooth motor driver. I was hoping you could look at it and see what i need to do about the “Hiresgyro” part in the code adc1 and adc5 I just made them the same. also I couldnt use the filtered values because i think they do some filtering down below and I didnt know what to take out so i could use the kalman filtered stuff i tried to highlight the part im having trouble with could you take a look at it for me if you would. theres some more stuff below the highlighted part that i think does what the kalman filter does but im not sure the “gangleratedeg” and “gangleraterads”. Sorry to bother you again. Here is the link: Thanks for all your help so far Hope Im not bugging you to badly@Lauszus

  324. Dustan
    January 18th, 2013 at 07:02 | #324

    the code makes my motors move back and forth but seems to be very slow to react to the change of the sensor im sure its because i screwed up the implementation tho

  325. January 18th, 2013 at 20:46 | #325

    Hmm, your code looks pretty messy. But I think you can just remove all this code:
    If the reactions are slow, then it’s because you don’t trust the accelerometer enough. Try to increase this value:

  326. Dustan
    January 18th, 2013 at 22:31 | #326

    Yes Im sorry for how messy it is, its mostly because i keep adding and subtracting things here and there to try to get it to work and also im not very good at code wirting in general. I am glad you said i could remove that whole section that was what was really confusing me. So is that where I would use the kalman filter and add something to tell my motors what to do? that value did help with the reaction time it was the only one i left alone because i thought it was just to keep the gyro drift calibrated. My cousin is comming intotown this weekend he has done alot of computer programming but no arduino but maybe he will be able to help me with some of this. Im sure he will be able to decipher it better than me so maybe i can stop bugging you with all my stupid questions. Again im really sorry for how much i do not know and for how much ive asked of you I im sure i bit off more than i could chew with this project but its mainly because i couldnt find the IMU that the original code used or else it wouldve worked right off but now ive spent over $300 on parts and supplies for this and would hate for it to not even get to balance because of me. SO THANKS A MILLION!!

  327. January 20th, 2013 at 14:40 | #327

    No problem. Don’t worry about it. I’m just happy that people use my code πŸ™‚

  328. boyfrind
    January 27th, 2013 at 14:42 | #328

    hello. excuse me for earlier but I don’t know English, because from Russia. to me your opinion concerning one idea is very important. what will you tell me to the person as that not knowing how it is difficult to make the balancing device on a basis the motor wheel?

  329. January 27th, 2013 at 15:49 | #329

    I have no idea sorry. I have never tried anything like that!

  330. boyfrind
    February 1st, 2013 at 14:29 | #330

    and it would be cool if it was possible to dress not each foot on a wheel from a bike and to go as on rollers!

  331. February 2nd, 2013 at 13:42 | #331

    What do you mean? I can’t understand your comment, sorry..

  332. Peter Seddon
    February 2nd, 2013 at 18:08 | #332

    I am trying to compile the Arduino Balancing robot to a Mega1280 board but I get a compile error “Kalman does not name a type”. I have the .ino and Kalman.h and BalRobot.h files in the same folder. Why does this error occur?

    Sorry to ask such a simple question!

    regards Peter, UK

  333. February 2nd, 2013 at 18:12 | #333

    @Peter Seddon
    Simply replace:

    #include <Kalman.h>


    #include "Kalman.h"

    Or you could put the library in the library folder and then it should be able to see it.

  334. Shane Pentz
    February 2nd, 2013 at 22:27 | #334

    I am very impressed with the fact that you have an android app to control your balancing robot. I have been working on a full size segway for some time now, and have been using bits and pieces from different sketches I have found. I am very interested in using the balanduino app that you made for android. Now, I have my segway mostly stable, and I can ride it (mostly just fine tuning now). I really want to use your Application for remote control of it, but I am no android app developer and am having a hard time figuring out your code and why you used multiple wireless communication options simultaneously. I purchased a simple bluetooth addon for arduino. It just has rx and tx going to the Arduino Mega board I am using.
    I was wondering if you would be willing to simplify/explain the coding you use for communication between the android app and the arduino, and maybe help me along. This is one of the last things I would like to really implement on my segway and I am kinda really stuck.

    Any help would be greatly appreciated.


  335. February 3rd, 2013 at 00:51 | #335

    @Shane Pentz
    Do you simply want to use my app for your segway or do you want to write your own?
    I basically control the robot in two ways, either by sending pitch and roll from the phone as a string or sending a x,y-coordinate from the phone based on the joystick.

    To use it for your segway all you have to do is implement these lines:

    I think it will be easier for you to understand if you try to connect to your PC and so you can see the incoming data. Basicly it consist of a letter and then some values seperated by commas. Then a semicolon in the end, so the microcontroller knows when it has reached the end of the string – see these lines:

    For instance if the x-coordinate is 0.5 and the y-coordinate is 0.75, then the string sent from the phone would look like this:

  336. Shane Pentz
    February 3rd, 2013 at 23:01 | #336

    Ok, so if I created a new sketch consisting of only that remote control section, and pair my phone with my computer’s bluetooth, I should be able to see the data coming in?

    A couple questions about bluetooth working with my computer (I’m new to the bluetooth communication).

    How would I set up the PC to run sketch that would show this information? Use processing?

    And how do you setup the correct COM Ports within the sketch? Is that a setting in the library you created?

    And I am going to be using this bluetooth module…

    should that be sufficient for this application? And will it work with the your library?

    It’s really tough and sometimes very frustrating to take on a large project like this when I’m not all that good at coding, but I know exactly what I want to accomplish. It’s nice to have someone to bounce ideas off of.


  337. Shane Pentz
    February 4th, 2013 at 22:23 | #337


    In other words, if I have my bluetooth adapter connect to TX/RX pins 18/19 on my Arduino Mega. Where would I change the port setting in the sketch itself?

    And also, my method of balancing is slightly different than yours. What do you consider the best method of actually commanding the bot to drive forward/backward. Would you just alter the setpoint/targetangle higher and lower?


  338. February 5th, 2013 at 00:35 | #338

    @Shane Pentz
    What OS are you running. Each OS set up the Bluetooth communication different, if I I’m going to guide you then I need to know.
    If you just want to use an Android phone, then take a look at this post:
    I recommend starting out with the example code I wrote until you get familiar with how it works:

    The com port can’t be set in the Arduino code, but is assigned by your computer.

    If you are using that Bluetooth module then you will only be able to use it as a wireless UART and not use it with all the other Bluetooth libraries I wrote. But if you only need SPP, then you can buy that module just fine.

    To use the module simply connect TX to RX on the Arduino and RX to TX on the Arduino. If you choose to use pin 18 and 19, then you are using the second hardware UART on the Arduino. To send a “Hello World” to your PC/phone simply use this code:

    Serial1.println("Hello World");

    Remember to run the following in your setup():


    Right now I simply adjust the targetangle, but I’m actually thinking about rewriting it so you set the desired speed and then this goes into a PID loop which then outputs the needed angle.

  339. Shane Pentz
    February 5th, 2013 at 03:57 | #339

    Thanks for getting back. I am using Windows 7. I’ve been trying to accomplish anything meaningful using the guidance you have provided but I am still failing to establish any kind o meaningful communication between my android device and the arduino…
    I can get the android bluetooth emulator to pair with the arduino bluetooth, but that’s all I can get it to do. Serial1.println isn’t showing anything on the android side.

    Do you have a sketch that would work with the balancing control App that you created. Like just the bare-bones that would allow me to Serial.print the joystick information that is coming into the arduino?

    I think that would be a good start for me…

    also, I see the USB host shield being used a lot, is that something I would need to make this work?

    Thank you.

  340. February 5th, 2013 at 11:01 | #340

    @Shane Pentz
    Here a two pages to get you started: and It should be fairly simple to get it working.

    What Android Bluetooth emulator are you talking about?

    You only need the USB Host Shield if you want to be able to control it with a PS3, Wii, Xbox, Joystick, mouse etc.

  341. Shane Pentz
    February 6th, 2013 at 06:15 | #341

    Well, I started out using Bluetooth SPP, but I have a question about that. When I pair with the arduino bluetooth, the Bluetooth SPP app asks if I want to use Realtime, Command line, etc….
    if I select command line, it asks me about /r, /n/, or /r/n, etc. Not sure which one I should use.

    I’ve also attempted to use BlueTerm, which doesn’t ask me about any of those settings…

    So which settings should I use?


  342. February 6th, 2013 at 14:17 | #342

    @Shane Pentz
    Just use rn and normal mode. rn means carriage return new line, just let both be on.
    You can read the description here about what the different modes do:

  343. February 6th, 2013 at 15:37 | #343

    @Shane Pentz
    Here is a guide on how to use Windows 7 with the library written by the other developer of the USB Host library: It’s basically the same procedure if you are using your own Bluetooth module.

  344. Shane Pentz
    February 6th, 2013 at 21:28 | #344

    Ok, you are being a lot of help, I appreciate that. I have been researching and I believe I am beginning to grasp it.

    A question about a section in the example sketch.

    USB Usb;
    BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
    /* You can create the instance of the class in two ways */
    SPP SerialBT(&Btd); // This will set the name to the defaults: “Arduino” and the pin to “1234”
    //SPP SerialBT(&Btd, “Lauszus’s Arduino”,”0000″); // You can also set the name and pin like so

    What does it mean when you set pin to 1234 or 0000?

    And do I need to include the USB section if I’m not using a USB Host Shield?

    And the SPP SerialBT part of it? Is this all referring to the use of the USB host shield?

  345. Shane Pentz
    February 6th, 2013 at 21:33 | #345

    I thought the pin was set in the firmware of the bluetooth module, not within the sketch.

  346. February 7th, 2013 at 13:18 | #346

    @Shane Pentz
    You can set the pin to whatever you like, but the maximum size by Bluetooth specs is 16 characters.
    No you don’t have to include the the USB section when you are not using the USB Host Shield. Simply replace all the SerialBT function with Serial1!

    When you create the SPP instance it will write the name and pin to the BTD instance like so:

    Which will then check if the pin is set like so: and then send the command: Just ignore the part for the wii controller. This is the part that reads the pin:

  347. Shane Pentz
    February 11th, 2013 at 22:16 | #347

    Thanks again,
    I’ve just made some modifications to the segway that I have to sort out now, but as soon as I get everything solid again…I will be implementing the bluetooth.

    I will be sure to post a video or something to show you how it worked out.


  348. February 11th, 2013 at 22:21 | #348

    @Shane Pentz
    Nice. Looking forward to see the result! πŸ™‚

  349. Dustan
    February 13th, 2013 at 08:29 | #349

    Hey Shane i am also trying to do the same thing you are, I have same bluetooth adapter. have you gotten anywhere with this? I have noticed I get all the values from his app if i load a completely empty arduino sketch just a void setup and void loop that are empty and open the com port. I start getting “S;” and if i touch joystick tab i start gettin j,0,0; and if i move joystick it spits out the values. But i havent figured out a way to read the values after i write any code that has Serial.begin in it i was hoping you had some helpful info that could guide me. I was hoping to not use Lauszus’ library only because it is so large I cant fit any of my other code for my segway onto my duemilanove@Shane Pentz

  350. February 13th, 2013 at 16:46 | #350

    Simply replace with
    Also change this line: to:

    if(input[i] == -1)

    You also have to replace these lines: and


    It’s should be pretty easy!

  351. February 13th, 2013 at 18:18 | #351

    Is there a trick to get all this to work on a Arduino Uno with the PS3 Bluetooth Library. I’ve commented out the DEBUG lines in the code but the sketch is still too large.

  352. February 13th, 2013 at 18:22 | #352

    Have you remembered to comment out this line as well:

  353. February 13th, 2013 at 18:34 | #353

    Lauszus. I really applaud you for all the support you provide here. There are a ton of comments and it’s easy to see you invest a lot of time in all your responses. I think there is enough interest you could start a forum dedicated to this. One benefit would be that others might be able to search to find their answers and possibly reduce some repeated questions. Plus others might be able to start helping you out with all the questions. I know there’s also a thread on the Arduino forum, but it’s the same problem with a single thread being too long. Just a thought and thanks again!

  354. February 13th, 2013 at 18:38 | #354

    Thanks for your feedback. We will differently think about doing something like that!

  355. February 13th, 2013 at 18:45 | #355


    I did comment that one. Am I loading more than I need to maybe?

    #include "BalancingRobot.h"
    #include <Kalman.h> // Kalman filter library see:
    Kalman kalman; // See for source code

    #include <SPP.h> // SS is rerouted to 8 and INT is rerouted to 7 - see at "5. Interface modifications"
    USB Usb;
    BTD Btd(&Usb); // Uncomment DEBUG in "BTD.cpp" to save space
    SPP SerialBT(&Btd,"BalancingRobot","0000"); // Also uncomment DEBUG in "SPP.cpp"
    #include <PS3BT.h>
    PS3BT PS3(&Btd);
  356. February 13th, 2013 at 19:02 | #356

    Yes, if you only want to use the PS3 controller, then remove the SPP library.

  357. Dustan
    February 14th, 2013 at 08:43 | #357

    @Lauszus Hey Lauszus Thanks for the info. I hope you didnt take what i said wrong. Im very glad for all your help you have given,I just was saying I cant fit your library and my segway code both onto my Duemilanove board so i was needing something small and simple for my own useage. I have come up with this It doesnt work like you intended I dont guess but I get the values that I need and I can plug them into my code and it compiles at 8k bytes even with the debug so now I can fit my code too. @Shane Pentz You might be able to use it or do something more with this Im not very good at codeing but this did work for me and my particular needs

  358. Dustan
    February 14th, 2013 at 09:05 | #358

    Oops i accidentally posted the messed up version of that code this one works Sorry. the code needs work (probably a lot of it) but i just thought it would be helpful

  359. February 14th, 2013 at 15:56 | #359

    No problem. You can use the code for ever you like as long as you publish it under the same license πŸ™‚ But my code it’s actually running on a ATmega328P (The AVR used for Duemilanove, Uno etc.).

    The reason why the library is returning 255 (I believe it’s actually returning null) is because you need a delay here: Just add:


    Also you should check if data is available or not. Like I do here:

    Finally replace this line: with:

    if(input[i] == -1)

    As I told you earlier! πŸ˜‰

  360. Dustan
    February 14th, 2013 at 19:53 | #360

    @Lauszus OK thanks. When you say “Finally replace this line: with:
    if(input[i] == -1)”

    do you mean add that back to my version because i removed it and that it needs to be -1? above this line:

  361. February 14th, 2013 at 19:56 | #361

    Yes you need to add the following above that line:

    if(input[i] == -1) // Error while reading the string

    The reason why you are reading 255 here: Is because is actually returning -1. If you change the datatype of the variable incoming to char, then you will see it.
    See the source code for more information:

  362. Dustan
    February 14th, 2013 at 21:04 | #362

    Hmm I changed the variable incoming to char instead of uint8_t but when i print it is pirints this weird thing here: ÿ am i doing something wrong? i just did a
    char incoming =;

    is it becuase im using the SoftwareSerial library and not your library for the communication?

  363. February 14th, 2013 at 21:09 | #363

    It’s because the Arduino library think you are trying to write a character.
    Simply print it like so:

  364. Dustan
    February 14th, 2013 at 21:50 | #364

    Oh ok sorry for the noob mistake. that fixed it

  365. February 14th, 2013 at 21:52 | #365

    No problem πŸ˜‰

  366. Dustan
    February 17th, 2013 at 12:18 | #366

    i found out why i couldnt fit your code onto my arduino duemilanove its because the boot loader takes up more space than the one for say the uno. I can compile if i say the target board is the uno but cant compile (too large) if I say target board is the duemilanove. Do you know if i could load the code some other way?

  367. February 17th, 2013 at 14:48 | #367

    You can upload the Uno bootloader using what is called ICSP (In-Circuit Serial Programming).
    I have written a blog post about it a while ago:
    But you could also just use the PS3 only code at the PS3 branch: When I compile it only takes up 26.798 bytes – remember to comment DEBUG in BTD.h and PS3BT.h.

  368. Dustan
    February 19th, 2013 at 23:59 | #368

    Ok Great Thanks. I think ill try the ICSP because I have Wii, PS3, and an XBOX controllers and would like to test each one to see which I’d like to use permanently.

  369. February 20th, 2013 at 00:05 | #369

    Okay. Please make a video of it. I would love to see your robot in action πŸ˜‰

  370. February 20th, 2013 at 14:30 | #370

    Please publish your newest gist as I need to send it to another guy who want to do something similar.

  371. Mr.Engineer
    February 23rd, 2013 at 02:24 | #371

    Hi Lauszus,
    It is amazing u get the robot very much stable. How did you use the PID to drive the motor?? and it is your sampling rate?

    Thanks in Advance

  372. February 23rd, 2013 at 13:56 | #372

    Take a look at my pid loop: and this is the part that reads the PS3 controller and convert it to movement of the robot:

    In the video I used a 10ms time fixed loop.

  373. waszil
    February 25th, 2013 at 14:09 | #373

    First, congratulations!
    My question: if im right, neither the PID controller, nor the Kalman filter runs at a fix sample rate, instead they get called as often as the loop() allows?
    I am not too familiar with arduino, but as i cannot see any timer interrupts calling the PID and kalman at given frequency, i assume, that is the situation.
    Approximately what is the sample rate of the control loop anyways?

  374. February 25th, 2013 at 15:54 | #374

    Yes they are just called in the loop. The sample rate is 10ms.

  375. Barry
    February 26th, 2013 at 00:43 | #375

    Hi Lauszus.

    Thanx for the great project & explanation!

    i can’t seem to get your Processing Application to work (Windows32 application). No errors, just nothing when I run the .exe. Any idea what this could be? Using latest Java version and also copied ‘lib’ and ‘data’ dirs.

    Regards Barry

  376. Barry
    February 26th, 2013 at 00:44 | #376


    Forgot to mention: Using Windows 7

  377. February 26th, 2013 at 17:26 | #377

    There seems to be a problem with the Processing app on some Windows computers (I’m on a Mac). Instead I will be working on writing an app that uses Java’s Bluetooth API in the next month or so.

  378. sir66
    March 4th, 2013 at 17:35 | #378


    I am Sergey from Moscow.

    This is my balancing micro robot LambdaB. It based on your program for Arduino. It can only stand on one place now, no encoders, but encoders and moving are in my nearest plans. There is TpLink 703N router on the top for remote control and video streaming in the future (it is not uses in this test), MultiWii V2 flight controller board uses for move control. It is also uses Pololu 100:1 Micro Metal Gearmotor and Pololu TB6612FNG Dual Motor Driver Carrier.

    Thanks for your sources and recomendation.

  379. March 5th, 2013 at 14:13 | #379

    Sounds really interesting! Unfortunately the video is private, so I can’t see it. Can you make it public so I can have a look? πŸ™‚

  380. hajime
    March 5th, 2013 at 14:49 | #380

    The zip file of the BalancingRobotArduino in seems to missing some files.

  381. March 5th, 2013 at 14:54 | #381

    It’s because it doesn’t download the submodules.
    That’s why I wrote this shell script:

    Just run:

    curl | sh -s git://

    in a terminal and it will download the newest version of the code including all the submodules!

  382. sir66
    March 5th, 2013 at 15:30 | #382
  383. March 6th, 2013 at 17:13 | #383

    Looks good, but try to increase the height a little and then put some weight on top. I think you will get better performance that way.

  384. Shane Pentz
    March 8th, 2013 at 22:25 | #384

    Hi guys!
    Good to see so many are interested in doing this…I’m still with it, I just took a break from it, because it can get very obsessive/frustrating when you don’t get the results you are looking for. Especially when you’re not that great at coding in particular…like me. I will be getting back to building my segway very soon and will implement the bluetooth control as well! I will post a video showing it soon.

    Sorry I didn’t get back to you, I haven’t been checking this site for a little while…resting my brain. I will post my final code and all that when I get everything figured out.

  385. March 9th, 2013 at 15:25 | #385

    @Shane Pentz
    Good to have you back. We will publish some great news hopefully this week that might interest you πŸ˜‰

  386. Abdullah
    March 14th, 2013 at 15:18 | #386

    is it possible to get connection schemes for all system?

  387. March 14th, 2013 at 18:28 | #387

    No, but it should be pretty easy to connect everything.
    Read some of the previous comments, I believe I wrote it at some point.

  388. flyrobot
    March 20th, 2013 at 18:08 | #388

    Hi Thomas,

    Your balanduino is really interesting, but you see this and the guys who made the board is from your country name jussi

    My point here is, your balanduino is almost using the same component and same basic principal as the gimbal project. why dont you made the code for both small segway robot and for brushless gimbal using this board. I dont know what motor type you are using for H-Bridge on this board, i meant, is it 2 phase motor or 3 phase motor, (they do some re wounded 3 phase motor to fit the gimbal use, some of them already sell it at this ).
    FYI Now the brussless motor gimbal become very popular in FPV market and im waiting to have too πŸ˜‰


  389. March 20th, 2013 at 18:14 | #389

    We use a motor controller intended for normal dc brushed motors. So you can’t easily just connect a brushless motor, but I agree that you could still use the main board for other applications if you want to.

  390. flyrobot
    March 21st, 2013 at 18:14 | #390

    Hi Lauszus,

    I was thinking the brussless motor gimbal has a quite big market from amateur video/photographer up to professional. and i see you guys has capability to be one of other players. just an idea, how about this board split in two part cpu+acc+gyro and motor controller (has option for brushless 3 phase RC motors and normal dc brushed motors). actually the idea is, i like to have both balanduino and brushless motor gimbal camera. but why should i bought two, if the the main board is the same and im sure i will bored play the balanduino within 1 or 2 month then it will become wasted, but if the board can be used as gimbal too, it will be very useful. Im sure many multi rotor for FPV or Aerial video/photo will get additional advantage if they are buying your board.



  391. getSurreal
    March 22nd, 2013 at 00:10 | #391

    You mention how you would like to convert to using pulleys. I was picking out all the parts I think I would need from servo city and it ended up being over $100US. Do you know of a better/cheaper source than servo city?

  392. Barry
    March 22nd, 2013 at 11:59 | #392

    Hi Lauszus,

    my first prototype is ready, but i will (most likely) not use encoders. Could you indicate what i have to change in your code to make this work?


    Also: looking at my proto, you see that im using small wheels (in this version i want them to be 20mm diameter max, the next version, which will be half scale of this one has max 10mm wheels). Were in the balancing code does this affect?

    Thankx in advance!


  393. Barry
    March 22nd, 2013 at 12:01 | #393


    Hi Sergey,

    can you tell me what you did to make your version work without using Encoders?

    regards Barry

  394. March 22nd, 2013 at 14:01 | #394

    Thanks for your feedback, I will keep that in mind.

    I’m unsure if you were asking me in your comment? But what do you need servos for anyway? I usually buy my parts online.

    You should put the accelerometer and gyroscope in the bottom of the robot, also you are properly going to put some weight on top – I used the battery for that.
    Regarding the wheels: you properly only need to change the scale values here:

    Btw. have you guys seen the Balanduino kit I just released: and here is the link to the Kickstarter: I would appreciate if you could share it with your friends and colleagues, so we can reach the funding goal.

  395. Barry
    March 22nd, 2013 at 14:06 | #395

    Hi Lauszus,

    thanks for the fast reply. I will re-arrange my robot so the battery is more on top and the acc&gyro to the bottom. However: i still do not understand how your code should work if im not using any encoders..

    Nice project on kickstarter! I will share it with people i know!!

  396. March 22nd, 2013 at 14:16 | #396

    Simply remove all the parts related to the encoders. The main purpose of the encoders i actually to stop the motor from keep driving forward when you let go of the joysticks.
    I could actually drive the robot around before I implemented the encoders, so just need to spend some time tuning the PID loop.
    You can check out this early version of the robot: where I haven’t implemented the encoders.

    Thanks. I really appreciate it!

  397. getSurreal
    March 22nd, 2013 at 18:12 | #397

    Sorry I wasn’t clear. I’m not asking you about servos, I’m asking you about pulleys. You post mentions them as one of your ways to reduce backlash. I was curious if you had researched where to buy the pulleys. Servo City has pulleys but are pretty expensive to get pulleys with pillow blocks and rods and hubs.

  398. March 23rd, 2013 at 13:51 | #398

    No I’m afraid I don’t know any good sources for pulleys.

  399. getSurreal
    March 30th, 2013 at 20:46 | #399

    How stable are your pitch readings? I’ve tried several different filters, but with the device sitting flat on the table the best I’m able to get is fluctuations +/- 0.5 Maybe that’s good enough and I just have to work on my PID.

  400. WilcovT
    April 2nd, 2013 at 18:16 | #400

    Hi Kristian,

    I decided to give the balancing robot a try, and downloaded the code from github, but i cant get it compiled, since im new to arduino libs, thats where the trouble is i think.
    I just have to put the master library in the library folder, and point to the file?
    Shouldnt there be a *.ccp file with the *.h file?
    Im a bit confused… can you shed some light? Did everything as explained in the official arduino explanation on the arduino site, but keep getting that usb does not name a type.

  401. April 2nd, 2013 at 22:37 | #401

    Check out the readme: it describes how to add the library.

  402. April 2nd, 2013 at 22:48 | #402

    No mine is more like +/- 0.05 degrees – I just checked with my Balancing robot that uses a MPU-6050.

  403. WilcovT
    April 4th, 2013 at 19:17 | #403

    Dear Kristian,

    Now i got it, had a typo, after some days with little sleep, looked over a period. But when i compile it says:
    In file included from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/Usb.h:40,
    from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/BTD.h:21,
    from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/SPP.h:21,
    from BalancingRobotArduino.ino:16:
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/message.h: In function ‘void ErrorMessage(const char*, ERROR_TYPE)’:
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/message.h:48: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/message.h:50: warning: only initialized variables can be placed into program memory area
    In file included from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/BTD.h:21,
    from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/SPP.h:21,
    from BalancingRobotArduino.ino:16:
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/Usb.h: At global scope:
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/Usb.h:168: warning: ‘packed’ attribute ignored
    In file included from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/BTD.h:22,
    from C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/SPP.h:21,
    from BalancingRobotArduino.ino:16:
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h: In member function ‘void ConfigDescParser::PrintHidDescriptor(const USB_HID_DESCRIPTOR*)’:
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:176: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:177: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:180: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:183: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:186: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:189: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:201: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:204: warning: only initialized variables can be placed into program memory area
    C:UsersWilcoDocumentsArduinolibrariesUSB_Host_Shield_201/confdescparser.h:207: warning: only initialized variables can be placed into program memory area
    BalancingRobotArduino.ino: In function ‘void loop()’:
    BalancingRobotArduino:115: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:115: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:115: error: invalid use of void expression
    BalancingRobotArduino:117: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:117: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:117: error: invalid use of void expression
    BalancingRobotArduino:119: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:119: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:119: error: invalid use of void expression
    BalancingRobotArduino:121: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:121: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:121: error: invalid use of void expression
    BalancingRobotArduino:128: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:128: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:128: error: invalid use of void expression
    BalancingRobotArduino:130: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:130: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:130: error: invalid use of void expression
    BalancingRobotArduino:132: error: invalid conversion from ‘int’ to ‘char*’
    BalancingRobotArduino:132: error: initializing argument 2 of ‘void SPP::doubleToString(double, char*, uint8_t)’
    BalancingRobotArduino:132: error: invalid use of void expression
    And in an orange banner it says : invalid conversion from int to char and highlighting :

    if(SerialBT.connected) {
    if(sendPIDValues) {
    sendPIDValues = false;
    —> strcat(stringBuf,SerialBT.doubleToString(Kp,2)); //here it begins with an error
    dataCounter = 1;
    } else if(sendData) {
    switch(dataCounter) {
    case 0:
    if(dataCounter > 4)
    dataCounter = 0;

    What to do?

  404. April 4th, 2013 at 19:25 | #404

    The doubleToString function has changed since I wrote the code. You can simply download this version of the USB Host Shield library: – this is the version that will come of if you click at the letters next to the submodule.

    If you want you can update the code and send a pull request – see this line on how to use the function after the update:

  405. WilcovT
    April 4th, 2013 at 19:44 | #405

    Thanks Kristian, that made all the difference!!! Just some memory allocation errors and im good to go!!!!
    Thank you for replying so fast, no i wont think about it all night haha.

    greetings Wilco

  406. WilcovT
    April 4th, 2013 at 19:48 | #406

    And only trying to get the mpu6050 in the code

  407. April 4th, 2013 at 20:01 | #407

    You are welcome πŸ™‚ You can just ignore all the warnings that says: “only initialized variables can be placed into program memory area”.

    You should check out the MPU-6050 example I wrote: We will also publish the code for our Balancing robot: in two weeks – it got several improvements and uses the MPU-6050.

  408. WilcovT
    April 4th, 2013 at 20:22 | #408

    I read about the robot kit, and looks very promising, when it is going to be available, i will order a kit, i am trying to teach my nephew some arduino and want him to have fun, and although i am in the car restoration bussines, it is fun for me too.
    I tried the sketch, and it worked, and that i have to implement in the arduino robot sketch, and replace all the analog gyro lines with the example sketch. Sorry for asking so much, but im still learning and in the spare time i have left, im learning code.

    greetings Wilco

  409. April 4th, 2013 at 20:25 | #409

    It is available – you simplify order it through Kickstarter πŸ™‚

    Okay. No problem, you can ask as much as you like!

  410. Wilco van Toorn
    April 18th, 2013 at 10:31 | #410

    Hey Kristian, congrats on the kickstarter. I could not get a kickstart set, due to the fact that i dont have a credit card… But in the future i will, and then there will be a set for me. Right now it will be a arduino board and all the other things seperate.
    But i have a question, has there be much to be adapted to use a Due, or are those minor changes to the software?
    I just got a Due and a sabertooth 2×25 for my birthday and want to port the code to the Due, but was wondering if i need a seperate shield for the bluetooth( due has usbhost) and if the code needs changes)

    Greetings Wilco

  411. April 19th, 2013 at 00:26 | #411

    @Wilco van Toorn
    Okay no problem πŸ™‚ You can’t easily use an Arduino Due, as you can’t use any of the USB Host libraries with it easily, as it’s not an AVR, but an ARM processor.

    But you could try to make a balancing robot to start with and then add remote control of some sort afterwards. It shouldn’t be that hard to port the balancing part of the robot to a Due as most of it’s the same. The major difference is in this file:

    You can’t use the EEPROM functions we use either, as they are for AVR’s only, but just remove them for now.

  412. YoBe
    April 21st, 2013 at 08:57 | #412

    @Lauszus: hi I am also working on my robot. I liketo ask 2 questions:
    1. When you let the robot spin (right forw/left rev), will it stay stable for long time?
    2. When you push the robot out of position, it returns. But what part of the sofw prevents it from overshouting when moving back to pos? This part steers it back to position but where is the “braking” part?
    if (abs(positionError) > zoneA) // Inside zone A restAngle -= (double)positionError/positionScaleA; else if (abs(positionError) > zoneB) // Inside zone B restAngle -= (double)positionError/positionScaleB; else // Inside zone C restAngle -= (double)positionError/positionScaleC; restAngle -= (double)wheelVelocity/velocityScaleStop;

  413. April 22nd, 2013 at 20:15 | #413

    1. Yes it will stay upright until the batteries runs out!

    2. This is the braking part:

  414. YoBe
    April 23rd, 2013 at 04:08 | #414

    @Lauszus: thanks for the reply and also GREAT job done on your robot.

    At the moment my robot is balancing quite good but during spinning it starts getting out of ballance. My IMU is mounted on the side of the robot. Perhaps this is the problem. I should move it to the center…

    I started my balancing robot by inspiration of “n-Bot”. I can only work on it during my spare time so progress is not so fast. But I need to say it takes quite some time to get it up and running.
    I use the Madgwick filtering. Perhaps Kahlmann is better, need to evaluate.
    I read you have pitch accuracy around 0.05deg (but this is with the latest IMU I think) I still use an 3G accelometer with analog outputs…perhaps need to change to SPI comm.

    On my motorcomtroller I need to have minimun 25 on the PWM value to make the motor run. So I always add this offset. You do not need to do this I see. Of course your motor regulater is much better. (I still use LM298…)

    At the moment I live in China and it is really fun to buy all these electronic stuff at cheap prices.
    My next steps are now improving the spinning, encoder regulation and implementation of a PS2 controller.
    Congrats with the Kick starter project!

    PS: is this part also not slowing down the robot when steering back to original position?: “restAngle -= (double)wheelVelocity/velocityScaleStop;”.

  415. April 23rd, 2013 at 10:58 | #415

    Okay, yes try that and see if it help. Put it as near to the rotational axis as possible.

    Yes it takes much more time than most people think πŸ™‚ Also by using a digital IMU you will typically get higher resolution too – for instance the MPU-6050 we use for the Balanduino (see: and has a resolution of 16bits while the ADC on the ATmega328P used on the Uno only has a resolution of 10bits.

    I would love to shop all my parts local so I don’t have to wait for them to arrive and I can get them in my hands before buying them – but almost everything electronics wise is manufactured in China so it’s not possible here in Denmark πŸ™‚

    Okay. Let me know your progress and I will be happy to help you out!

    Yes this line: will slow down the robot depending on the velocity of the wheels, this helps slow down the robot, so it never goes to fast.

  416. YoBe
    April 24th, 2013 at 05:47 | #416

    @Lauszus: Thanks for the tip on the IMU. Here in China: 4euro (incl. transport) for this part on a breakboard. Will order it tonight. I Love China for this…

  417. April 26th, 2013 at 13:22 | #417

    Okay. Actually my teammate Thomas is in China right now. Last time I spoke with him he was going to Shenzhen, so he is properly spending all his money on cheap electronics right now πŸ˜‰

  418. Ryan Lush
    May 1st, 2013 at 02:09 | #418

    Hey bud, how are you? You put that money I sent you to good use?

    So for the longest time I thought my problem was the gyro (mind you I’ve only spend about 2 hours on this thing since we last talked). Today I decided I would focus on the accelerometer and see if I could find anything. I realized I had the sampling frequency on the accelerometer set to 100Hz even thought I was reading it about about 400Hz. Problem fixed. That was my problem all along.

  419. May 1st, 2013 at 13:18 | #419

    @Ryan Lush
    Hi πŸ™‚
    Yes I bought a Sanguino, so I could test the ATmega644A we now use for our Balanduino project:!

    Good to hear you solved it! Are you planning to upload a video of it or something like that? πŸ™‚

  420. kim
    May 14th, 2013 at 05:45 | #420

    i can see the result of angel graphic measured by MPU6050 with Kalman filter in your bot? Because i feel my angle is not good so i want to see your angel graphic to compare them.Thank!

  421. ROY
    May 15th, 2013 at 11:17 | #421

    CÒn you tell me how do you calibrate mpu6050 and how do you get the stable angel from mpu6050.It’s better ì you give me your code.Please reply early ! sorry for my disturbing and thank very much.:)

  422. May 15th, 2013 at 19:23 | #422
  423. ROY
    May 17th, 2013 at 17:13 | #423

    I wonder about your power supply for your bot.You use battery as a common source to supply control board and motor or one source for control board and the other for motor ?.thank you?

  424. Viv
    May 23rd, 2013 at 07:18 | #424


    Great job on your Balanduino.
    I’m working on one myself. Just one question tho: why did you add counts from both encoders to get the wheel position?
    I’m a bit confused because if we add counts from both encoders, and then use it to calculate the wheel velocity, then the wheel velocity would not be for each wheel right?

  425. Viv
    May 23rd, 2013 at 07:48 | #425

    And al

    Viv :
    Great job on your Balanduino.
    IΓ’??m working on one myself. Just one question tho: why did you add counts from both encoders to get the wheel position?
    IΓ’??m a bit confused because if we add counts from both encoders, and then use it to calculate the wheel velocity, then the wheel velocity would not be for each wheel right?

    And also, I noticed in the header file of balanduino, you didn’t initialize most of the boolean variables, how do we know if it’s true or false then when the program starts?

    Thanks a bunch, I look forward to your reply! πŸ™‚

  426. May 23rd, 2013 at 16:30 | #426

    It doesn’t matter, but in the future I will work on using them separately so it will be possible to get even better performance.

    The boolean values will be set to false by default πŸ˜‰

  427. ROY
    May 25th, 2013 at 14:21 | #427

    I have a problem with my bot.My bot is never balance although i spend many time to tune PID parameter.I think my code is not good.Here my code PID:
    #define LIMIT 20

    double K = 1;
    double PIDValue;
    double Kp = 1;
    double Ki = 0;
    double Kd = 0;
    int last_error = 0;
    double integrated_error = 0;
    double pTerm = 0, iTerm = 0, dTerm = 0;

    int Pid(float targetPosition, float currentPosition) {
    error = targetPosition Γ’?? currentPosition;
    pTerm = Kp * error;
    integrated_error += error;
    iTerm = Ki * constrain(integrated_error, -LIMIT, LIMIT);
    dTerm = Kd * (error Γ’?? last_error);
    last_error = error;
    PIDValue = pTerm + iTerm + dTerm;
    PIDValue = constrain( PIDValue,-255,255);
    return -PIDValue;

    IN my code , i use analogWrite function to set PWM. My motor dont have encoder so i dont add PID for wheel you have any opinion abot that?
    Thank you very much!
    Best regard to you!.

  428. May 25th, 2013 at 14:31 | #428

    Your PID loop works fine, but I would change these lines:

    integrated_error += error;
    iTerm = Ki * constrain(integrated_error, -LIMIT, LIMIT);


    integrated_error += error;
    integrated_error = constrain(integrated_error, -LIMIT, LIMIT);
    iTerm = Ki * integrated_error;

    This will insure that the value is saved, so it will have effect the next time you run the loop.

    Also cast you int variables to double every time you do some calculations.

    You won’t get that good performance without encoders. You will basically only make it able to balance in the same spot. You won’t be able to drive it around.

    I must have overlooked your old reply. I only use one battery.

  429. Anakill
    May 30th, 2013 at 08:29 | #429

    Hello @Lauszus

    First of all, thank you very much for your wonderful work on this balancing robot; I’m trying to do my own (for my exam in France), and it inspires me a lot.

    I just need the Arduino code for this sensor : IMU Digital Combo Board – 6 Degrees of Freedom ITG3200/ADXL345 ; I can’t programm in Arduino so please help me I’ve my exam in two weeks.

    Thank you very much.

  430. May 30th, 2013 at 09:57 | #430

    Here is some code I wrote for that particular combination: πŸ™‚

    To be honest it is quite a task you are up to if you can’t even write code?

  431. Anakill
    May 31st, 2013 at 21:16 | #431

    Thank you very much for your prompt answer. It will help a lot.

    Now the issue is the motors control (I use the same motor Driver as yours : Dual VNH2SP30 Motor Driver Carrier MD03A) using the data coming from the sensor.

    How do I get all the programs to put in my Arduino to make the robot work ? (I don’t need to wireless control, just maintaining balance).

    I’m eighteen, in my last high school grade. For the last exam I have to do a project. When I proposed your balancing robot to my teacher he was enthusiastic but when I asked him some help for teaching to me Arduino code he answered he knew nothing in Arduino and no one could help me. I feel alone and a bit desperate; that’s why I address you.

    It’s very nice to help me, sorry for my frenglish.

  432. May 31st, 2013 at 21:42 | #432

    All the information you need can be found in the Balanduino code:
    Simply comment out all these lines: – this will disable all remote control.

    You will need to change all the lines to reflect your setup:

  433. Asen
    July 10th, 2013 at 12:29 | #433

    ı need circuit diagram this system.

  434. July 10th, 2013 at 12:46 | #434

    It’s not available as it’s pretty easy to connect the break out boards to each other – see the different product pages for more information πŸ˜‰

  435. kim
    August 26th, 2013 at 18:16 | #435

    How do you write app for your android to control your bot?

  436. August 26th, 2013 at 18:19 | #436

    The application is written in Java. You can find all the information you need to get started here:

    And here is the source for the app:

  437. kim
    September 1st, 2013 at 18:20 | #437

    helllo Lauszus!
    your BalanduinoAndroidApp can open in eclipse or android studio?
    And can you tell me how open this project?
    Thank you!

  438. September 1st, 2013 at 19:01 | #438

    It’s intended to be used in Android Studio by using Gradle.
    You can find more information about opening it here: Basically you select “Import” in the menu and then select the “build.gradle” file.

    Also note that you must use the “recursive” command when cloning the repository in order to clone the submodules as well. Here is the command you need to run:

    git clone --recursive
  439. kim
    September 6th, 2013 at 15:47 | #439

    Hi Lauszus!
    I do as you say me but it can’t your project!
    it notice:
    Could not fetch model of type ‘IdeaProject’ using Gradle distribution ‘’.
    A problem occurred configuring root project ‘BalanduinoAndroidApp-master’.
    A problem occurred configuring root project ‘BalanduinoAndroidApp-master’.
    Failed to notify project evaluation listener.
    Configuration with name ‘default’ not found.
    can you give any opinion?
    thank you!

  440. Victor
    September 7th, 2013 at 01:08 | #440

    I have several questions about the balanduino. Are the Pololu DC motors and motor driver given in the links above the same ones that will be shipped with the Balanduino? How much will the balanduino kit cost? Sorry if this information was already made available but I can’t find it anywhere.

    Also, how do you determine the max RPM and torque required from the motors to balance the robot? Or did you just go for the higher performance ones just in case? I have 75 max RPM motors but I’m not sure it will rotate fast enough to balance the robot with any significant deviation from vertical.

  441. September 8th, 2013 at 12:53 | #441

    What OS are you using? And also make sure to use the newest version of Android Studio. Also try to install Gradle manually and see if you can build it using the terminal.

    It’s not the Pololu motors, but some very similar with similar specs etc. The full kit costs $299. You can find more information about on the Kickstarter: and the homepage:

    I knew that the Pololu motors worked, so we simply found ones with similar specs. 75 rpm is too low for a balancing robot. For comparison the ones we use now are 350 rpm.

  442. sir66
    September 11th, 2013 at 19:36 | #442

    I tried to preorder kit on site, bit can not find the position. It is not available yet?

  443. September 11th, 2013 at 21:46 | #443

    No it’s not available yet. We need to ship out all the kits for the backers before we will open up for preorders. Please send me an email at, then I will let you know when we are ready for preorders.

  444. September 15th, 2013 at 12:03 | #444

    Try to download the newest version of the code. I just updated it, so it should be easier for other people to compile the Android app.

  445. September 20th, 2013 at 12:38 | #445

    I just wanted to inform you that it is now possible to sign up for reservation of the Balanduino at the homepage:

  446. mao007
    September 26th, 2013 at 22:18 | #446

    hola! muy buen trabajo.
    estoy desarrollando un control de equilibrio sobre dos ruedas (pΓ©ndulo invertido), estoy usando el mismo sensor, el MPU 6050 y una nexys 2 que incluye una spartan 3e, soy novato y necesito ayuda para poder comunicar los dos dispositivos y poder generar el codigo para la lectura del acelerometro y giroscopio que el MPU trae.

    muchas gracias a todos, especialmente a quien me pueda ayudar

  447. September 26th, 2013 at 22:21 | #447

    You should check out this code I wrote for the MPU-6050, that should get you started:
    After that check out the code for the newest version of the robot here:

  448. mao007
    September 26th, 2013 at 23:14 | #448

    gracias @Lauszus, pero mi pregunta es si con el cΓ³digo que usted creo, es posible aplicarlo a una Nexys 2?, es una FPGA que contiene una spartan 3E. le agradezco la ayuda

  449. September 27th, 2013 at 02:06 | #449

    Yes it is possible, but I haven’t got time to look into it right now, sorry. Do you have a I2C implementation already or do you need to write it yourself?

  450. mao007
    September 27th, 2013 at 04:08 | #450

    No aun no tengo la aplicacion I2C, estoy en la etapa de prueba, estoy mirando como puedo conectar el sensor MPU 6050 con la nexys 2, pues necesito traducir los datos del sensor para poder generar la rutina de control.

  451. September 28th, 2013 at 18:08 | #451

    Okay. I recommend taking one step at a time. First create the I2C library, then make a basic example using the MPU-6050, this will make it easier for you πŸ™‚

  452. kim
    October 8th, 2013 at 18:24 | #452

    how you control your bot remain balancing when it move?
    thank so much!

  453. October 8th, 2013 at 18:55 | #453

    I describes how it works in the video in the blog post.

    To drive it forward and backwards I simply set an offset angle – see:

    To turn the robot it I add a value to one of the motors PWM value and subtract the same value from the other motors PWM value – see:

  454. kim
    October 9th, 2013 at 18:14 | #454

    Thank for your answer!
    i’m using motor without encoder.I can do my robot balance with this motor.But i wonder that my robot can remain balance when it move if i use that robot?
    What do you think about that problem?

  455. October 9th, 2013 at 18:16 | #455

    You should take a look at the first version I ever made of a balancing robot: It doesn’t uses any encoders. It was only able to balance, but it would quickly fall over if it was pushed or the surface wasn’t level.

  456. kim
    October 10th, 2013 at 19:35 | #456

    do you use 2 wire A and B of encoder or one?
    Can using one wire of encoder to control robor?

  457. October 10th, 2013 at 22:38 | #457

    You will need to use both if you want to determine the direction, so no you can not just use one.

  458. @100892
    October 12th, 2013 at 17:43 | #458

    Hello Lauszus,
    Can you give your PID constants?

  459. @100892
    October 12th, 2013 at 17:48 | #459

    Can you also please tell me about the accuracy of tilt measurement u were getting ?

  460. October 12th, 2013 at 22:34 | #460

    The default PID values are defined here in the Balanduino code:

    It is pretty accurate. Of course it is not as precise when there is a lot of noise ie. when the robot is moving.

  461. kim
    October 31st, 2013 at 18:01 | #461

    Hi Lauszus!
    When you test your motor, the speed of motor is linear with PWM value?
    thank you!

  462. November 1st, 2013 at 14:32 | #462

    Yes it is pretty close πŸ™‚

  463. kim
    November 3rd, 2013 at 12:34 | #463

    HI Lauus!
    in your code, if you don’t control you robot, your controller will be a normal PID controller.And encoder will not be used in this case.That is right?
    Sorry ,if i wrong!
    thank Lauszus.

  464. November 3rd, 2013 at 23:49 | #464

    No. The encoders are also used to keep the robot staying in the same position. I explain it in the video.

  465. kim
    November 4th, 2013 at 14:28 | #465

    Hi Lauszus!
    In this your code:
    if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft);
    moveMotor(left, backward, PIDLeft * -1);
    if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
    moveMotor(right, backward, PIDRight * -1);
    you control direction motor based on PID values.What do you think about that if i replace “PID value” by “error” to control direction. It mean:
    if (error >= 0)
    moveMotor(left, forward, PIDLeft);
    moveMotor(right, forward, PIDRight);
    moveMotor(left, backward, PIDLeft * -1);
    moveMotor(right, backward, PIDRight * -1);
    And can you tell me Why you use PID values to determine Direction of motor ,not error?

  466. November 4th, 2013 at 14:49 | #466

    The PID values is the output to the motors. -100 means full speed backwards and 100 means full speed forward. You can’t not use the error in the PID controller for that. Note that the error might be negative, but the both PID values might both be positive, as the PID values depend on other things than just the error term.

  467. kim
    November 4th, 2013 at 18:43 | #467

    Thank for your answer.
    I have another question.
    You use 2 wire of encoder to determine direction of motor.In 2 this wire,you use one to connect a interupt pin and other to connect digital input.When interupt occur , you will check logic level on encoder digital input,if it is 1, counter increase and otherwise.I say right?
    unfortunately ,my encoder have one wire encoder but i have one idea.That is: i can determine
    direction based on PWM value because When PWM value is positive,motor run forward and when it negative, motor run backward so i know direction of motor.Anything opinion about that idea, Lauszus?.
    And finally,i thank you for all thing you help me!

  468. November 4th, 2013 at 22:37 | #468

    No it does not work that way. First I setup the pins to interrupt on any edge:

    And here is where I read the pins: Note that I compare them, so if each are the same it much be running one way and if they are different it must be running the other way.

    Note that I reed directly from the port registers, as this is faster. The macros are defined here:

    No you can not determine direction with only one pin, but yes you can try to use your approach, but it is really not ideal!

  469. kim
    November 5th, 2013 at 09:48 | #469

    Lauszus, your robot can balance on a inclined flat or when center of robot is changed?
    Thank you!

  470. November 5th, 2013 at 09:53 | #470

    It is not that good on an inclined surface, but it handles if it off balance pretty well. Just check out this video where I tried mounting an GoPro on it:

  471. kim
    November 5th, 2013 at 13:01 | #471

    Hi Lauszuz!
    Your bot has one supply from battery?
    If it right, do you ever think about that inverse current from motor can destroy other component such as IMU,Arduino?
    thank Lauszus!

  472. November 5th, 2013 at 13:15 | #472

    Yes I have only one supply.
    Yes I know, but it is not necessary for the Balanduino, as the motor driver takes care of that. You can see the schematic here:

  473. Dave
    November 15th, 2013 at 06:51 | #473

    Hi lauszuz,
    i am currently building my own self balancing robot but i am having difficulty in writing the code for the calculation of the wheel-position and wheel-speed. i am using arduino nano for the calculation. do you have the code for reference purpose??

  474. November 15th, 2013 at 15:19 | #474

    First you need to setup the encoder pins and enable the interrupt:

    Every time a interrupt occur the following function will be run:
    I read directly from the port registers to save processing power. Here are the defines:
    Since you are using an Arduino Nano, you should check out the following page: This shows which pins are connected to which ports.

    Finally I simply read the encoder values using the following functions:

    It is very important that you set the variables to volatile that you use in the interrupt – see:

  475. Kim
    November 18th, 2013 at 17:01 | #475

    Nice to see you,Lauszus!
    I have made by myself an encoder.But it only provides a resolution of 20 counts per revolution of the motor shaft which corresponds 360 counts per revolution of the gearbox?s output shaft(gearbox 1:18).Do you think that is problem with encoder has small count.
    Thank you!

  476. November 18th, 2013 at 17:05 | #476

    I think that should be good enough. Just try it and see if it works πŸ™‚

  477. Dave
    November 21st, 2013 at 10:01 | #477

    Thank you so much Lauszus!
    Those information really helps me to understand a lot more. however there is one part i dun understand. the part when you are talking about port register. what does _BV(PIND2) means? how does it work?

    #define leftEncoder1Port PIND
    #define leftEncoder1Mask _BV(PIND2)
    #define leftEncoder2Port PINA
    #define leftEncoder2Mask _BV(PINA6)

    #define rightEncoder1Port PIND
    #define rightEncoder1Mask _BV(PIND3)
    #define rightEncoder2Port PINA
    #define rightEncoder2Mask _BV(PINA7)

  478. November 22nd, 2013 at 04:23 | #478



    Is the same as

    (1 << PIND2)

    It would be a good idea to read the following page as well:

  479. jb
    November 25th, 2013 at 12:12 | #479

    Hello Kristian
    Your new robot Balanduino is really very nice. I’m very impressed. I’m building the balancing robot too. It works fine. But I have problems with remote control. I found Your android application. And I’m trying to use it. Turning left and right work fine. But unfortunately moving forward and backward don’t. When I want to move for example forward robot tries to move and suddently it stops. And this repeats all the time. It is very nervous. Maybe do You know what can be wrong? For remote control I use part of Your code.
    I’ve got another question. Why do You devide D constant by 100 and multiply I constant by 100? And how quickly does Your main loop execute? And last question. PID values should be only cardinal numbers or I can try to add some fraction? For example P = 7.65; I = 1.15 …
    Thank You

  480. November 26th, 2013 at 23:32 | #480

    Are you sure that you sending the data continuously? It sounds like it sends a stop command sometimes. It easier to debug if you connect it to your PC and then print out all the incoming data. Or simply run this example: – this will print it as well.

    Can you post your code somewhere and then I will have a quick look.

    I simply divide/multiply it by 100, so the values get in the same range πŸ˜‰

    My main loop only takes 3-4ms.

    Yes PID values can be fractions as well.

  481. kim
    December 6th, 2013 at 10:40 | #481

    HI Lauszus!
    have you connect module hc05(master) with module hc06(slave).I can connect module bluetooth hc05 with PC or phone but I can’t connect 2 module bluetooth.

  482. December 6th, 2013 at 13:10 | #482

    No I have not tried them. Try to Google it, I’m sure that something will come up πŸ˜‰

  483. A.J.
    December 13th, 2013 at 06:49 | #483

    Hey Lauszus,
    I’m making a balancing robot for my senior design project. I’ve built it, smoothed the incoming data, and written the PID code. However, the robot is quite unstable and definitely won’t balance on its own. I’m using the same motors/driver/wheels as you but am using only the Arduino Mega to do everything. I’ve adjusted the PID constants quite a lot to no avail. My code isn’t efficient (I don’t write/read directly from ports,etc.), so could it be that my loop time is too long? I want to say it’s around 80ms last time I checked. One other fatal issue is that, sooner or later, the ‘output’ variable (mapped/constrained to be PWM value) begins streaming ‘nan’ and my wheels go full speed in one direction. It’s like it’s overflowing, but it shouldn’t since it stays in the range of -80~80ish. Any thoughts? Thanks.

  484. December 13th, 2013 at 11:51 | #484

    80ms is waaay to long! I just measured the Balanduino and each loop only takes 3.5ms and it also have to handle all the USB communication with the Bluetooth dongle etc.

    Can you post your code somewhere? Then I will take a look at how you map the values.

  485. A.J.
    December 13th, 2013 at 23:14 | #485

    I thought so. I hope I’m remembering incorrectly then. I’m not actually implementing any Bluetooth control or anything. Just trying to get it to balance. I’m even doing all my debugging through a USB, which is of course a pain. My email is If you could send me an e-mail just so I can attach my code and send it that way, I think it would be easiest. It’s fairly messy and long, mostly due to the code to get the raw data for my IMU (I just copied it from somewhere online).

  486. A.J.
    December 14th, 2013 at 00:12 | #486

    Okay, so I realized that I had a bunch serial.Print() statements that were affecting my loop time in a huge way. Before taking them out my loop time was around 80 ms. Now it’s about 15 ms. That’s way way better, and my robot seems to be jittering around the setpoint whereas it oscillated widely before. The “overflow” issue still occurs, and happens sooner too. After taking out the serial.prints, it happened almost instantly after upload, but I added a delay thinking it had to do with the data communication somehow. This actually made it a little better, but it overflows pretty quickly regardless. It’ll TRY to balance (better then it did before btw) for a few seconds, and then the motors go full speed in one direction.

  487. A.J.
    December 15th, 2013 at 21:05 | #487

    I keep making progress before you even get a chance to answer lol, but I’ll list the updates for when you come across my comments. I did some research to find out what the ‘overflow’ issue was all about. Something I read made me try to speed up the communication with the IMU using “TWBR = 12;” after the wire.Begin() command. This is supposed to speed up the communication rate from 100 Hz to 400 Hz. Now I don’t have this issue anymore, unless I set my Kp really high or something. So that’s good. Now I just need to figure out the tuning. The robot doesn’t respond to the tilt angle as quickly as it needs to, but when I increase Kp, it jitters quite hard around the set point. Motor backlash is a real killer here also, as it causes more instability. I sent my first code to the support address, but I’ll send the most recent one as several things were changed. Thanks.

  488. December 15th, 2013 at 22:34 | #488

    Nice idea about increasing the I2C frequency – I do not know why I have not though of that πŸ™‚ But it is better to set it like so: instead of hardcoding the value.

    I have sent you an email. I will take a look at your code when you reply πŸ˜‰

  489. December 16th, 2013 at 00:23 | #489

    Hi Lauszus

    Both me and my son are embarking on making a balancing robot from your information.
    My question is as followes.
    do you have a wiring diagram for the equipment you specified in you articles, as it would be extremely helpful ?



  490. December 16th, 2013 at 09:41 | #490

    No unfortunately not, but you should take a look at the Balanduino PCB: – it shows how the MCU should be connected to the IMU, motor driver etc.
    You could also consider pre-ordering the main board: or even the kit.

  491. December 23rd, 2013 at 23:36 | #491

    Thank you Lauszus, i placed a pre-order request recently, just deciding on the full diy kit
    or just the PCB.


  492. December 24th, 2013 at 00:05 | #492

    Okay. If you got quality motors, wheels and everything else you need, then I would say that you should go for the PCB, but note that in my experience the quality of the motors, wheels and other hardware makes a huge difference! So if you want parts that you know will work, then go for the kit πŸ™‚

    Anyway it also depends on the goal of your project – do you want to build it from the ground up or want to hack/modify/improve an existing platform.

    One of my big hopes for the Balanduino – and one of the big reasons for making the kit – is that in collaboration with a lot of other people we will be able to great a even more impressive robot! Up until now I have been the only one working on the code, so I can’t wait to see what other people can come up with πŸ˜‰

  493. December 24th, 2013 at 02:23 | #493

    The idea of working up from components to a finished robot is the idea, your original robot with separate processors ect, seems a far better learning tool for my son. he as just started
    to study engineering and needs a project. And yours is by far the best we have found.
    Along with your blog and information which can only improve his knowledge.



  494. December 24th, 2013 at 14:53 | #494

    Okay I see you point. Would you be interested in buying a version with all you need except the main board? If there is a demand for it we might open up for that as well.
    It will consist of all the parts in the full kit except the main board, so people can use there own electronics.

  495. December 27th, 2013 at 21:07 | #495

    Happy New year kristian

    i have been looking at your balancing robot sketch, the IMU that you used is no longer available.
    would it be very difficult to use a MPU6050 imu as a substitute ?
    Would i just have include the code, or would it mean a major re-write of the whole balancing robot sketch ?



  496. December 27th, 2013 at 23:26 | #496

    You should use the Balanduino firmware: It uses the MPU-6050 and has several improvements compared to the old code.

  497. December 28th, 2013 at 14:55 | #497

    Thank you for the advice, so we can use a normal Arduino board, which do you recommend ?
    Along with a motordrive ect, would the pin-out be the same as for the old code, except for the encoder connections. I appreciate this cant be very entertaining as we are only just getting started but thank you for your help.


  498. December 28th, 2013 at 18:24 | #498

    Sorry Kristian, i meant to say IMU connections.


  499. December 28th, 2013 at 19:12 | #499

    I recommend the Arduino mega if you are going to use an Arduino, as you will run out of space on an ATmega328P based (Uno, Mini etc.) really quickly.

    I have actually made it really easy to change the pins recently: It might look a bit more complicated than using digitalWrite/digitalRead, but they advanced is that it is just as fast as reading and writing directly to the registers, but it is much easier to port to other boards than just the Balanduino main board.

    For the IMU connections you should connect the IMU to the I2C pins on the boards. It should be labeled in the Arduino – there is a reference here as well:

  500. Blanco Lucas Ezequiel
    December 30th, 2013 at 21:12 | #500

    Hello! with respect to code MPU6050 have a problem, my balancing robot needs an angle, but when I remove the code the angle I need not make sampling very boring. Aydar Could I find the problem? Sorry for my English and I am from Argentina. Sample Code:

    while(i2cRead(0x3B, i2cData, 14));
    accX = ((i2cData[0] << 8) | i2cData[1]);
    accZ = ((i2cData[4] << 8) | i2cData[5]);
    gyroY = ((i2cData[10] << 8) | i2cData[11]);
    accYangle = (atan2(accX,accZ) + PI) * RAD_TO_DEG;
    double gyroYrate = -((double)gyroY/131.0);
    gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
    kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
    timer = micros();
  501. December 31st, 2013 at 01:39 | #501

    @Blanco Lucas Ezequiel
    Sorry, but I don’t understand what you are trying to say?
    Anyway you will find the Balanduino code here: It is the newest version of the code and uses the MPU-6050, which you are trying to use, as indicated by your code above.

  502. Venugopal
    February 12th, 2014 at 11:34 | #502

    I am a final year student of Aeronautical engg,. from India
    I would be happy if i get an aid from you for doing a project on SELF BALANCING ABILITY OF THE AIRCRAFT using this method. kindly give me a reply Sir

  503. February 19th, 2014 at 16:49 | #503

    What do you got problems with? You should checkout the Balanduino repo: where you will find all the hardware drawings and firmware.

  504. March 10th, 2014 at 09:56 | #504

    Do you have the schematic of the robot circuit. I didnt see any schematic and block diagram of the robot. If it present, can you please inform me and give its link


  505. March 10th, 2014 at 22:38 | #505

    @Lentin Joseph
    You should take a look at the Balanduino schematic:

  506. March 25th, 2014 at 10:46 | #506

    I’m not sure why but thos weblog is loading very slow for me.
    Is anyone else having this problem oor is it a issue on my end?
    I’ll check back laater and see if the problem still exists.

  507. March 25th, 2014 at 17:53 | #507

    It might be slow at peak times, but in general it should respond pretty fast. Are you still experiencing problem?

  508. Warman
    April 13th, 2014 at 04:19 | #508


    A nice piece of work. I am curious to know about the PID controller architecture. Do you have any information about the controller design. (E.g: what is/are your reference commands? what variables are used to calculate the PID error terms? Inner/Outer loop architecture etc? Any information about the design of the controller for your specific robot would be of great help. Please let me know.


  509. April 17th, 2014 at 15:05 | #509

    You should take a the source code here:
    Currently I use kind of a PD controller for the encoders, the output of this is then used as the input for a second PID controller which outputs the PWM value.

    I did not calculate the PID values. I tuned it manually.

  510. MKB
    May 3rd, 2014 at 12:29 | #510

    Hello Lauszus!
    I bought a motor DC 12V speed load 220rpm weight 250g, current load 1200mA, you think that it has enough torque to robot balancing ? I try to look for value PID but my robot not balancing.
    Thanks you!

  511. May 3rd, 2014 at 13:20 | #511

    Please send me the link to the motors. I need to know the torque.
    But they sound like they would work.
    What code are you using?

  512. MKB
    May 3rd, 2014 at 18:09 | #512

    Torque : This is code:

    #define   GUARD_GAIN   20.0

    float K = 1.4;
    int   Kp =11;
    int   Ki = 0;
    int   Kd = 10;
    int last_error = 0;
    int integrated_error = 0;
    int pTerm = 0, iTerm = 0, dTerm = 0;

    int updatePid(int targetPosition, int currentPosition)   {
      int error = targetPosition - currentPosition;
      pTerm = Kp * error;
      integrated_error += error;
      iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
      dTerm = Kd * (error - last_error);
      last_error = error;
      return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);

    I was very impressed with your robot. Thanks you.

  513. MKB
    May 3rd, 2014 at 18:11 | #513


    int Drive_Motor(int PWM_val)  {
    if (PWM_val& > 255)
    if ((kalAngleY > 150) && (kalAngleY < 166))
      if ((kalAngleY > 166) && (kalAngleYsetPoint)
      analogWrite(PWM1, PWM_val);
      analogWrite(PWM2, PWM_val);
      digitalWrite(InA1, LOW);
      digitalWrite(InB1, LOW);
      analogWrite(PWM1, PWM_val);
      analogWrite(PWM2, PWM_val);
      digitalWrite(InA1, HIGH);
      digitalWrite(InB1, HIGH);
      analogWrite(PWM1, 0);
      analogWrite(PWM2, 0);
      digitalWrite(InA1, HIGH);
      digitalWrite(InB1, HIGH);
  514. slo3
    May 28th, 2014 at 05:13 | #514

    Great site and thanks for sharing the code… but I’m a bit confused about how everything “fits together”… What code runs where?
    The mbed code runs on the mbed (or a second Arduino in the updated version?)
    But does that mean that the Kalman filtering code runs on an Arduino alongside the USB shield and the PS3 control input? How? I’ve only ever run a single piece of code on an Arduino at once… What am I missing?

  515. June 6th, 2014 at 09:25 | #515

    In this video demonstration all the real-time control and regulation (stabilization) of the robot is done with the Arduino. The mbed is only used for the USB control with the PS3 controller thru Bluetooth.

    In our new version, the Balanduino, the main board takes care of all this in one single processor, an ATmega1284P. This Arduino compatible board includes both the regulator design and the USB Host control application, embedded into one system and one project.

  516. July 10th, 2014 at 19:48 | #516


    I am a high school student who is building this self-balancing robot, and I was wondering for the Kalman filter Arduino library, how did you calculate the GYRO_SCALE and ACC_SCALE? My raw gyro values are fairly small, and dividing them by the even smaller GYRO_SCALE makes them very large and noisy. Where should I look on the IMU’s data sheet? Thank you very much!


  517. July 11th, 2014 at 23:58 | #517

    You should read my guide here:,58048.0.html. I believe it will answer your questions.

  518. Jan
    August 14th, 2014 at 15:06 | #518

    Hi Lauszus, thanks for sharing your project. I just watched your video and I recently started with my own(Just a different shape and so on). How did you start tuning your PID controller? Did you set the Ki and Kd values to 0 and then increased the Kp value like Ziegler Nicols tuning or did you just manually tune them until they worked?


  519. August 14th, 2014 at 20:38 | #519

    I simply increase Kp until it oscillates, then turn it a bit down. Then increase Ki until it oscillates slowly. Finally I increase Kd to damped the system.

  520. Piet
    August 28th, 2014 at 09:05 | #520

    Hi, your robot looks great. Im also trying to build one using the exact same motors but using a pic. My pid algorithm looks mores or less the same as yours but im am having a problem with the motors. It seems like the motors take to long to respond, they only start moving when it is to late and then the robot falls over. Did you also have this problem? Thanks

  521. August 28th, 2014 at 17:58 | #521

    It sounds to me like you need to tune your PID controller. Perhaps you could upload a video somewhere, so I could have a look?

  522. Piet
    August 29th, 2014 at 05:27 | #522

    Thanks i will defnitly upload a video as soon as i can. I have only been trying to set the proportional constant to get it to oscillate at least but no luck

  523. Mounika
    January 3rd, 2015 at 16:33 | #523

    Hello Lauszus,
    I would like to know the total cost of the project involved in its making.

  524. January 3rd, 2015 at 19:49 | #524

    I honestly can’t remember, but you can just add up all the parts yourself. But it quickly becomes quite expensive, so expect to spend $500 if you built it from scratch.

  525. Rafael
    January 7th, 2015 at 22:27 | #525

    Your robot is impresive. Also the Balanduino project. Unfortunately at our school we can’t affford to get one and we are trying to build it like the one in this article. We have the motors with encoders, an MPU6050 and an Arduino Due. But we do not know how to connect them, and we do not find that information in this page. Could you please tell me how to do it?
    Also, I have read that the accelerometer is better to be placed near the wheels axis, but we do not know if it needs to go in a special position. Could you clarify this, please?

    Thank you very much
    Rafael Caballos

  526. January 14th, 2015 at 14:56 | #526

    The best resource is to look at the Balanduino schematic:

    Just between the two motors is the ideal location.

  527. Osama
    January 26th, 2015 at 23:08 | #527

    Hi Lauszus
    I’m an Electrical Engineering student
    you have a really nice robot there ^^

    I wonder if you could update the hardware and software so we can try to work on it
    Also, have you ever considered using Wiimote instead of IMU??

    Thank you

  528. nela
    March 7th, 2015 at 23:03 | #528

    Hi Lauszus. I am working on my own self-balancing robot. I have everything done. The software seems to be ok. But is this normal that motors start to spin only when the torque send is high (over 180)? I keep tracking torque on the serial monitor. I tried to tune PID, becuase I thought that may be the issue. But after couple of tries with tuning PID, I am not sure now if this a problem with this or something else. Motors start to spin when torque on the serial monitor is like 180 or more. When it’s like for example 30 or 50, motors do not move. Could you help me? Did you had that problem too? Maybe you know what can couse that? I look foward to hearing from you.

  529. March 9th, 2015 at 22:56 | #529

    You should check out the Balanduino: which is the newest is the robot. No I see no need for it, as IMU’s like the MPU-6050 is so cheap it does not make sense to use the one in the Wiimote.

    No that is not normal. They should start spinning at even low values. When you talk about torque, are you then talking about the value here: or something else?

    Can you send me a link to the motors you are using?

  530. nela
    March 11th, 2015 at 18:43 | #530

    Hi yes I’m sorry, I named it as a torque in my code. Anyway I’m talking about speed of the motors. I try to tune PID and motors only start spinning while the pwm value is bigger than 150 more less. These are the motors I am using. I cannot say now if this is a problem with motors (hopefully not!) or my code or just pid tuning… I actually spend hours on tuning pid and I can see that behaviour of motors changes but still cannot get the right values. If you want I can send you my code. I would be pleased if you could give me any advice/ help with this. I really want to see my robot balancing πŸ™‚ Thanks

  531. nela
    March 12th, 2015 at 23:25 | #531

    update: I tried to fix that my motors do not move at low speeds. I read about the ‘dead zone’ of the dc motors, and maybe that is the reason. So I changed my speed form 0 -> 255 to 70 -> 255. I don’t know if I did right thing, but anyway motors start moving at small angles as well. But then I tried to tune my pid and only I get is jittering around error=0 position. With changing pid constants, jittering gets bigger or smaller, but the robot cannot balance. I cannot find that ‘oscillation point’ while tuning kp. My last constants were like float kP = 280;
    float kI = 0;
    float kD = 110;
    The biggest kI I can use is 1, becuase bigger makes my robot even worse. For try and error method I left kI = 0. With bigger kP I see the response is faster but still it cannot balance. After maximum 3 seconds of jittering it falls over. Could you help me? I will send you my code on e-mail, so you could take a look. Thank you

  532. March 15th, 2015 at 00:30 | #532

    It would be better if you could upload a video on YouTube and then send a link to it? It sound to me like you might need some better motors.

  533. March 15th, 2015 at 00:35 | #533

    Also I could see from your email that you are using a Intel Galileo.

    It worries me a bit, as it is not good for applications like this, as it is not real time. You could try to install a real time OS (RTOS) on it though and see if it helps (if it even possible), but I would recommend using a “normal” microcontroller instead.

  534. nela
    March 15th, 2015 at 00:53 | #534


    The thing is I have to use Intel Galileo (college project requires that). I saw few self-balancing robots with using Intel Galileo so that’s why I know it’s possible to do that. They didn’t say about installing RTOS, so I think they didn’t install it, but I could be wrong. Anyway, I am still trying to balance that robot πŸ™‚ here’s my youtube video. Take a look

  535. nela
    March 15th, 2015 at 18:02 | #535

    update: I saw I got really odd values from my mpu ( I got results 0 -> +/- 1.5 instead of 0 -> +/- 90 degrees), so I changed calcOutput to be a global variable equal to 0 at the beginning of my code ( the result from the mpu are now from 0 to +/- 86 degrees). I did the same with calcPID.. The robot stopped jittering at error = 0 and I can see that is oscillates now with kp = 10. With that value, robot becomes pretty unstable and moves from side to side at high speed. What is more important, my motors spin at low angles now as well. Well, maybe that’s some progress? I don’t know yet, because it’s still not balancing, but I eliminated that annoying vibration. If my robot don’t balance, there is something wrong with the code. But what can be wrong? Any suggestions? πŸ˜‰

  536. March 19th, 2015 at 23:48 | #536

    I really don’t think it’s a good idea to use the Galileo, as you can only toggle a “normal” pin at about 230 Hz according to this page: when using the standard digitalWrite function. See that page for some tips on how to increase the I/O speeds.

    You are using the Intel Galileo 1, right? The Intel Galileo 2 has more pins that can be used as “fast pins”. You can check if it is a fast pin and set the output like so:

    if (pinToFastPin(pin))
        pinMode(pin, OUTPUT_FAST);
        pinMode(pin, OUTPUT);

    And then you can write to the pin like so:

    fastDigitalWrite(pin, HIGH);

    Also check out these lines in the USB Host library:

    It sounds like you need to make a calibration routine for the accelerometer. Please see these lines for the Balanduino:

    Btw also see:

  537. nela
    March 20th, 2015 at 15:28 | #537

    Hi Lauszus,
    Thank you for your reply.
    I am using Galileo Gen 2. I saw two projects with this Galileo, there were working fine. I am sure mine one will work fine too πŸ™‚ I wish I could use Arduino (I am not a Galileo fan), but I have to make my robot with using Galileo. Anyway, I will read about these ‘fastpins’. Maybe that will help somehow. But I have one more quiestion about PID tuning and the ‘oscillation point’. With just only kp set, my robot will balance or almost balance? I now there will be some overcorrection etc. But I am not really sure how this should look like. Do I need to turn up kp to the moment the robot will catch itself? Or am I thinking wrong.. Because sometimes tuning is difficult when you don’t know what to expect. Thanks

  538. nela
    March 20th, 2015 at 16:04 | #538

    On the website I can read:

    3.2 Faster GPIO performance on shield pins IO0-IO6 and IO9-IO13

    GPIO functionality on these shield pins is provided directly by the Quark X1000. Accessing these pins through the standard Arduino API functions digitalWrite() and digitalRead() allows a sketch to toggle these pins at a frequency of approximately 390 kHz. This performance capability was available on the original Galileo, but only on digital shield pins IO2 and IO3, and only when OUTPUT_FAST pin mode was selected for those pins.
    For even better performance, the following Galileo-specific API functions may also be used:

    fastGpioDigitalRegWriteUnsafe(id, mask)

    So is 390khz with normal digitalWrite fast enough?

  539. March 23rd, 2015 at 23:44 | #539

    You basically increase Kp until it oscillates, then decrease it a little bit. Then increase Ki until it oscillates slowly. Finally increase Kd a little bit to dampen the system. Then increase Kp a little bit. Continue until it can balance.

    It’s hard to say what is “fast enough”. Try to print out your loop time. If it get’s above 10ms, then I would be worried if I were you.

  540. nela
    March 24th, 2015 at 13:16 | #540

    My loop time is about 9ms. I’ll try tune the pid some more. Thank you for your advices. I have one more question. Could you please upload a video of your robot while oscillating? I think it would help me a lot (maybe not even me, but many other people fighting with pid πŸ™‚ ) Thank you

  541. March 24th, 2015 at 16:07 | #541

    No I can’t do that at the moment, as I have actually borrowed my robot out to a friend.

  542. nela
    March 27th, 2015 at 22:32 | #542

    hi, I think I discovered one problem. I set my setpoint to -0.60 where the centre of gravity is. With only P term everything’s ok. Motors don’t move at the setpoint. But when I add K term, my setpoint moves a little in one direction. It’s about at 9 degrees and at that point motors stop moving. The robot behaves like 9 degrees was its setpoint, not -0.60. Do you know what could cause that? I’m confused….

  543. nela
    March 28th, 2015 at 01:16 | #543

    * sorry I meant integral not “K”

  544. nela
    March 30th, 2015 at 21:41 | #544

    Nevermind, I found out why it’s like that.
    What I know there has to be a problem in my complementary filter.
    You said about the accelerometer calibration. I did the very simple one with the gyro, but it doesn’t work with the accel. You can check it on the code I sent you. When I do the same thing with accel like:
    accX = (ax – avgAccX)/ACCEL_FACTOR;
    accY = (ay – avgAccY)/ACCEL_FACTOR;
    accZ = (az – avgAccZ)/ACCEL_FACTOR;
    I get weird results.. Without it, complementary filter seems to be ok. Do you know how can I make the same simple calibration for accel? I know you used the kalman filter, but maybe you know how to program complementary filter?
    ps. the code you showed me with accel calibration.. I guess it’s perfect, but I am not a professional programmer (yet! haha πŸ˜€ ) and I don’t really understand what have you done there.
    I look forward to hearing from you. Thank you so much.

  545. March 31st, 2015 at 15:11 | #545

    I recommend you take a look at the calibration routine for the Balanduino: Btw have you connected the encoders or not?
    I don’t have time to adapt it to your code, sorry.

  546. nela
    April 1st, 2015 at 20:21 | #546

    Well, I tried to do calibration for the accelerometer, but with no success. I got NaN from the complemetnary filter or weird values. Without the accelerometer calibration the values were ok, but delayed. I found out that the complementary filter gave me a lag. There was a noticable delay which affected the control. It took few seconds(?) for robot to realize that it’s not in a balanced position and then it was too late. I could do anything, but I couldn’t get rid of that delay. I started to use DMP library and the problem dissapeared. There is no delay, robot reacts immadiately. Did you had that delay with the kalman filter? Maybe kalman is more accurate? Thank you for your time and help.

  547. April 4th, 2015 at 12:19 | #547

    No I did not have that problem with my Kalman filter.

  548. Ahmed Zak
    April 7th, 2015 at 12:12 | #548

    Hi, I wanted to know how many external interrupts are you using? I am using the Uno with Pololu motors (with encoders) and MPU6050. I also plan on using a bluetooth module for remote control later on. This required me to have 3 external interrupts right? But the Uno only has 2.

    Is there anything I can do about this?

  549. April 7th, 2015 at 12:17 | #549

    @Ahmed Zak
    No the original code only uses two, but the Balanduino just uses pin change intterupt, so you might consider using that instead. See: and

  550. Ahmed Zak
    April 7th, 2015 at 12:24 | #550

    @Kristian Lauszus
    Ok i’ll look into this. Thanks for the quick reply. Your code helped me a lot! Save me many hours of despair and also helped me learn many things. Peace.

  551. nela
    April 11th, 2015 at 19:25 | #551

    Hi Lauszus,
    I got it working! πŸ˜€ My robot balances now pretty well. Thank your for all your help!

  552. April 14th, 2015 at 15:16 | #552

    You are welcome πŸ™‚ Glad to hear you got it working!

  553. ali
    April 20th, 2015 at 17:19 | #553

    dear lauszus

    i am making a self balancing robot using arduino mega ,mpu6050 and polulu’s 30:1 12v 350 rpm motors with encoder, i am using jeff rowberg library for mpu sensor to get pitch values, the data from mpu comes on interrupt pin which is triggered when fifo buffer has a value , i want to know how can i read encooders using interrupts, because everytime i use ISR for encoders i get false values which i do understand because arduino is already busy on mpu data interrupt ,

    pls help me


  554. Greg
    April 21st, 2015 at 18:28 | #555

    I just had a question about how you implemented the PID loop onto your robot. I have built a robot very similar to yours and I do have it balancing. To do this, I have two PID loops. One varies the angle on which to set the robot to which is determined by how far away from zero the wheels have moved using encoders and the other PID loop gets the robot to that determined angle. Mine is not quite as stable as yours however. Is this similar to how you have set your PID loops up or have you taken a different approach?

  555. April 21st, 2015 at 23:16 | #556

    Yes that is the same approach I have used. Can you share your code somewhere, then I will have a look at your code πŸ™‚

  556. Mansor
    April 23rd, 2015 at 12:35 | #557

    Hi Lauszus,

    Any chance porting the code to Due?

  557. April 24th, 2015 at 21:33 | #558

    No unfortunately that is not something I will be working on in the near future.

  558. arpit
    May 6th, 2015 at 14:13 | #559

    hi lauszus, ur robot balances very well …. great work.
    1.I would like to know can the same robot balance for polulu’s 19:1 12v 500 rpm motor with encoder…??
    2. what was ur pwm value and how to adjust pwm value for 500rpm motor…

  559. May 6th, 2015 at 14:15 | #560

    It possible will, but why not just use the same ones as I used?

    You will have to tune the PID controller, so it works with your motors.

  560. arpit
    May 7th, 2015 at 05:19 | #561

    @Kristian Lauszus
    i will tune the pid controller…because my robot is heavier some what larger in size than urs …. so for my application those motor were suitable…
    by the way thanks for ur suggestion.

  561. May 7th, 2015 at 18:11 | #562

    I would have recommended motors with more torque if it is heaver i.e. a higher gear ratio.

  562. arpit
    May 13th, 2015 at 03:43 | #563

    hi kirstian,
    how did u tune ur PID values so precise ??did u used some software or just by trial and error?

  563. arpit
    May 13th, 2015 at 04:01 | #564

    i would also like to know if the robot balances pretty well using arduino or it balances more precisely using mbed controller and arduino as u have mentioned on ur blog….thanks

  564. Bill Parker
    May 15th, 2015 at 22:47 | #565

    Hi Would it be practical to use this to control a full size self balance ridable 2 wheeled Segway type of transport? Or do you know of anything that might help me I have trouble walking and thinking this may help.

  565. June 7th, 2015 at 15:06 | #566

    I tuned them manually. Start by setting all value to 0, then increase Kp until it starts oscillating, then turned it down a little. Then start increasing Ki until it starts oscillating, but this time the period should be longer. Finally you can increase Kd to dampen the system.

    The performance is the same between the two systems.

    @Bill Parker
    Please see the following blog post:

  566. Rodrigo
    June 17th, 2015 at 22:43 | #567


    I am little puzzled by the way you implemented: (1) the control to reduce the target angle at high velocities and (2) the control to return to the stop position. A more intuitive approach would have been to use standard PID loops for each. For example in the ?non-stop? condition a PID finds the target angle (restAngle) based on the velocity error (difference between the desire velocity and the actual velocity). In the ?stop? condition a PID finds the target angle (restAngle) based on the distance away (encoder counts) from the stop position. Does this make sense? Did you try an approach like this instead? If yes, what was your experience? If not, why not? Any suggestions?

    Thank you,


  567. June 20th, 2015 at 15:21 | #568

    You should check out this branch of the Balanduino repository: where I have experimented with just that, but I never got it to work properly.

  568. Ryuoh
    June 28th, 2015 at 05:23 | #569

    Hi Lauszus
    I’m making balanced robot now but no matter how I tune PID parameter it can’t be balanced
    it swing like this

    I use pololu 50: gear motor with encoder
    robot weight is around 1.5 kg
    can you give me some advice

  569. June 30th, 2015 at 00:57 | #570

    I have replied to your Facebook message πŸ™‚

  570. Siddhant
    July 16th, 2015 at 06:19 | #571

    Hi Kristian,

    Thanks for the great guides you have posted all over the internet and for regularly replying to posts here! There’s a lot to read and I’m hoping I’ll be able to solve most of my troubles during my quest for balance by reading posts from this blog.

    I do have one problem that I cannot seem to solve though. When laying flat on a table, the sensor shows 180 degrees but when I roll it to a vertical position moving it by 90 degrees, the serial monitor does not show a value of 90. It shows 90 degrees at a position that is clearly not 90 degrees. How can I go about fixing this?

    Thank you!


  571. July 16th, 2015 at 07:34 | #572

    You need to calibrate the accelerometer. This can be done by finding the accelerometer values when the robot is horizontal and then subtracting the values from any further measurements. See:

  572. Larry McGovern
    August 23rd, 2015 at 16:47 | #573

    Hi Kristian, your website has a wealth of data, and was a huge source of inspiration for my own balancing robot “Zippy”. Here’s my homage to your project: You may notice one or two design similarities… Thanks for making all this publicly available! I’ll be watching your site.

  573. August 23rd, 2015 at 23:15 | #574

    @Larry McGovern
    Looking good πŸ™‚ How is your experience with those wheels? I might try something similar at some point.

  574. Larry McGovern
    August 24th, 2015 at 01:48 | #575

    @Kristian Sloth Lauszus
    I think the scooter wheels are great, but I can’t say I have any experience with other wheels. They work just as well on hard surfaces and carpet, and have a nice rubbery grip. I used a scooter wheel adapter from Pololu, which directly interfaces with a 6 mm diameter motor shaft. My daughter has wheels that light up on her Razor, which might also be a fun addition to a balancing robot…

  575. August 24th, 2015 at 19:58 | #576

    @Larry McGovern
    I guess you are talking about these ones: I might try them out. They must be relatively new, as I have not seen them in their store earlier.

    Haha yes it might be fun to try someone with flashing lights!

  576. YOBE
    August 29th, 2015 at 03:57 | #577

    Hi Kristian,
    Im also working on my balancing robot but facing problems with interrupt and I2C communication.
    I am using Chipkit, but once I create the high freq interrupts from the encoders, my I2C communication to the IMU is crashing and hangs the uc.
    Have you ever encountered something like this?

  577. mohamed siddik
    September 9th, 2015 at 11:50 | #578

    hi frd;
    i am siddik from india (TN). I am interested to do the self-balance robot. I done mobile controlled robot. I want to do this help me frd . I have Arduino uno, bluetooth transceiver module(hc-05) then two 5v dc motor .

  578. September 11th, 2015 at 08:27 | #579

    No I have not. Maybe it is a problem with your connections? Please try to solder the connections if you haven’t already. Also try to add some capacitors close to the regulators in oder to filter out the noise.

    @mohamed siddik
    Okay sounds good. What do you need help with?

  579. Geoffrey
    February 1st, 2016 at 21:48 | #580

    Hi Kristian,
    I had built my own self-balance robot and your website had been useful, thanks.
    However, I have some difficulties on the last stage: the PID tuning.

    In your method, what do you mean by “it starts oscillating” ? Does it stabilize already or does it only vibrate ?

    Then, can you give me some details about the reaction of the robot along the tuning ?

    Thank you !

  580. February 2nd, 2016 at 00:29 | #581

    It should start to oscillate or vibrate when the gains are too high. Simply increase them until to you reach this point and then decrease them until it stops.

  581. Pat
    February 22nd, 2016 at 21:05 | #582

    Hi Kristian,
    I have just finished my project and thank you for the inspiration.I have just received the mpu6050 in post today and my question is do i need to calibrate the mpu6050 offsets while the unit is off the robot and on a flat level surface or is this taken care of by the software.

    Thank You

  582. Patrick
    March 28th, 2016 at 05:51 | #583

    Great robot! Is this possible without a microcontroller? I have an raspberry pi, L298N stepper drivers, NEMA17 stepper motors, and an MPU6050 for the gyro.

    Before I get too far is the RPI not fast enough to properly calculate IMU dimensions and give the steppers a fine level of control?

  583. March 28th, 2016 at 08:20 | #584

    Hi Patrick.
    It is definitely possible to do with a Raspberry Pi as well, but as you say it would require some cautious Kernel modifications to enable Real time support to get the latency down.

    Please see the following links where the Raspberry Pi is used as a flight controller:

    Regards Thomas

  584. April 14th, 2016 at 23:15 | #585

    In my experience it is usually not needed, but in some rare occasions it is needed, but normally you can just adjust the target angle manually. I simply implemented the calibration algorithm for the Balanduino, so it would be easy for people to do it themselves.

    It is differently possible on a RPi, but as Thomas wrote you will need a real time support. If you are not that experienced with Linux I would recommend sticking with a simpler microcontroller.

  585. Pat
    May 26th, 2016 at 19:31 | #586

    Hi and thanks for your advise Kristian, one last question. My robot is up and running and am very happy with my build it balances very well and moves Forward and Backward great but the Left and and Right steering seems very nervous any suggestions what the problem might be would be very helpful.

  586. Jean
    June 16th, 2016 at 18:52 | #587


    Hey Krisitian,

    I’m having problems with tuning the PID, I’m not really following what do you mean by oscillating, whatever values I’m giving for Kp, the motors react fourth and back, so in every case they are oscillating. Could you please explain more, or maybe record a video while doing it?
    Thank you

  587. June 17th, 2016 at 13:06 | #588

    You should simply just subtract the turning value from one motor and add it to the other, as it is done here:

    Can you upload a video of it, then I will take a look πŸ™‚

  588. Jean Khoury
    June 18th, 2016 at 19:04 | #589

    @Kristian Sloth Lauszus

    Hey Kristian,
    Before recording a video, I wanted to know if it’s necessary to use a motor encoder? I’m using an arduino motor shield instead of a motor driver too.

  589. June 19th, 2016 at 09:10 | #590

    @Jean Khoury
    Yes I would differently recommend using encoders!

  590. Jean Khoury
    June 19th, 2016 at 21:08 | #591

    @Kristian Sloth Lauszus

    Ok then that’s my starting point. I’ve done a little research about rotary encoders, and all I got is that it gives us the speed of the wheels with its direction, but I didn’t find any related arduino libraries or how to get the informations, codings, anything in general.

  591. Jean Khoury
    June 21st, 2016 at 15:10 | #592

    @Kristian Sloth Lauszus

    Hello again Krisitan,
    So I managed to stabilize it with some oscillations of course due to not using encoders for the moment, however I’m trying now to make the robot move forward/backward. I’ve tried adjusting the PID’s setpoint but due to the current parameters it falls almost instantly. How did you manage to do it?

  592. June 24th, 2016 at 09:08 | #593

    @Jean Khoury
    Please read the following page:

    Also take a look at the Balanduino code:

    Can you record a video of it and upload it somewhere, then I will have a look.

  593. Jean Khoury
    June 29th, 2016 at 14:59 | #594

    @Kristian Sloth Lauszus

    Hello Kristian,
    Sorry for too many questions πŸ˜€
    So I re-arranged my PID parameters, and managed to move the robot forward ( had to add some I for a faster respond). it’s not perfect however due of course to not using an encoder.
    I found an arduino encoder, the KY 040, I managed to read the data ( value between 0 and 255) but the link you gave me did not help much, the code didn’t work for me.
    my question now is how to calculate the wheel speed ( to have it as an input for speed control pid and get as an output the angle that will be used as a setpoint for the balancing pid).
    since speed = d position/d time, I thought of fixing d position as the distance between two clicks, but I don’t even know how to do that, am I even thinking correctly?

  594. Jean Khoury
    June 29th, 2016 at 18:24 | #595

    @Kristian Sloth Lauszus

    So I took some time to think about it, here’s what I got.
    the encoder gives 20 ppr, which means 18 degrees per tick. ( that’s a shity resolution to me !)
    pi->180 degrees, which means 18 degrees -> pi/10.
    so the distance between two consecutive ticks would be d position = pi/10 * diameter of the wheel. I only need to calculate d time between the two ticks using millis and voila.
    What do you think? Or maybe enlighten me with the way you did it πŸ˜€

  595. Pat
    June 30th, 2016 at 19:48 | #596

    @Kristian Sloth Lauszus

    Thank you again Kristian, as i am already using the latest version of your Balanduino code it was my understanding that the bot should turn as good as it goes forward and backwards without needing to adjust the code. Could it be a Bluetooth speed issue with the dongle

  596. August 7th, 2016 at 17:10 | #597

    @Jean Khoury
    I would use a encoder with higher resolution.

    It actually turns quicker than it can move forward and backward.

  597. <