Mindsensors Light Sensor Array

Mindsensors is working on a new NXT sensor, called a Light Sensor Array. It’s similar in form factor to the LineLeader that they have sold previously, but it doesn’t have a PID algorithm built in. Instead, it just gives you access to the individual light sensors, and their binary states. The reason for this, is that the LineLeader was disqualified from many competitions, since it was too much of a “canned” line follower (it did all the necessary calculations on board).

I made a function that you can use to determine where a line is compared to the sensor. It returns a number from 10-80 based on where it “sees” the line. 45 is the middle, so you can compare the position to 45 to determine the offset from being centered on the line. 0 represents no line detected. Use this in conjunction with the LSA library provided by mindsensors.

float ScaleRange(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)
{
  return (DesiredMax - DesiredMin) * (Value - ValueMin) / (ValueMax - ValueMin) + DesiredMin;
}

float LSA_Get_Average(byte port, byte addr, byte & Bool_LSA_Values, byte & LSA_Array_Values[], byte LSA_Threshold = 40){
  Bool_LSA_Values = 0;                                                          // Reset from last state
  int bit_index_i = 0;                                                          // The bit it's working on
  int averaging_weight_i = 10;                                                  // The weight to add the the Weighted_Position, if the bit is 1
  byte total_bits = 0;                                                          // The number of bits that are true. This is needed for the division at the end
  bool set_bit;                                                                 // The bit it's working on
  char low_bit = 0;                                                             // The bit just below the lowest 1
  char high_bit = 7;                                                            // The bit just above the highest 1

  LSA_ReadRaw_Calibrated(port, addr, LSA_Array_Values);                         // Fill LL_Array_Values with the calibrated values of the 8 sensors

  for (byte i = 0; i<8; i++){                                                   // Build a byte with the bits set to the boolean state of the sensors
    Bool_LSA_Values = Bool_LSA_Values | ((LSA_Array_Values[i]<=LSA_Threshold?1:0)<<i);
  }

  float Weighted_Position = 0;                                                  // The value that will contain the overall weighted avarage
  float Weighted_Position_Adder = 0;                                            // The value that will contain the average adder, based on the effected, but not true sensors
  float Absolute_Average = 0;

  repeat(8){
    set_bit = Bool_LSA_Values >> bit_index_i & 0x01;                            // Seperate the bits out of the byte
    Weighted_Position += set_bit * averaging_weight_i;                          // Add the 1's to the value
    total_bits += set_bit;                                                      // If the bit was 1, add 1 to the counter (divide by the sum at the end).
    averaging_weight_i+=10;                                                     // Set the variables for the next round
    bit_index_i++;                                                              //                    ''
  }

  until((Bool_LSA_Values >> high_bit) & 0x01){                                  // Determine the bit just above the highest set bit
    high_bit--;
    if(high_bit==-1)break;
  }
  high_bit++;

  until((Bool_LSA_Values >> low_bit) & 0x01){                                   // Determine the bit just below the lowest set bit
    low_bit++;
    if(low_bit==8)break;
  }
  low_bit--;

  if (0 < high_bit && high_bit < 8 && -1 < low_bit && low_bit < 7){             // Both ends can be used
    Weighted_Position_Adder = (ScaleRange(LSA_Array_Values[low_bit], LSA_Threshold, 100, 0, 100) - ScaleRange(LSA_Array_Values[high_bit], LSA_Threshold, 100, 0, 100)) / 20;
  }
  if(high_bit == 8 && low_bit == -1){                                           // According to logic, blacked out totally
    return 45;
  }
  else{
    if(high_bit==8){                                                            // Black off the high side, but not totally blacked out
      Weighted_Position_Adder = ScaleRange(LSA_Array_Values[low_bit], LSA_Threshold, 100, -2.5, 2.5);//(100/(100-Threshold)*lower/20)-5;
    }
    if(low_bit==-1){                                                            // Black off the low side, but not totally blacked out
      Weighted_Position_Adder = ScaleRange(LSA_Array_Values[high_bit], LSA_Threshold, 100, 5, 0);//((100/(100-Threshold)*higher/20)-5) *(-1);
    }
  }
  if(low_bit==7&&high_bit==0){                                                  // According to logic, whited out completely
    if(LSA_Array_Values[0]<LSA_Array_Values[7]){                                // More detection on the low side
      Weighted_Position_Adder = ScaleRange(LSA_Array_Values[0], LSA_Threshold, 100, 10, 5);
    }
    else{
      if (LSA_Array_Values[0]>LSA_Array_Values[7]){                             // More detection on the high side
        Weighted_Position_Adder = ScaleRange(LSA_Array_Values[7], LSA_Threshold, 100, 0, 5) + 82;
      }
    }
  }

  //NumOut(54, LCD_LINE5, Weighted_Position_Adder);                             // The average's adder calculated by the closest non-true bytes

  if (Weighted_Position/total_bits+Weighted_Position_Adder){ //Not equal to 0
    Absolute_Average = ScaleRange(Weighted_Position/total_bits+Weighted_Position_Adder, 5, 86, 10, 80); // Avaragetotal divided by the number of bits high, plus the adder. Then scale this from 0-82 to 0-80 so it is the same as the LL native average.
  }
  return Absolute_Average;
}
Advertisements
This entry was posted in Drivers, Mindsensors, Mindstorms, NXC, NXT and tagged , , , , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s