Some cleanup
Some code for color and object sensors, needs to be checked on real robot