Path finding fixed!!

original message: "Seems like it's working!!!!!!!!!!"

Was iterating twice on a loop... >_<