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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
| int iAna0 = 1;
int iAna1 = 2;
int pPwm1 = 5;
int pPwm2 = 6;
int pLed1 = 3;
int pLed2 = 9;
int base0 = 0;
int base1 = 0;
#define avelg 2
#define avesz (1<<avelg)
int avebf[2][avesz];
int avelc[2] = {0,0};
int ave[2] = {0,0};
int Ave(int id, int value) {
if (id > 2) return 0;
ave[id] -= avebf[id][avelc[id]]; avebf[id][avelc[id]] = value >> avelg;
ave[id] += avebf[id][avelc[id]];
avelc[id]++;
if (avelc[id] == avesz) avelc[id] = 0;
return ave[id];
}
const int vBaseOffset = 10;
void setup() {
Serial.begin(9600);
pinMode(pPwm1, OUTPUT);
pinMode(pPwm2, OUTPUT);
pinMode(pLed1, OUTPUT);
pinMode(pLed2, OUTPUT);
analogWrite(pPwm1, 0);
analogWrite(pPwm2, 0);
base0 = analogRead(iAna0) + vBaseOffset;
base1 = analogRead(iAna1) + vBaseOffset;
Serial.print("base == ");
Serial.print(base0);
Serial.print(", ");
Serial.println(base1);
for (int i = 0; i < avesz; i++) {
avebf[0][i] = 0;
avebf[1][i] = 0;
}
}
int gSwtchingLock = false;
int gDir = 0;
void loop() {
int value0 = analogRead(iAna0);
int value1 = analogRead(iAna1);
int ave0 = Ave(0, value0);
int ave1 = Ave(1, value1);
int acc = (int)((ave0 - base0) * 0.7);
if (acc < 5) acc = 0;
if (acc > 254) acc = 254;
bool sw = (ave1 - base1) > base1;
if (gSwtchingLock == false && sw == true && acc == 0) {
gDir = 1 - gDir;
gSwtchingLock = true;
}
if (gSwtchingLock == true)
{
Serial.print((gDir == 0) ? "200\t100\t" : "100\t200\t");
Serial.println(gDir);
if (sw == false) gSwtchingLock = false;
return;
}
if (gSwtchingLock) {
analogWrite(pPwm1, 0);
analogWrite(pPwm2, 0);
delay(10);
return;
}
if (true) {
Serial.print(ave0);
Serial.print("\t");
Serial.print(ave1);
Serial.print("\t");
Serial.println(acc);
}
if (gDir == 1) {
analogWrite(pPwm1, 0 );
analogWrite(pPwm2, acc);
analogWrite(pLed1, 0 );
analogWrite(pLed2, acc);
}
else {
analogWrite(pPwm1, acc);
analogWrite(pPwm2, 0);
analogWrite(pLed1, acc);
analogWrite(pLed2, 0);
}
delay(100);
}
|