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
|
#ifndef __REGULATOR_H__
#define __REGULATOR_H__
class Regulator
{
private:
long int lasttime;
bool first_run;
double prop,integ,diff;
double sum, result;
double damped_val;
double damping;
public:
void reset(double p, double i, double d, double damp)
{
prop=p;
integ=i;
diff=d;
damping=damp;
first_run = true;
sum=0;
result=0.0;
}
void reset()
{
reset(prop,integ,diff,damping);
}
Regulator(double p, double i, double d, double damp)
{
reset(p,i,d,damp);
}
void put(double val, long int time_msec)
{
double deriv;
double last_damped_val;
if (first_run)
{
damped_val = val;
last_damped_val = damped_val;
lasttime = time_msec - 1; // prevent divison by zero
first_run = false;
}
else
{
last_damped_val = damped_val;
damped_val = damping * damped_val + (1-damping) * val;
}
double delta_t = (time_msec - lasttime) / 1000.;
lasttime = time_msec;
sum += val * delta_t;
deriv = (damped_val - last_damped_val) / delta_t;
result = prop * val + integ * sum + diff * deriv;
}
double get()
{
return result;
}
};
#endif
|
