static void REF_Measure(void) {
ReadCalibrated(SensorCalibrated, SensorRaw);
refCenterLineVal = ReadLine(SensorCalibrated, SensorRaw, REF_USE_WHITE_LINE);
#if PL_CONFIG_HAS_LINE_FOLLOW
refLineKind = ReadLineKind(SensorCalibrated);
#endif
}
void REF_Measure(void) {
ReadCalibrated(SensorCalibrated, SensorRaw);
#if PL_APP_LINE_MAZE
REF_SampleHistory();
#endif
refCenterLineVal = ReadLine(SensorCalibrated, SensorRaw, FALSE);
if (isCalibrated) {
refLineKind = ReadLineKind(SensorCalibrated);
}
}
// Operates the same as read calibrated, but also returns an
// estimated position of the robot with respect to a line. The
// estimate is made using a weighted average of the sensor indices
// multiplied by 1000, so that a return value of 0 indicates that
// the line is directly below sensor 0, a return value of 1000
// indicates that the line is directly below sensor 1, 2000
// indicates that it's below sensor 2000, etc. Intermediate
// values indicate that the line is between two sensors. The
// formula is:
//
// 0*value0 + 1000*value1 + 2000*value2 + ...
// --------------------------------------------
// value0 + value1 + value2 + ...
//
// By default, this function assumes a dark line (high values)
// surrounded by white (low values). If your line is light on
// black, set the optional second argument white_line to true. In
// this case, each sensor value will be replaced by (1000-value)
// before the averaging.
static int ReadLine(SensorTimeType calib[NOF_SENSORS], SensorTimeType raw[NOF_SENSORS], bool white_line) {
unsigned char i;
bool on_line = FALSE;
unsigned long avg; // this is for the weighted total, which is long
// before division
unsigned int sum; // this is for the denominator which is <= 64000
static int last_value=0; // assume initially that the line is left.
#define LINE_MIN_VAL 0x50 /* 200 */ /* minimum value indicating a line */
#define MIN_NOISE 16 /* 50 */
ReadCalibrated(calib, raw);
avg = 0;
sum = 0;
for(i=0;i<NOF_SENSORS;i++) {
int value = calib[i];
if(white_line) {
value = 1000-value;
}
/* keep track of whether we see the line at all */
if(value > LINE_MIN_VAL) {
on_line = TRUE;
}
/* only average in values that are above a noise threshold */
if(value > 50) {
avg += (long)(value) * (i * 1000);
sum += value;
}
}
if(!on_line) { /* If it last read to the left of center, return 0. */
if(last_value < (NOF_SENSORS-1)*1000/2) {
return 0;
} else { /* If it last read to the right of center, return the max. */
return (NOF_SENSORS-1)*1000;
}
}
last_value = avg/sum;
return last_value;
}
static void REF_Measure(void) {
ReadCalibrated(SensorCalibrated, SensorRaw);
}
