very urgent help needed regarding algorithm for self balancing of two wheeled robot

[Molten]

From what I know about dynamics here is a picture of what is happening.


....................................._____________
.....................................\............ .............\
......................................\........... ...............\
.......................................\.......... ..C............\
........................................\......... ....---..........\
.........................................\.......--.......--.........\
......................<---------- ...\__.|....O.....|_____\
.................................................. --......--
.................................................. ....---


Ok, so basically. The C is the center of mass, O is the center of the axle, and U is the distance between C and O. There is a moment created by gravity anytime that C isn't directly over O. The accelerometer provides you with an angle that can be used with geometry to find the displacement in the horizontal plane. The formula for the moment created is mass*gravity*displacement. Now, you have the amount of torque that needs to be corrected against. The way you plan on fixing this is by adding another moment in the opposite direction created by the movement of the axle. The only way this will work well is if the axle supports the frame with bearings so that the actual turning does not create a torque about the axle. All you want is a push along the arrow marked above. Now, the way to calculate this moment is to use the angle found by the accelerometer and geometry to find the vertical displacement between the axle and the C. This moment is going to be displacement*pushing force. The pushing force is going to be mass*gravity*mu*pi*diameter of wheel*rpm. The rpm may take a sensor of sorts.(not sure on that part) So, in essence:

mass*gravity*mu*pi*U*sin(90-N)*diameter of wheel*rpm=mass*gravity*U*cos(90-N)
I go ahead and cancel the mass, gravity and U. This leaves:

sin(90-N)*diameter of wheel*rpm=cos(90-N)

So, rpm=cot(90-N)/diameter of wheel.

OK, now for the disclaimer: All the math posted above was done at 3 in the morning while watching tv and being half asleep. I am not by any means confident in all of the math I did. I think that it should give you a rough estimate of what you are looking for. Anyone that can confirm or improve my work would be greatly appreciated.