Pathfinding

Added pathfinding to the maze. It checks after each turn and seems to
work with all the trials I ran with it. Not the most effecient
algorithm, but it works.