1
4
5int actual = -1;
6int encActual = -1;
7
8
9
10void setup()
11{
12 Serial.begin(9600);
13}
14
15int readEncoder() {
16 int p1 = analogRead(1);
17 int p2 = analogRead(2);
18 int p3 = analogRead(3);
19
20 if (p1+p2+p3==0)
21 return -1;
22
23 int l = p1+p2+p3;
24 int x = (p2-p3)*86;
25 int y = p1*100 - (p2+p3)*50;
26 int p=-1;
27 if (y>0) {
28 if (abs(x) < y*57/100)
29 p=0;
30 else
31 p=(x<0) ? 5 : 1;
32 }
33 else {
34 if (abs(x) < -y*57/100)
35 p=3;
36 else
37 p=(x<0) ? 4 : 2;
38 }
39 return p;
40}
41
42
43
44
45boolean interpolate() {
46 int delta[11] = {
47 -1, -2, 0, +2, +1, 0, -1, -2, 0, +2, +1
48 };
49 int v = readEncoder();
50 if (v==-1)
51 return false;
52 if (actual==-1) {
53 actual = 0;
54 encActual = v;
55 return true;
56 }
57
58 int d = delta[v-encActual+5];
59 actual += d;
60 encActual = v;
61 return true;
62}
63
64void loop()
65{
66 if (interpolate())
67 Serial.println(actual);
68 delay(20);
69}