Not quite self-balancing robot

Hi,
I have built a self-balancing robot, which does not perform at all well.
While it will balance, it will oscillate quickly around vertical which is not bad in itself I guess, but this is even if the kP is very low. In addition, it cannot recover from quite small tilt angle.
I cannot see any significant conceptual difference between my code/build and various other examples on the internet.

Schematic: Attached (fritzing file and picture of fritzing schematic)
Code: Attached.
Video: 20170524 213104 001 - YouTube

What have I tried:
I originally took the accelerometer and gyro readings from the MPU and calculated the angles on the arduino.
I could not get it to balance at all this way, I think it was just too slow to respond, although the time taken to calculate the angle after receiving MPU data was ~800us which didn't seem too slow to me.
Anyway, I changed to using Jeff Rowberg's code to utilise the MPU's onboard DMP features and this made a huge difference - it can now balance a bit, but with the limitations described at the start.

I have spent a long time playing with the PID parameters. Most advice I could find on tuning the PID controller was roughly one of the following 2 approaches:

  1. increase P until it oscillates, then increase D to smooth, then increase I to correct for drift
  2. increase P until it oscillates (only a little), then increase I to correct for drift, then increase D to make it correct after a shock
    I've tried both, and I cannot seem to get any better than what you can see in the video. One problem is that when increasing P, it goes from not balancing at all to oscillating quickly - there doesn't appear to be much in the middle.
    So I'm not really sure where to go from there.
    I originally used my own PID controller, although I also tried Brett Beauregard's PID library (which is still in the current code) in case I'd done something wierd, but there was no difference.
    In the current code/video I and D are zero.

