Began rewriting vision code to work in SmartDashboard

Tried and failed to make the old vision code work