When an unit is on or walking on a cell, that cell should be marked 1 (not walkable for others) in the map.

Since the filled cells will change constantly while units are walking, it looks like they have to do a pathFinder after each step. But if the destination is already 1, it should try to do a "spiral" search from the destination cell to find the nearestFreeCell.

If the unit(s) are already moving, and you give them a new target, the game will crash, as they are unable to do corrections..