Added some of the logic from Zumo maze solver modified to fit new algorithm