Added robot.cpp and Makefile and mappath.py algorithm