The Balancing Robot

Now avaliable as a kit
A balancing robot kit is now avaliable via Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit. Check out the blog post as well: http://blog.tkjelectronics.dk/2013/03/balanduino-balancing-robot-kit-kickstarter/.


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

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

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


The Hardware
Here are some pictures of the robot:


Here is a list of all the hardware I used:

I also used:

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

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

Check out these rendered images of the robot:

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

Also check out the wiki.

Update
I have now ported the code to Arduino. The code can be found at github: https://github.com/TKJElectronics/BalancingRobotArduino.

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

Conclusion
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. Lauszus
    April 23rd, 2012 at 21:48 | #1

    @kas
    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 :)

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

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

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

    @kas
    Hi, take a look at my updated code: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L320
    I have implemented the new method on how to get full 360 degrees resolution.

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

    >> 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 http://en.wikipedia.org/wiki/Atan2
    >> // 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

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

    @kas
    Okay I will try it tomorrow and let you know how it went :)

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

    @Lauszus
    For what is worth, I use my own arctan2 with int values for optimisation
    please see http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization

    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

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

    @kas
    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++ (http://www.cplusplus.com/reference/clibrary/cmath/atan2/) 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.

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

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

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

    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!

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

    @Luigi
    Have a look at my earlier reply to kas: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/comment-page-1/#comment-20112

    Just write me again, if you need more details :)

    Regards
    Lauszus

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

    @Lauszus

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

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

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

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

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

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

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

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

    @Lauzus
    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.

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

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

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

    @Lauszus
    This one: http://www.pololu.com/catalog/product/1443
    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?

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

    @Luigi
    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.

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

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

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

    @Luigi
    Your welcome ;) Please write again if you need further assistance!

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

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

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

    @batista1987
    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 (http://www.pololu.com/catalog/product/1444) and compare them.

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

    @Lauszus

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

    TIA

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

    @kas
    Mine read 180 degrees too, after I changed the pitch calculation to use atan2(), so yes my targetAngle is 180 degrees, see https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobot.h#L88

    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);
          delay(20);
       }
       for(uint8_t i = 100; i > 0; i--) {
          moveMotor(left, forward, i);
          moveMotor(right, forward, i);
          delay(20);
       }    
    }

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

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

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

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

    @kas
    Argh, forgot to do that :)
    Okay. I will take a look at it tomorrow.

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

    @kas
    You can just uncomment the following line: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L96 and it will work the same way as there is not a PS3 controller connected.

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

    @Lauszius
    That’s what I did; I also commented those lines
    //#include
    //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 ??

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

    @kas
    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:
    http://www.tkjelectronics.dk/uploads/BalancingRobotArduinoWithoutPS3.zip

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

    @Lauszius
    Thanks, I will let you know the outcome

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

    @Lauszus
    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);
    else
    moveMotor(left, backward, PIDLeft * -1);
    if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
    else
    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)

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

    @kas
    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.

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

    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 ? http://www.pololu.com/catalog/product/1439
    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() ;)

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

    @kas
    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: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L406
    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.

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

    @all
    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 :)

    @Lauszius
    I will fit those wheels and let you know the outcome

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

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

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

    @kas
    @Lauszus

    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!

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

    @Luigi
    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: http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

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

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

    @Lauszus

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

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

    @Lauszus
    @Kas
    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,

    Luigi

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

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

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

    @Lauszus
    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,

    Luigi

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

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

    Regards
    Lauszus

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

    Yes, motors are wired to OUT screw terminals.

    Regards
    Luigi

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

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

    Regards
    Luigi

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

    @Luigi
    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.

    Regards
    Lauszus

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

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

    Regards,
    Luigi

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

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

    Regards
    Lauszus

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

    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.

    Cheers!!!

Comment pages
1 2 3 4 11 2196
  1. March 4th, 2012 at 21:03 | #1
  2. March 7th, 2012 at 21:22 | #2
  3. March 7th, 2012 at 21:29 | #3
  4. March 7th, 2012 at 23:33 | #4
  5. March 8th, 2012 at 02:49 | #5
  6. March 8th, 2012 at 05:03 | #6
  7. March 8th, 2012 at 08:31 | #7
  8. May 14th, 2012 at 15:14 | #8
  9. May 23rd, 2012 at 20:02 | #9
  10. February 17th, 2013 at 15:13 | #10
  11. February 17th, 2013 at 15:24 | #11
  12. March 18th, 2013 at 23:18 | #12
  13. March 24th, 2013 at 17:58 | #13