Code for doing SLAM (Simultanous Localization and Mapping) using Lego Mindstorms