Smarter Pathfinding

Signed-off-by: Seth Battis <SethBattis@stmarksschool.org>