Vielleicht passt die Kostenrechnung für diagonale Wege nicht?
Ich hab dieses Konzept übernommen:
http://pille.iwr.uni-heidelberg.de/~astar01/

Hab ein Feld mit 1000 Zellen, der Atmega berechnet den Weg ohne merkbare Verzögerung.

LG!