Eerste werkende versie van A* pathfinder implementatie

TODO:
Mijnen
Extra heuristische waarde voor bochten
Compatibiliteit met robot