Teleop finalization! Only some double-checking left

(and motor direction stuff, and PID, etc.)