Revert "Update Util.asm"

This reverts commit 3d4011e681afb7f008272b957880b1fe18589e8b.
Factored out some ShortestPath code.

To shrink binary size.
Merge branch 'master' of https://github.com/ECE2031Untitled/de2bot-pathfind