Adding teleop test code