Added vision code, in theory

This code was ripped blatantly from the vision sample provided by FRC
with minor changes to work in our code. That said, the code will most
likely work, and can be used by declaring a Vision object, than calling
the doVisionCalc() method. Of course, more tweaks and changes are
necessary to make it work all the way.