Finish teleop (untested), a few small bugs