TKJ Electronics

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 http://dl.dropbox.com/u/24235756/new/P3040964.JPG http://dl.dropbox.com/u/24235756/new/P3040965.JPG http://dl.dropbox.com/u/24235756/new/P3040967.JPG

@Gary
Hi,
So far it looks pretty good. But I think your wheels are a bit to small. Originally I used these wheels from Pololu: http://www.pololu.com/catalog/product/1439. They worked okay, but I got much better performance by buying these wheels: http://www.robotshop.com/eu/banebots-08in-5in-hex-wheels-4.html. 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

@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: http://www.batteryspace.com/dcmotorhightorquemini12vdcgearmotor200rpmforhobbyprojects.aspx. they do not have encoders, but I think I will test how they perform before buying new motors with encoders. =D

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

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?

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

Hi,

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 http://www.robotshop.com/eu/hex-hub-s40-6mm-2wide.html
why not “One wheel wide” hub ??

Keep up the good job
Kas

@Kas
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: http://banebots.com/pc/WHB-WS-498/T80P-493BG-HS4 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”: http://www.x-firm.com/?page_id=145? 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 🙂

Hi,
nice work

I tried to test the library but I get stuck at:
S;
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?

regards,
Cosmin

@Cosmin
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: https://github.com/TKJElectronics/USB_Host_Shield_2.0/blob/master/PS3BT.cpp#L52 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: https://github.com/TKJElectronics/USB_Host_Shield_2.0/wiki

Regards
Lauszus

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 🙂

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

Regards
Lauszus

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

kas

@Lauszus
finally it works,
I had 2 problems
put the mac addres of the dongle in the dualshock3, I used http://www.dancingpixelstudios.com/sixaxiscontroller/SixaxisPairToolSetup-0.2.3.exe

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

regards,
Cosmin

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?

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

@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: http://www.pololu.com/catalog/product/1439, but they where not as good as the BaneBots one.

@Kas
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: https://github.com/TKJElectronics/BalancingRobot/blob/master/BalancingRobot.cpp#L198.
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.

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
see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0

@Kas
Nice. Do you got a blog or anything like that, so I can follow your progress? 🙂

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 😀

@zmiguelb
I didn’t calculate the values – it was more like trial and error. But I followed this guide:
http://robotics.ee.uwa.edu.au/theses/2003-Balance-Ooi.pdf 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.

@Lauszus
Thanks for the fast reply Lauszus.
I will try that approach as soon as I get home. (Crossing finger hoping it will work 😉 )

No blog, will keep you informed

@Kas
Okay, looking forward to see your robot in action 😀

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

Are you still following the Arduino Forum Lauszus?

@Niels Bo
Thank you Niels, nice to hear that from an engineer 🙂

