70 lines
1.4 KiB
C++
70 lines
1.4 KiB
C++
#include "LineSensorArray.h"
|
|
|
|
LineSensorArray::LineSensorArray(LineSensor *sensors[5])
|
|
{
|
|
for(int i = 0; i < 5; i++)
|
|
this->sensors[i] = sensors[i];
|
|
|
|
line = 0.0;
|
|
sharpness = 0.0;
|
|
gw = 0.0;
|
|
|
|
min_diff = 0.0;
|
|
max_white_diff = 0.0;
|
|
}
|
|
|
|
LineSensorArray::LineSensorArray(LineSensor *sensor1, LineSensor *sensor2, LineSensor *sensor3, LineSensor *sensor4, LineSensor *sensor5)
|
|
{
|
|
sensors[0] = sensor1;
|
|
sensors[1] = sensor2;
|
|
sensors[2] = sensor3;
|
|
sensors[3] = sensor4;
|
|
sensors[4] = sensor5;
|
|
|
|
line = 0.0;
|
|
sharpness = 0.0;
|
|
gw = 0.0;
|
|
|
|
min_diff = 0.0;
|
|
max_white_diff = 0.0;
|
|
}
|
|
|
|
void LineSensorArray::update() {
|
|
int values[5];
|
|
int nBlack = 0;
|
|
bool strange = false;
|
|
|
|
|
|
for(int i = 0; i < 5; i++) {
|
|
sensors[i]->updateValue();
|
|
if((values[i] = int(isSensorBlack(i))) == 1)
|
|
nBlack++;
|
|
else if(nBlack > 0)
|
|
strange = true;
|
|
}
|
|
|
|
if(nBlack == 0) {
|
|
line = 0.0;
|
|
sharpness = 0.0;
|
|
}
|
|
else {
|
|
line = (-2*values[0]-1*values[1]+values[3]+2*values[4])/(float)nBlack;
|
|
|
|
if(strange) sharpness = -1.0;
|
|
else sharpness = 1/(float)nBlack;
|
|
}
|
|
}
|
|
|
|
bool LineSensorArray::calibrate() {
|
|
float white = (sensors[1]->getValue() + sensors[3]->getValue())/2;
|
|
|
|
if((sensors[2]->getValue() - white) < min_diff) return false;
|
|
if(ABS(sensors[1]->getValue() - sensors[3]->getValue()) > max_white_diff) return false;
|
|
|
|
if(gw == 0.0)
|
|
gw = sensors[2]->getValue()/2 + white/2;
|
|
else
|
|
gw = sensors[2]->getValue()/4 + white/4 + gw/2;
|
|
|
|
return true;
|
|
}
|