Added test for vision

Added test function for the vision processing.  Test function also
writes the images to the cRIO memory for later inspection.