@rob4white
Hi,
Yes I’m stil following it, but didn’t have the time to check out the forum until now 🙂
Your robot looks really nice (http://arduino.cc/forum/index.php/topic,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 🙂

Hi,

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?

Thanks.

@Quentin
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: https://github.com/TKJElectronics/BalancingRobot/blob/master/BalancingRobot.h#L98
I don’t know the maximum inclination, I will let you know if I ever measure it.

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. (http://www.x-firm.com/?page_id=315)

@X-firm
It’s always nice to inspire people, I got inspiration from 🙂

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

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

Regards
Lauszus

Hi,

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?

Thanks!

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

Regards
Lauszus

@Lauszus
Thank you,

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

Thanks again

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

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

@Lauszus

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 ?

TIA

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

@kas
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 http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/. 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:

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!

@Lauszus

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

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

@Lauszus
This is the return value of getAccY()

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

@Lauszus

It’s OK now
My AccY (your AccX) was not (yet) connected
Replacing
double R = sqrt((accXval*accXval) + (accYval*accYval) + (accZval*accZval));
by
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 🙂

@kas
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: http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/

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

@Lauszus

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

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

Okay nice, looking forward to see your robot 🙂

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

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

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

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

@kas
I worked just as fine.
I have now updated the github: https://github.com/TKJElectronics/BalancingRobotArduino/commit/60fc754d93ee6ad8619fd2ed952bf631c2ccea11 🙂

@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

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

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

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!

@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

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

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

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

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

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

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

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

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

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

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

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

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

@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

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

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

@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

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

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

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

@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

@Lauszius
Thanks, I will let you know the outcome

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

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

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() 😉

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

@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

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

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

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

@Lauszus

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

@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

@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

@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

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

Regards
Lauszus

Yes, motors are wired to OUT screw terminals.

Regards
Luigi

@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

@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

@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

@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

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

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

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

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

@Claudio
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: http://www.robotshop.com/eu/hex-hub-s40-6mm-2wide.html

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.

Regards
Lauszus

@Lauszus

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.

Thanks!

@Claudio
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:
http://www.alibaba.com/product-gs/201734052/Motor_Encoders.html

Regards
Lauszus

@Lauszus

Nice, so this one should work fine, right?

http://www.jayconsystems.com/product_detail.php?prod_id=296

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.

@Lauszus

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.

Thanks!

@Claudio
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: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/#update and have a look at Github: https://github.com/TKJElectronics/BalancingRobotArduino.

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.

Regards
Lauszus

@Lauszus

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!

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

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: http://www.x-firm.com/?page_id=145), before I created my own.

Regards
Lauszus

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

Regards,
Luigi

@Luigi
That’s would make perfectly sense 🙂 I will have that in mind if I ever have to create a new one.

@Lauszus

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.

http://www.udacity.com/overview/Course/cs373/CourseRev/apr2012

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!

@Claudio
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 (http://en.wikipedia.org/wiki/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!

Regards
Lauszus

@Lauszus

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.

@Lauszus

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

@Claudio
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: http://blog.tkjelectronics.dk/2012/03/quadcopters-how-to-get-started/

I can use it for nearly two hours or so!

@Lauszus

Lauszus, how do you check for the battery voltage?

@Claudio
Right now I don’t. I just change the battery when it the speed of the robot is to slow 🙂

@Lauszus

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

Cheers!

@Claudio
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!

@Lauszus

Nice, good to know. Thanks!

@Lauszus

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

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

@Lauszus
@kas

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

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

Look at that:
http://www.youtube.com/watch?v=DMEYUsGte9o&feature=youtu.be

He uses stepper motors
http://www.pololu.com/catalog/product/1200
and drivers
http://www.pololu.com/catalog/product/1182

Zero backslash !!!

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

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

No idea

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

More info here:
http://groups.google.com/group/ioio-users/browse_thread/thread/f35ab15f9826c682/66b517865a856e2e?q=%22balancing+robot%22

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

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

Just collected today, on a rainy Sunday 🙂

Another one with steppers:
http://www.youtube.com/watch?v=w6VqASRawgg

and a ballBot:
http://www.youtube.com/watch?v=f8jxGsg3p0Y

So many projects…

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,

Luigi

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

Regards
Lauszus

I ordered and received a MPU-6050 breakout board (15$ delivered):
http://www.ebay.com/itm/ws/eBayISAPI.dll?ViewItem&item=261050654320&ssPageName=ADME:L:OC:FR:3160
This is a 3 Gyro + 3 Acc + data fusion in a single chip,
I2C digital output and adjustable Gyro/Acc sensitivity
http://www.invensense.com/mems/gyro/mpu6050.html
Looks promising 🙂

I will let you know the outcome

@kas
Nice, we have thought about buying the MPU-6050 as well! It looks really interesting – the new Nexus 7 even uses it: http://youtu.be/GoDAg8mYYz4?t=9m14s 🙂
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: https://github.com/felis/USB_Host_Shield_2.0/blob/master/examples/RFCOMM/RFCOMM.ino and the source code: https://github.com/felis/USB_Host_Shield_2.0/blob/master/RFCOMM.cpp regarding RFCOMM.
We will write a blog post tomorrow with more information.

I though you heard about MPU-6050 allready 😉
Let me know the outcome for my code

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

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 (https://www.sparkfun.com/products/10269)? I do not have a xbee module. Thanks!

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

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.
Thanks!

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

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.

@Ryan
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: https://github.com/TKJElectronics/BalancingRobotArduino or somebody else? Or are you rewriting it from scratch?

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

@Ryan
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: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L32-36 and here is PWMVALUE defined here: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobot.h#L6-7.

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

Regards
Lauszus

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

@Ryan
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: http://www.pololu.com/docs/0J1/4.d. 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 🙂

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:

https://github.com/lusher00/balance2/blob/master/safertos_demo.c

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.

@Ryan
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: https://github.com/TKJElectronics/BalancingRobot/tree/master/PIDController as it will safe you a lot of time finding the PID values.

Nice – I would love to get a nice bit of kit 🙂

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

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

@Ryan
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: https://github.com/lusher00/balance2/blob/master/pid.c#L35?

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: http://processing.org/

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!

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.

@Lauszus
I added a few photos to the repo

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

@Ryan
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: https://github.com/felis/USB_Host_Shield_2.0

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?

@Lauszus

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.

Cheers!

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

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

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

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

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

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.

@Jetguy
Hi,
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: http://arduino.cc/forum/index.php/topic,58048.0.html. Also take a look at the code I provided: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/Analog_IMU

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: http://mbed.org/handbook/SerialPC
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: https://github.com/TKJElectronics/BalancingRobot/blob/master/BalancingRobot.cpp#L17.

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

@Jetguy
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: https://github.com/TKJElectronics/BalancingRobotArduino, 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: https://github.com/TKJElectronics/BalancingRobotArduino which is the one I currently use.

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.

@Jetguy
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: http://www.circuitsathome.com/usb-host-shield-hardware-manual.

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

@Ryan
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 🙂

@Lauszus
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 http://www.x-firm.com/?page_id=315
So now it’s a matter of merging them.

@Jetguy
Okay, good to hear that you are finally getting some progress 🙂

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

@Jetguy
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: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/Analog_IMU/Analog_IMU.ino#L52.

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

@Lauszus
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 http://www.pololu.com/catalog/product/1267 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.

@Jetguy
I know that the they don’t align, that’s why I put a minus infront of the gyro value: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L306.

It doesn’t make any difference if you calculate y before S. See this code: https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.h

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: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/Analog_IMU/Analog_IMU.ino 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.

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

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

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.

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.

@Andrew
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: https://github.com/felis/USB_Host_Shield_2.0/wiki/PS3-Information#wiki-Hardware

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

Additional notes on switching the code to 5DOF SEN-11072 and (Mbed code):
The IDG500 has a sensitivity (rated) 2mv/degree/sec
double gyroRate = -((gyroY.read() – 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?

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

use this accelerometer http://www.robogaia.com/6-axis-accelerometer-gyro-arduino-shield.html

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 😉

@Ryan

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.

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

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

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

@Jetguy
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 🙂

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?

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

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

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

@Lauszus
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…
http://www.youtube.com/watch?v=KGkKl7vBQrc&feature=youtu.be

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

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

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

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

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

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

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?
https://github.com/lusher00/balance2

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.

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

@Jetguy
I just pushed a new commit to github, with comments about the wiring of the motor driver and encoders: https://github.com/TKJElectronics/BalancingRobotArduino/commit/df3c17ecd51250a0ea1d3319db126eeb16c49001
But I found out that I actually mistyped the names of the encoders (https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobot.h#L53-56)!! 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.

@Jetguy
I was the one who drilled though the hub and put in a hardened pin, I mention it in the video: http://youtu.be/N28C_JqVhGU?t=9m6s
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.

@Jetguy
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!

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

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

Took me far too long to figure this out…

https://github.com/lusher00/safertos_demo

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.

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

@Ryan
Didn’t you forget to use “delta_t” in the Iterm: https://github.com/lusher00/safertos_demo/blob/master/pid.c#L104? You do it in the Dterm: https://github.com/lusher00/safertos_demo/blob/master/pid.c#L108, so I thought it might be a mistake. See: http://en.wikipedia.org/wiki/PID_controller#Pseudocode

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

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.

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

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

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

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

@Ryan
Have you seen my previous reply on how I tuned the filter: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/comment-page-1/#comment-20170?

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

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

@Lauszus

http://www.youtube.com/watch?v=iYZVowXWa_s

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?

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

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

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 .. http://www.youtube.com/watch?v=_hLSYejQDy8

At the moment I’m not using the encoder .. only as the accelerometer sensor ADXL345, gyroscope L3G4200D, arduino nano, these engines http://www.pololu.com/catalog/product/2273, these wheels http://www.pololu.com/catalog/product/1556 for road, and this power amp http://www.pololu.com/catalog/product/708, 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

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

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

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

@Ryan
This is the part where I found an error in the original code: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#step2

You could try to implement a complimentary filter instead like so: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino#L67-68. 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: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-54328

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: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph, 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.

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.

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.

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

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

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

@Ryan
Okay. What IMU are you using?

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

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

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

@Ryan
Okay. Have you tried to do something like this first: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino
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.

@Lauszus
HAHAHAHAHAHAHAHAHAHAHAHAHAHAHAHAHAHAHA

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.

@Ryan
You are not the first one to make that mistake 😉
Did you get it to balance?

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

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

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

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

@Lauszus
Here is your English lesson for the day.
https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L121

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.

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

@Ryan
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: http://youtu.be/pOJy0YtPuF4

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

@Ryan
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 🙂

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

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

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

@Ryan
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: https://github.com/Lauszus/sanguino. 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 🙂

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

2 ATMEGA644P-20AUR

and

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.

@Ryan
Nice thank you! 🙂
I will send my address by email.
Haha, it’s just a plugin, but I will have a look at it.

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

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

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

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

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

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

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

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

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

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

http://www.youtube.com/watch?v=MFSPMxr1eiI

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

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

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

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

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

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 : http://www.hobbyking.com/hobbyking/store/__27109__MultiWii_Lite_V1_0_Flight_Controller_w_FTDI.html 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.

Thanks,

John

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

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 ?

Thanks.

Best Regards,

John

@flyrobot
It is indeed supported, but you will need a USB Host Shield: http://www.circuitsathome.com/products-page/arduino-shields

I’m a developer on the library. Take a look at github for the source code: https://github.com/felis/USB_Host_Shield_2.0

Also take a look at this Android app I use to control the robot: https://github.com/TKJElectronics/BalanduinoAndroidApp and this branch of the code: https://github.com/TKJElectronics/BalancingRobotArduino/tree/SPP

Hi Lauszus,

Many thanks. Its really very very interesting project. I will continue my balance robot (never stable) http://m.youtube.com/watch?v=2ATaShsXK7U. I was inspired by this http://m.youtube.com/#/watch?v=NAojeqJCzvo&desktop_uri=%2Fwatch%3Fv%3DNAojeqJCzvo
Hopefully i can built like that, i was thinking it can help many people such as disability people.

Best regards,

John

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. http://www.hobbyking.com/hobbyking/store/__309__190__Tx_Rx_Systems_Parts-2_4Ghz_Turnigy.html
And the most active hacked http://9xforums.com/forum/

Best regards,

John

@flyrobot
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!

Once im ready with this small segway, i will continue with unicycle.
Now i have several IMU from Fabio varesano http://www.varesano.net/topic/freeimu and chinese made.
You probably can see the multiwii code, i have multiwii, mikrokopter.de, 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.
http://www.multiwii.com/forum/viewforum.php?f=8

Best regards,

John

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,

@flyrobot
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! 🙂

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

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?

Thanks.

Regards Thomas

Dear Thomas,

And you check this :
http://www.rcgroups.com/forums/showthread.php?t=1510047

With Best Regards,

John

Dear Thomas,

Thanks for the info. This the link :

– for 9x forum : http://9xforums.com/forum/
– the most active hacked firmware from eraz : http://9xforums.com/forum/viewforum.php?f=5
– this is the code : http://code.google.com/p/er9x/
– multiwii telemetry using 9x with frysky module : http://www.multiwii.com/forum/viewtopic.php?f=7&t=1929
– multiwii telemetry using Graupner Hott Tx : http://www.multiwii.com/forum/viewtopic.php?f=7&t=1284
– my Graupner Hott Tx for tuning multiwii PID : http://m.youtube.com/#/watch?feature=plcp&v=a2DZAsmd_LE

Best Regards,

John

@flyrobot
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: http://www.hobbyking.com/hobbyking/store/__14349__FrSky_DJT_2_4Ghz_Combo_Pack_for_JR_w_Telemetry_Module_V8FR_II_RX.html
Or only this one: http://www.hobbyking.com/hobbyking/store/__14355__FrSky_DF_2_4Ghz_Combo_Pack_for_JR_w_Module_RX.html

Regards Thomas

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 http://www.hobbyking.com/hobbyking/store/__14355__FrSky_DF_2_4Ghz_Combo_Pack_for_JR_w_Module_RX.html

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

Regards,

John

@Lauszus
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?
Cheers!
Jack

@Project 11
Yes of course 🙂 What do you need help with?

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

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

@Lauszus
Thanks for the idea for the spinning disk, we will look into it for more information 🙂

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.

Thanks,

John

@flyrobot
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: https://github.com/Lauszus/VoiceRecognitionService

I might work on implementing that to my project 🙂

Hi Lauszus,

can we chat using skype?

thanks,
john

@flyrobot
Yes, my Skype name is just: “Lauszus” 🙂

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: http://www.youtube.com/watch?v=cz62trqTmiA&feature=youtu.be
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

@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: http://en.wikipedia.org/wiki/Rotary_encoder#Incremental_rotary_encoder

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.

@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: http://blog.tkjelectronics.dk/2011/12/sneak-peak-segway-guide-code/. 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.

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.

@Jakub Blokesz
Okay. Of course, just ask, I’ll be happy to help 🙂

Hello
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: http://arduino.cc/forum/index.php?PHPSESSID=5c4b75a6c40a67709b984ca46ee94310&topic=8871.120 )
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

@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: https://github.com/TKJElectronics/NXTShield/blob/master/NXTShield.cpp#L116

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

And it’s also used to scale down turning at high speed: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L186 and https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L193

Hello
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

@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: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobot.h#L137-L141. Try to divide all by about 2, as the ratio between my encoders resolution and yours is approximately 2.

Hello
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

@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 https://github.com/TKJElectronics/BalancingRobotArduino/tree/master/ProcessingApp or this Android app: https://github.com/TKJElectronics/BalanduinoAndroidApp.

No I never used two modules, at the beginning I used Xbee modules, but right now I use the SPP library I wrote: http://blog.tkjelectronics.dk/2012/07/rfcommspp-library-for-arduino/.

I have actually made three different versions of the of the code depending on how I want to control it. The SPP version: https://github.com/TKJElectronics/BalancingRobotArduino, PS3 version: https://github.com/TKJElectronics/BalancingRobotArduino/tree/PS3 and the Wii version: https://github.com/TKJElectronics/BalancingRobotArduino/tree/Wii.

Hello
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: http://blog.tkjelectronics.dk/2011/12/sneak-peak-segway-guide-code/
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:)

@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: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/comment-page-1/#comment-20170

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.

@Jakub Blokesz
You’re welcome.

Are you using the same motors as me: http://www.pololu.com/catalog/product/1443? They have a stall current of 5A, but the L298 can only deliver a output current of 2A per channel! See: http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00000240.pdf

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().

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

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

@Jakub Blokesz
You can easily just copy the micros() from the Arduino source. First you need to set up timer 0 like so: https://github.com/arduino/Arduino/blob/master/hardware/arduino/cores/arduino/wiring.c#L189-L231
Then you need the micros() function: https://github.com/arduino/Arduino/blob/master/hardware/arduino/cores/arduino/wiring.c#L81-L107

Then you have to define timer0_overflow_count: https://github.com/arduino/Arduino/blob/master/hardware/arduino/cores/arduino/wiring.c#L40

And sbi and cbi: https://github.com/arduino/Arduino/blob/master/hardware/arduino/cores/arduino/wiring_private.h#L39-L44

And finally clockCyclesPerMicrosecond(): https://github.com/arduino/Arduino/blob/master/hardware/arduino/cores/arduino/Arduino.h#L76-L77

Okay it sounds like you are making progress though. What wheels are you using? 🙂

Thank You for the instruction. I made a timing in microseconds:) But unfortunately it doesn`t help.
I use these wheels: http://www.pololu.com/catalog/product/1436
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?

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

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.

@Kaelin Ellis
To control the robot back and forward I set a offset angle, see: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L151-L173
And to turn it I set the turning values like so: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L182-L202

One important thing I also mentioned in the video is that I limit the value depending on how fast the wheels are turning, see: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L152

Another thing I do is that I set a new target position when I want it to stop, see: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L100-L103 and the video: http://youtu.be/N28C_JqVhGU?t=8m3s

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

@Dustan
First of all have you seen this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino I have written?
All you have to do is to replace these functions: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L427-L462 and of course remember to initialize the IMU like so: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino#L32-L39

If you post your code somewhere – for instance at gist: https://gist.github.com/, then I will have a look at it.

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 https://gist.github.com/eebedb99830bd4d637b0/d574e5a209285ed2c1ddf54b0a97352a9652b791@Lauszus

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: http://www.dimensionengineering.com/products/sabertooth2x25
A MPU6050 acc/gyro i bought on ebay eaxctly like this one:
http://www.ebay.com/itm/New-6DOF-MPU-6050-3-Axis-Gyroscope-And-Acce-lerometer-Sensor-Module-For-Arduino-/370691521469?pt=LH_DefaultDomain_0&hash=item564eefbbbd
and some electric wheelchair motors here:
http://www.ebay.com/itm/170969974029?ssPageName=STRK:MEWNX:IT&_trksid=p3984.m1439.l2649

Hopefully these links work

@Dustan

@Dustan
Yes just comment out all the Bluetooth stuff: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L16-L19. 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: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino?

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.

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

@Dustan
I just created this gist for you. Check it out: https://gist.github.com/4549162

You have to understand that the Kalman filter is a library in it self I created. You can find the source code here: https://github.com/TKJElectronics/KalmanFilter.

And read more about it at my blog post: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

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

@Dustan
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 🙂

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: https://gist.github.com/a35cff8a4f139f7e475e#file-my-attempt-L247-L310 Thanks for all your help so far Hope Im not bugging you to badly@Lauszus

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

@Dustan
Hmm, your code looks pretty messy. But I think you can just remove all this code: https://gist.github.com/a35cff8a4f139f7e475e#file-my-attempt-L265-L387.
If the reactions are slow, then it’s because you don’t trust the accelerometer enough. Try to increase this value: https://gist.github.com/a35cff8a4f139f7e475e#file-my-attempt-L77

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

@Dustan
No problem. Don’t worry about it. I’m just happy that people use my code 🙂

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? http://www.bikemaster.ru/catalog/motor_crystalyte_hs24.html?ELEMENT_CODE=motor_crystalyte_hs24&ELEMENT_ID=100930&SECTION_ID=3365

@boyfrind
I have no idea sorry. I have never tried anything like that!

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!

@boyfrind
What do you mean? I can’t understand your comment, sorry..

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

@Peter Seddon
Simply replace:

with:

Or you could put the library in the library folder and then it should be able to see it.

Hi,
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.

Thanks,
Shane

@Shane Pentz
Hi,
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: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L261-L277.

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

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:

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…

http://www.ebay.com/itm/TTL-Bluetooth-Module-Micro-Controllers-Arduino-Easy-Wireless-Serial-UART-/330780911604?_trksid=p5197.m1992&_trkparms=aid%3D111000%26algo%3DREC.CURRENT%26ao%3D1%26asc%3D14%26meid%3D5341563163173169395%26pid%3D100015%26prg%3D1006%26rk%3D1%26sd%3D330780911604%26

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.

Thanks-again,
Shane

@Lauszus

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?

Thanks.

@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: http://www.circuitsathome.com/mcu/using-usb-bluetooth-dongle-on-arduino-to-connect-to-android-phone
I recommend starting out with the example code I wrote until you get familiar with how it works: https://github.com/felis/USB_Host_Shield_2.0/blob/master/examples/Bluetooth/SPP/SPP.ino

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:

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.

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.

@Shane Pentz
Here a two pages to get you started: http://www.jayconsystems.com/tutorial/btmatemod/ and http://jondontdoit.blogspot.dk/2011/11/bluetooth-mate-tutorial.html. 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.

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?

Thanks…

@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: https://play.google.com/store/apps/details?id=mobi.dzs.android.BluetoothSPP

@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: http://www.circuitsathome.com/mcu/using-usb-bluetooth-dongle-on-arduino-to-connect-to-windows-pc. It’s basically the same procedure if you are using your own Bluetooth module.

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.

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

I thought the pin was set in the firmware of the bluetooth module, not within the sketch.

@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: https://github.com/felis/USB_Host_Shield_2.0/blob/master/SPP.cpp#L51-L52.

Which will then check if the pin is set like so: https://github.com/felis/USB_Host_Shield_2.0/blob/master/BTD.cpp#L451-L458 and then send the command: https://github.com/felis/USB_Host_Shield_2.0/blob/master/BTD.cpp#L899-L942. Just ignore the part for the wii controller. This is the part that reads the pin: https://github.com/felis/USB_Host_Shield_2.0/blob/master/BTD.cpp#L932-L939

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.

-Shane

@Shane Pentz
Nice. Looking forward to see the result! 🙂

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

@Dustan
Simply replace SerialBT.read() with Serial.read().
Also change this line: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L225 to:

You also have to replace these lines: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L122 and https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L133.
With:

It’s should be pretty easy!

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.

@getSurreal
Have you remembered to comment out this line as well: https://github.com/felis/USB_Host_Shield_2.0/blob/master/BTD.cpp#L19?

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!

@getSurreal
Thanks for your feedback. We will differently think about doing something like that!

@Lauszus

I did comment that one. Am I loading more than I need to maybe?

@getSurreal
Yes, if you only want to use the PS3 controller, then remove the SPP library.

@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 https://gist.github.com/natsud1/bcfae5b19e6272912b3e 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