Most efficient path finder for a grid

Not my code- from http://www.robotc.net/blog/2011/08/08/robotc-advanced-training/ -might be useful for us to find the kickstand based on positioning on the playing field.