blob: 48590cc5391dde77925dd44e1a8409c5255a5b3e (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
|
#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;
}
|
