Dijkstra aangepast, aantal watpoints zitten er nog niet in. De avgs juist aangeroepen en de juist x y cordo. Verder methode voor move agv aangepast, er voorgezorgd dat hij de juiste begin punt weer. Eind punt is nu ook zo goed als af (alleen de dockingcranes moet nog even juist bepaald worden)
Merge pull request #67 from pasibun/Move_command

Dijkstra aangepast, aantal watpoints zitten er nog niet in. De avgs juis...