Added mapping, wall-following, imaging capabilities