-
Notifications
You must be signed in to change notification settings - Fork 1
/
PID.cpp
114 lines (91 loc) · 2.06 KB
/
PID.cpp
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
109
110
111
112
113
114
#include "PID.h"
void PID::init(float Kp, float Ki, float Kd, float tau, float T) // Constructor
{
this->Kp = Kp;
this->Ki = Ki;
this->Kd = Kd;
this->tau = tau;
this->T = T;
// Init to Zero
integ = 0.0f;
diff = 0.0f;
prev_err = 0.0f;
prev_meas = 0.0f;
val = 0.0f;
}
float PID::update(float setpoint, float meas) // Calculate PID
{
// Error Signal
err = setpoint - meas;
// Proportional Term
prop = Kp * err;
// Integral Term
integ += 0.5f * Ki * T * (err + prev_err);
// Anti-wind-up via Dynamic Integrator Clamping
// Limits Computationa and Application
if (lim_max > prop)
lim_max_integ = lim_max - prop;
else
lim_max_integ = 0.0f;
if (lim_min < prop)
lim_max_integ = lim_min - prop;
else
lim_min_integ = 0.0f;
// Constrain Integrator
if (integ > lim_max_integ)
integ = lim_max_integ;
else if (integ < lim_min_integ)
integ = lim_min_integ;
// Derivative (Band - Limited Differentiator)
/**** Derivative on Measurements ****/
diff = (2.0f * Kd * (meas - prev_meas) + (2.0f * tau - T) * diff) / (2.0f * tau + T);
// Calculate Output and Apply Limits
val = prop + integ + diff;
// Constrain Value in Given Bounds
val = constrain(val, lim_min, lim_max);
// Store error and Measurement for Later Use
prev_err = err;
prev_meas = meas;
return val;
}
void PID::set_bounds(float min, float max)
{
this->lim_max = max;
this->lim_min = min;
}
void PID::set_gains(float Kp, float Ki, float Kd, float tau, float T)
{
this->Kp = Kp;
this->Ki = Ki;
this->Kd = Kd;
this->tau = tau;
this->T = T;
}
float PID::get_p()
{
return Kp;
}
float PID::get_i()
{
return Ki;
}
float PID::get_d()
{
return Kd;
}
float PID::get_tau()
{
return tau;
}
float PID::get_err()
{
return err;
}
float PID::constrain(float val, float low_bound, float up_bound)
{
if (val < low_bound)
val = low_bound;
else if (val > up_bound)
val = up_bound;
return val;
}