Changes to Map.cpp and main.cpp
Merge branch 'devel_week5' of https://github.com/fernandomdo/roomba_pp into devel_week5