Teleop mode working okay