Resolve bug

When the robot had to search the shortest path from 0,0 to a negative pos, the robot didn't find anything, the problem was in stop condition and heuristic function. Now looks ok