Is this a snippet of the code you sent me 2 months ago?
I would suggest to solve this not by short-distance recognation and turning but with long-distance recognation and calculating a valid path for the entities.
black=block
red=pathwalker
green=destination
blue=long range path scan
lila=valid path
yellow=smoothed path of the pathwalker
mercuryus