ironbot:
PeterH, I'm little bit confused that you say we don't need accel, actually without filtering, if using only gyro output, the big drift I'd have and change of set-point that happens, I get nothing, I tried it long before this experiment!It is impossible in my practice to not to combine output of gyro and accel, and yet balance the bot! I've read so many works (of other people already done it) and everybody is doing it this way, writing that impossible to use only gyro!
Of course, I'm in no way on an authority to claim anything in control, just I report what I've read. Actually if you personally can do it, is because of your skills that I unfortunately don't have, all i have is a background in math and a lot of reading online about this project, and still with a bot that can't balance really
If you are in contact with somebody who has actually done it, then follow their advice. I haven't - I am only telling you my understanding of the theory.
In my opinion, the accelerometer will not do what you intend it to do. The tilt calculations are based on measuring very small lateral accelerations that occur when the sensor is not perfectly level. This only works on a stationary object, and of course your bot is not stationary. As soon as your bot starts to move sideways the sensor will get a big horizontal acceleration that swamps the tiny signal you were using to calculate the direction of gravity.
Now, you can try using that instead of calculating the lean angle, if you like. As soon as the bot starts to fall to the left you move the wheels in the direction it's falling. To get that working reliably you'd really need to be able to work out and control the acceleration caused by your motors and model the relationship between the movement of the base and the top of your bot. Although it would be possible to do, it'd look nothing like the algorithm you have got there and would be quite hard to get working.
I think you are better off with the approach you're using, of trying to measure the orientation of the bot. The primary sensor for doing this is the gyro. You have it sitting in front of you; I don't. What signal do you get off the gyro in practice? What is the minimum angular rate you can read from it, and how much latency is there in reading it?
You are right that you need to cope with drift. If it was my project, the way I'd do that was to add a second feedback loop which tried to drive the amount of correction towards zero. In effect it would self-calibrate towards the vertical at startup, and inherently deal with gyro frift. But there's no point thinking about that until you have got the basic underlying feedback loop working. And that depends on know what your sensors are telling you. Hence I keep asking the question: what are you getting from that gyro?