ROB-15: Add new vision code and add it to autonomous