Sorted vision code into functions (mostly)

TODO:
-get live images from the camera
-green ball
-harder (or just less relevent) stuff (green button vs green ball, detect tower, yellow wall,
etc)