I love reading code for great robots with a high level of automation. You can tell just how much time was spent optimizing everything.
One question. I noticed in your autonomous driving code, you do some compensation for voltage level:
Code:
Vect output = new Vect(xOut*12.5/voltage, yOut*12.5/voltage);
Did you find that this made a significant difference in your application as opposed to just outputting xOut and yOut directly? It looks like you had both modes available, so you must have done some sort of comparison.