Autonomous now works. Added vision code.