Fix setLedToColor function and add some testing code.