Many fixes!

There's still some while loops that need removed in autonomous, so not
stable.