Finish the robot side code for Sparki Mapper. Fix some bugs.