Programming NXT color sensor with NXC?

Question

Currently I am working Lego robot that solves 2x2x2 Rubik's cube. I am programming Lego Mindstorms NXT 2.0 color sensor in BricxCC.

I already have RGB data with the command ReadSensorColorRaw. I heard that it is easier to get the colour from HSL format. I've tried a lot of algorithms, but none succeeded. How I can fix it to work?'

Also I heard that I need to calibrate white balance. How can I do this??

Answer 1

Since you are trying to solve a Rubik's cube, check out the source code for the MindCuber. The solution there is that the values are not actually converted into colors. Rather the raw values are converted to HSV and then sorted by hue. The actual value does not matter, just how close it is to the other values.

The Mindcuber also has a white calibration. This is used to identify the white values and removed them from the list before sorting which is useful since white doesn't have a meaningful hue value.

Answer 2

I'm not entirely sure if you need the raw data, but if you're just looking to read the different colour values, there is a much easier way.

To turn on and set up the sensor, use SetSensorColorFull(S3);. Then to retrieve the value just use Sensor(S3). The value returned will be an integer from 1 to 6, corresponding to the following colour values:

1 = black
2 = blue
3 = green
4 = yellow
5 = red
6 = white

Here is a sample program that will read the current value from the colour sensor and print the colour name on the NXT screen:

task main() {
    string sColor;
    int nColor;

    // Turn on the sensor
    SetSensorColorFull(S3);

    while (1) {

        // Read the numerical color value from the sensor
        nColor = Sensor(S3);

        // Determine the color name from its number
        switch(nColor) {
            case 1: sColor = "Black";
            case 2: sColor = "Blue";
            case 3: sColor = "Green";
            case 4: sColor = "Yellow";
            case 5: sColor = "Red";
            case 6: sColor = "White";
            default: sColor = "???";
        }

        // Print the color name to the LCD screen
        ClearScreen();
        TextOut(0, 56, NumToStr(nColor) + ". " + sColor);

        // Wait until the color changes
        while( Sensor(S3) == nColor ); 
    }
}

Answer 3

The following works if you just want the builtin six/seven colors, without any calibration.

task main() {
  SetColorSensorFull(S3);
  while(true){
    int value=Sensor(S3);
    // value now contains 1 for black, 2 for blue, 3 for green, 4 for yello, 5 for red, 6 for white, and something else for an unknown color.
  }

However, if you want to do proportional control, want more colors, or aren't satisfied with the builtin thresholds, you'll need to use the RGB or HSL values. Here's RGB (Threshold is of course, my own defined struct and my solution to this problem):

struct Threshold{
  int minR, minG, minB;
  unsigned int maxR, maxG, maxB;
};

bool InRange(ColorSensorReadType &csr, Threshold &thresh){
  bool in=true;
  in&=csr.NormalizedArray[0]<=thresh.maxR;
  in&=csr.NormalizedArray[1]<=thresh.maxG;
  in&=csr.NormalizedArray[2]<=thresh.maxB;
  in&=csr.NormalizedArray[0]>=thresh.minR;
  in&=csr.NormalizedArray[1]>=thresh.minG;
  in&=csr.NormalizedArray[2]>=thresh.minB;
  return in;
}

Threshold Red={180,20,20,250,130,130},
Green={90,160,90,160,255,160},
Blue={40,100,160,80,150,200},
White={210,210,210,300,300,300},
GroundBlack={0,0,0,150,150,150},
GroundGreen={120,180,120,170,220,160};

task main(){
  SetSensorColorFull(S2);
  ColorSensorReadType csr;
  csr.Port=S2;

  while(true){
    SysColorSensorRead(csr);
    ClearScreen();
    NumOut(0,LCD_LINE1,csr.NormalizedArray[0]);
    NumOut(0,LCD_LINE2,csr.NormalizedArray[1]);
    NumOut(0,LCD_LINE3,csr.NormalizedArray[2]);

    if(InRange(csr,Blue)){
      TextOut(0,LCD_LINE5,"BLUE!");
    }else if(InRange(csr,Green)){
      TextOut(0,LCD_LINE5,"GREEN!");
    }else if(InRange(csr,White)){
      TextOut(0,LCD_LINE5,"WHITE!");
    }

    Wait(10);
  }
}

If you want to transform into HSV values, try this:

struct Threshold{
  int minR, minG, minB;
  unsigned int maxR, maxG, maxB;
};

struct HSVThreshold{
  int minH, minS, minV;
  unsigned int maxH, maxS, maxV;
};

struct HSVColor{
  int hue, sat, val;
}
void RGBToHSV(ColorSensorReadType &csr, HSVColor &color){
  int maxColor=csr.NormalizedArray[0], minColor=csr.NormalizedArray[0];
  for (int i=1;i<3;i++) {
    if (maxColor<csr.NormalizedArray[i]) maxColor = csr.NormalizedArray[i];
    if (minColor>csr.NormalizedArray[i]) minColor = csr.NormalizedArray[i];
  }
  if (minColor==maxColor) {
    color.val=minColor;
    color.sat=0;
    color.hue=0;
  } else {
    int d = (r==minColor) ? g-b : ((b==minColor) ? r-g : b-r);
    int h = (r==minColor) ? 3 : ((b==minColor) ? 1 : 5);
    color.hue=60*(h-d/(maxColor-minColor));
    color.sat=(maxColor-minColor)/maxColor;
    color.val=maxColor;
  }
}

bool InRange(ColorSensorReadType &csr, Threshold &thresh){
  bool in=true;
  in&=csr.NormalizedArray[0]<=thresh.maxR;
  in&=csr.NormalizedArray[1]<=thresh.maxG;
  in&=csr.NormalizedArray[2]<=thresh.maxB;
  in&=csr.NormalizedArray[0]>=thresh.minR;
  in&=csr.NormalizedArray[1]>=thresh.minG;
  in&=csr.NormalizedArray[2]>=thresh.minB;
  return in;
}
bool InRange(HSVColor color, HSVThreshold &thresh){
  bool in=true;
  in&=color.hue<=thresh.maxH;
  in&=color.sat<=thresh.maxS;
  in&=color.val<=thresh.maxV;
  in&=color.hue>=thresh.minH;
  in&=color.sat>=thresh.minS;
  in&=color.val>=thresh.minV;
  return in;
}

Threshold Red={180,20,20,250,130,130},
Green={90,160,90,160,255,160},
Blue={40,100,160,80,150,200},
White={210,210,210,300,300,300},
GroundBlack={0,0,0,150,150,150},
GroundGreen={120,180,120,170,220,160};

// Define your own HSVThresholds here

task main(){
  SetSensorColorFull(S2);
  ColorSensorReadType csr;
  csr.Port=S2;

  while(true){
    SysColorSensorRead(csr);
    HSVColor color;
    RGBToHSV(csr,color);

    ClearScreen();
    NumOut(0,LCD_LINE1,color.hue);
    NumOut(0,LCD_LINE2,color.sat);
    NumOut(0,LCD_LINE3,color.val);

    Wait(10);
  }
}