Adds preliminary shortest path code. Still needs to check for ultrasonic blocks