static const size_t AVG_DIFF_BUFFER_SIZE 50 double getDiff double left

 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
static const size_t AVG_DIFF_BUFFER_SIZE = 50;
double getDiff(double left, double right)
{
return max(left, right) - min(left, right);
}
int main()
{
Servo::init();
initDiff();
sei();
int avgDiffBuffer[AVG_DIFF_BUFFER_SIZE];
for (size_t i = 0; i < AVG_DIFF_BUFFER_SIZE; ++i)
{
avgDiffBuffer[i] = 0;
}
double bufferSum = 0;
unsigned avgDiff = 0;
size_t bufferIndex = 0;
while (true)
{
while (!angleApplied);
unsigned left = readLeft();
unsigned right = readRight();
int diff = int(left) - right;
double prev = avgDiffBuffer[bufferIndex];
bufferSum -= prev;
bufferSum += diff;
avgDiffBuffer[bufferIndex] = diff;
bufferIndex = (bufferIndex + 1) % AVG_DIFF_BUFFER_SIZE;
avgDiff = bufferSum / AVG_DIFF_BUFFER_SIZE;
int shift = remap(abs(avgDiff), 0, 1024, 1, 50);
if (min(left, right) > 800 && max(left, right) - min(left, right) < 100)
{
continue;
}
if (left > right)
{
angle = max((angle - shift), 0);
}
else if (right > left)
{
angle = min((angle + shift), PULSE_MAX_WIDTH - 1);
}
setServoAngle(angle);
}
return 0;
}