Dijkstra integration code added. Need to clean up code, finished the
dijkstra task. Also have dijkstra transferring directions array to the
state machine task.
Merge remote-tracking branch 'origin/master' into snap_tested
made some minor changes and fixes