ROB-16: Add diagnostics class and implement vision