First try of code involving all subsystems

Dependencies:   Motor Servo TCS3472_I2C mbed