I have tried having the batteries right at the top, and a bit lower down (as they are now).
I have changed from 4xAA (~5V) to 6xAA (~7.5V) batteries - no change in balancing ability (it just meant that the wheels would turn at slightly lower PWM).
I was considering different motors but in this instructable (http://www.instructables.com/id/Self-Balancing-Robot/) the exact same ones have been used and it does not have my issue
I originally had the MPU6050 on the top of the robot but have now moved it down to be close to the axis of rotation as was suggested. This possibly gave a little improvement.

I've also tried playing with the

  • low pass filter setting
  • DMP sample/FIFO output rate
  • PID calculation interval

What I have not tried:
Encoders, but I'm not concerned about it holding position yet so I don't this this is relevant yet.
Kalman filter - but I've seem plenty of examples without it.
Changing PWM frequency (as done for Balanduino).

Can anyone give me any suggestions for what is causing this (if there's something I've done wrong or badly), or just ideas of things I could try and change? I am running out of ideas.

Any suggestions greatly appreciated.

self_balancing.ino (7.13 KB)

bms001:
I have spent a long time playing with the PID parameters. Most advice I could find on tuning the PID controller was roughly one of the following 2 approaches:

  1. increase P until it oscillates, then increase D to smooth, then increase I to correct for drift

I think I was the one who suggested that. My mistake, you increase I to smooth out the oscillations and increase D to reduce long-term offset.
link

Quick tip: From your vid, your kp value is much, much too low.

Also, you could benefit from using a complimentary filter (this is a low-pass on the accelerometer and a high-pass on the gyroscope data).

you increase I to smooth out the oscillations and increase D to reduce long-term offset.

Other way around. "I" stands for Integral, which averages and calculates the long term offset. Increase Ki to reduce the long term offset, but it is not likely to be useful with a balancing robot.

One problem is that when increasing P, it goes from not balancing at all to oscillating quickly - there doesn't appear to be much in the middle.

What do you mean by "middle"?

What were the actual values of Kp that you entered?

The Fritzing diagram is, as usual, utterly useless. One possibility is that the battery/motor driver cannot supply the required stall current, so the motor "wimps out". In such a case it will be impossible to tune the PID algorithm.

The following comment is not reassuring:   mpu.setZAccelOffset(1788); // this is what was in JR code - not sure what mine is or how to find out. You MUST understand if the motion processor is giving you the correct orientation.

Check this by putting in some print statements, for example to make sure that the measured tilt angle make sense.

      // Get the pitch angle
      actual = ypr[1] * 180 / M_PI;
      // Run PID

jremington:
Other way around. "I" stands for Integral, which averages and calculates the long term offset. Increase Ki to reduce the long term offset, but it is not likely to be useful with a balancing robot.

Oops. I get the balancing of I and D mixed up all the time.

jremington:
What were the actual values of Kp that you entered?

The code said kp=5.

bms001:
One problem is that when increasing P, it goes from not balancing at all to oscillating quickly - there doesn't appear to be much in the middle.

It's ok to have quick oscillations; that's what the D is for (to quiet the oscillations down).

Power_Broker:
Quick tip: From your vid, your kp value is much, much too low.

I had previously tried with much larger values, though perhaps not exhaustively. I will have another go.

Power_Broker:
Also, you could benefit from using a complimentary filter (this is a low-pass on the accelerometer and a high-pass on the gyroscope data).

I will try this.

jremington:
What do you mean by "middle"?

I'm not sure I can explain this well verbally beyond the fact that when increasing Kp it goes from not balancing at all (not even close) to oscillating quickly. In other PID tuning videos I've seen a smoother transition (which I have referred to as "middle"). If after trying to tune the ID again I still have this problem I will upload a video to demonstrate exactly clearly what I mean. Of course, it may be my expectations of what should happen are incorrect.

jremington:
What were the actual values of Kp that you entered?

In the code/video it is 5. Though I have been from 1 to 100.

jremington:
The Fritzing diagram is, as usual, utterly useless.

The fact that it's a picture or the fact that it's fritzing or something else. What would have been useful?
I did try to upload the actual fritzing file but that file type wasn't allowed. I didn't realise I could just zip it and upload so I will do that when I get home in case that adds anything.

jremington:
One possibility is that the battery/motor driver cannot supply the required stall current, so the motor "wimps out". In such a case it will be impossible to tune the PID algorithm.

The current motors (link) have 250mA max current which should be well within the capability of the batteries I believe.
The motor driver (link) should be able to provide 1A continuous. Beyond that I'm not sure how (without replacing it) to determine if it is a limitation.
If I cannot get everything working based on the other suggestions here, I will replace the battery and/or the driver. Do you have any suggestions of the characteristics I should look for?

jremington:
The following comment is not reassuring:

 mpu.setZAccelOffset(1788); // this is what was in JR code - not sure what mine is or how to find out

You MUST understand if the motion processor is giving you the correct orientation.

Err yes, I had meant to address this before posting. I think I played around with this figure to see what effect changing it had on my application (i.e. only using the pitch) and couldn't see any observable difference but I will sort this out asap.

jremington:
Check this by putting in some print statements, for example to make sure that the measured tilt angle make sense.

Yes, I removed all print/debug/random comments before posting to make it clearer to read (and tested behaviour was still the same of course).
The angle being output certainly makes sense to my eye, is very stable when not changing the tilt and doesn't jump really high when tilting fast, so it appears to me to be ok. I will certainly address the ZAccelOffset though.

Thank you both for your comments. I will try and make these changes and checks in the next few days (aside from changing battery/driver which will be a bit longer).

the fact that it's fritzing or something else. What would have been useful?

An actual schematic, with proper components and values. Fritzing diagrams are misleading and confusing at best, uninterpretable at worst.

I've been really short of time so it's taken until now to try everything. In fact, following the suggestion of increasing Kp a lot I got a huge improvement. (I think the only change I made since last trying large Kp values was to move the motion sensor closer to the axis of rotation, but perhaps this made enough difference)

Anyway, here is a video of it now.

I have also attached a proper (I hope) schematic (although please excuse where I have labelled the Arduino 5V pin as 5.5V).

I did also try and implement the complementary filter (this is actually what I started out using before using the DMP output from the MPU-6050) although had some funny results which I still need to sort out.

schematic2.zip (420 KB)