rewrite of teleop code (mostly working!)