self balancing pid?

No one will be able to help, unless you post all the code.