navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1