Dijkstra

Adding dijkstra's algorithm used in the robotic classes