the path finding works in a way - it calculates the next command
I will try out if it is the correct one
important change - the degrees are no longer calculated in radiant
added comments to PathFinder
the path finding (better the command finding) seems to work quite well