Maze solving works, but robot can get stuck if he cant get to the goal