// Color sensor test program.
// Displays 3 digits on LCD, corresponding to RVB reflected levels.
// Requires RCX2 firmware

// Color sensor is connected to RCX sensor 2 input
#define Color SENSOR_2

// Depending on sensitivity of your color sensor, scale factor may need to be modified
#define ScaleFactor 38
// Depending on sensitivity of your color sensor, offset may need to be modified
#define Offset 980

// Defines the value above which you are on sensor key position
#define DummyThreshold 1005

int Red, Green, Blue;
int RGB, RGB1;

task main ()
{
    SetSensor (Color, SENSOR_LIGHT);
    SetSensorMode (Color, SENSOR_MODE_RAW);

    InitColSensor();
    SetUserDisplay (RGB1,2);
    while (true)
    {
          ReadColor();
          RGB=Red/ScaleFactor*100 + Green/ScaleFactor*10 + Blue/ScaleFactor;
          RGB1=RGB;
    }
}

// Advance to next color till we reach dummy position where reading IS 0
void InitColSensor ()
{
    while (Color < DummyThreshold)
  {
    NextColor ();
  }
}

// Reads all three color components and place them in Red, Green and Blue variables
sub ReadColor ()
{
    NextColor ();
    Red=Offset - Color;
    NextColor ();
    Green=Offset - Color;
    NextColor ();
    Blue=Offset - Color;
    //Skip Dummy
    NextColor ();
    if (Color < DummyThreshold)  InitColSensor();
}

// Advance to next color, sends a clock pulse by
// changing sensor type to passive mode for a short time
void NextColor ()
{
    SetSensor (Color, SENSOR_LIGHT);
    SetSensorType (Color, SENSOR_TYPE_TOUCH);
    SetSensor (Color, SENSOR_LIGHT);
    SetSensorMode (Color, SENSOR_MODE_RAW);
}