Mindsensors LineLeader Average Position Calculation

The mindsensors LineLeader is a great sensor for following a line. It uses an array of 8 sensors, and 9 LEDs to see the line. Not only does it allow you access to the scaled values from the sensors, but it also gives you access to some other information.

There is an internal threshold of 40 (or there-about), and any sensor below that threshold will be represented by a boolean value of 1. The boolean value of all the sensors is stored in a single byte for user-access.

There is also a value you can access that ranges from 10-80 (by 5’s), that represents the position of the sensor compared to the line. 0 means that it doesn’t see the line at all. This range is based on the boolean values. It calculates the value by weighting each of the boolean values, combining the weights, and dividing by the sum of the bits.

Here is the weight of the bits:

bit 0 = 10, bit 1 = 20, bit 2 = 30, bit 3 = 40, bit 4 = 50, bit 5 = 60, bit 6 = 70, bit 7 = 80.

So if you add up all the bits that are true, and divide by the sum of them, you get the average position on a scale of 10-80, with a resolution of 15 values plus 0 for all false.

For the sake of an example, lets say the sensors have these values (based on being scaled from 0-100 from black to white):
100, 100, 100, 100, 45, 32, 21, 93

So, comparing those values to the threshold of 40, sensors 5 and 6 (of 0-7) are true, so the boolean byte would be 96 (0x60). Now, add up the weight of those. 60 + 70 = 130. Then divide by the number of true bits. 130 / 2 = 65. This works fine, but I want better resolution (just ’cause I can get it ;)).

Now, remember what I said about the scaled values being compared to 40 (the threshold)? Well that’s all fine and good, but the LL reads more than just a single bit for each of the 8 sensors, and I think it’s reasonable to take advantage of that.

Just the other day someone explained how he would do it, and it made perfect sense, so I decided to try it. Take that example I just gave of bits 5 and 6 being true, what about the values of sensors 4 and 7? Surely those could have been effected (and in the example I gave them values of 45 and 93), even though they are not < the threshold of 40. Furthermore, they will be effected fairly inversely from each other based on the position. So if we scale the values of sensors 4 and 7 to 0-5 on top of the threshold, we can subtract one from the other to see the proportion between them, and add that to the weighted average.

(5-0) / (100-40) * (Value-40) – 0 or 5/60 * (Value-40) gives us the value scaled to 0-5 on top of the threshold. Now, take those new values of 0-5 (sensor 4 = 0.417, and sensor 7 = 4.417), and subtract one from the other. 0.417 – 4.417 = -4. Now, add that -4 to the weighted average of 65 that we calculated earlier, and you get 61. As you can see, the dark is effecting sensor 4 a lot more than sensor 7, so it makes sense that the “line” is further on the lower side (and the number 61 is less than 65).

I made a library function that does all of the math. I included that function as well as a couple other necessary ones in this example program:

float ScaleRange(float Value, float ValueMin, float ValueMax, float DesiredMin, float DesiredMax)    //Seems to work properly now!
{
  float Result = (DesiredMax - DesiredMin) / (ValueMax - ValueMin) * (Value - ValueMin) + DesiredMin;
  return Result;
}

int LL_ReadRaw_Calibrated (byte port, byte i2cAddr, byte &returnValue[])
{
  byte message[];
  unsigned byte buf[8];
  int count, l;
  byte nByteReady = 0;

	// calibrated readings are at 0x49
  ArrayBuild(message, i2cAddr, 0x49);
  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);
  count = 8;
  if(I2CBytes(port, message, count, returnValue)) {
	  return 1;
	}
	return 0;
}

float LL_Get_Average(byte port, byte addr, byte &Bool_LL_Values, byte &LL_Array_Values[]){
  const byte LL_Threshold=40;                                                   //To compare the calibrated values to. LL internal default is 40
  Bool_LL_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

  LL_ReadRaw_Calibrated (port, addr, LL_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_LL_Values = Bool_LL_Values | ((LL_Array_Values[i]<=LL_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_LL_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;                                                      //Of 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_LL_Values>>high_bit) & 0x01 ){                                   //Determine the bit just above the highest set bit
    high_bit--;
    if(high_bit==-1)break;
  }
  high_bit++;

  until( (Bool_LL_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(LL_Array_Values[low_bit], LL_Threshold, 100, 0, 100)-ScaleRange(LL_Array_Values[high_bit], LL_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(LL_Array_Values[low_bit], LL_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(LL_Array_Values[high_bit], LL_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(LL_Array_Values[0]<LL_Array_Values[7]){                                  //More detection on the low side
      Weighted_Position_Adder = ScaleRange(LL_Array_Values[0], LL_Threshold, 100, 10, 5);
    }
    else{
      if (LL_Array_Values[0]>LL_Array_Values[7]){                               //More detection on the high side
        Weighted_Position_Adder = ScaleRange(LL_Array_Values[7], LL_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;
}

byte LLArrayValues[];
byte LLBoolValues;
int LLAverage;

task main(){
  SetSensorLowspeed(S1);
  while(true){
    LLAverage=LL_Get_Average(S1, 0x02, LLBoolValues, LLArrayValues);
    ClearScreen();
    NumOut(0, LCD_LINE1, LLAverage);
    for (int d_i = 0; d_i<8; d_i++){
      NumOut(18, LCD_LINE1-8*d_i, LLArrayValues[d_i]);
    }
    for (int d_i = 0; d_i<8; d_i++){                                            //Repeat 8 times
      NumOut(44, LCD_LINE1-8*d_i, LLBoolValues>>d_i&0x01);                      //Display all 8 bits
    }
    NumOut(54, LCD_LINE1, LLBoolValues);

    Wait(50);
  }
}

Note, that the LL_ReadRaw_Calibrated function was taken directly out of the mindsensors LL-lib.nxc library, and has not been modified.

Using this method of getting the average, the values have now become 5-86. To make this range match the mindsensors LineLeader range of 10-80, I scaled the values.

I know the program isn’t very optimized, but it works fine.

Advertisements
This entry was posted in Drivers, Mindsensors, Mindstorms, NXC, NXT and tagged , , , , , . Bookmark the permalink.

2 Responses to Mindsensors LineLeader Average Position Calculation

  1. bruno says:

    hi, where you buy this sensor?

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