code with working vision

Vision is operational with this code.
Merge pull request #2 from GearheadsFIRST/ShifterGrabber

code with working vision