works and balances pretty well.
P went down
I went up
cleanup organize
hysteresis to reanable the balacing when we are very close to vertical