Creating the quickest route to a target

Not my code. From "http://www.robotc.net/blog/2011/08/08/robotc-advanced-training/" Could be useful for reaching the kickstand or rolling goals in autonomous mode.