Vision code works now!
Need to optimize camera intialization, as it takes six seconds.
Minor changes, bugfixes
Updates and things