Touching up

Fixing some issues mostly with teleop, and some reformatting for
autonomous