Camera refactored completly, working on robot