Updated so it's using the changed OccupancyGrid.cs