Minor cleanup, along with some safety stuff.
This is our teleop. Oh yeah.

Except we don't have drive base code yet :P