Added some basic vision plan code
Set up plans for vision/control