Revision c7e517ce
ID | c7e517ce43e8346527b9d5780bf7e1394a8e38a8 |
Added ability to change constants in PID_control
control/pid_control.cpp | ||
---|---|---|
2 | 2 |
|
3 | 3 |
PID_control::PID_control(float p_term, float d_term, float i_term, float goal) |
4 | 4 |
{ |
5 |
P_err = 0; |
|
6 |
I_err = 0; |
|
7 |
D_err = 0; |
|
8 |
|
|
9 | 5 |
k_p = p_term; |
10 | 6 |
k_d = d_term; |
11 | 7 |
k_i = i_term; |
12 |
|
|
13 | 8 |
set_config = goal; |
9 |
reset(); |
|
14 | 10 |
} |
15 | 11 |
|
16 | 12 |
PID_control::PID_control(float p_term, float d_term, float i_term) |
17 | 13 |
{ |
18 |
P_err = 0; |
|
19 |
I_err = 0; |
|
20 |
D_err = 0; |
|
21 |
|
|
22 | 14 |
k_p = p_term; |
23 | 15 |
k_d = d_term; |
24 | 16 |
k_i = i_term; |
17 |
set_config = 0; |
|
18 |
reset(); |
|
25 | 19 |
} |
26 | 20 |
|
27 |
void PID_control::change_goal(float goal)
|
|
21 |
void PID_control::reset()
|
|
28 | 22 |
{ |
29 |
set_config == goal;
|
|
23 |
P_err = 0;
|
|
30 | 24 |
I_err = 0; |
25 |
D_err = 0; |
|
26 |
prev_error = 0; |
|
27 |
} |
|
28 |
|
|
29 |
void PID_control::change_goal(float goal) |
|
30 |
{ |
|
31 |
set_config = goal; |
|
32 |
reset(); |
|
33 |
} |
|
34 |
|
|
35 |
void PID_control::change_p(float p) |
|
36 |
{ |
|
37 |
k_p = p; |
|
38 |
reset(); |
|
39 |
} |
|
40 |
|
|
41 |
void PID_control::change_i(float i) |
|
42 |
{ |
|
43 |
k_i = i; |
|
44 |
reset(); |
|
45 |
} |
|
46 |
|
|
47 |
void PID_control::change_d(float d) |
|
48 |
{ |
|
49 |
k_d = d; |
|
50 |
reset(); |
|
31 | 51 |
} |
32 | 52 |
|
33 | 53 |
float PID_control::pid(float input, double time) |
34 | 54 |
{ |
35 |
prev_error = error; |
|
36 |
error = set_config - input; |
|
55 |
float error = set_config - input; |
|
37 | 56 |
double dt = time - prev_time; |
38 | 57 |
|
39 | 58 |
P_err = error; |
40 |
I_err += (prev_error*dt);
|
|
59 |
I_err += prev_error*dt;
|
|
41 | 60 |
D_err = (error - prev_error)/dt; |
42 | 61 |
|
43 | 62 |
prev_time = time; |
44 |
return k_p*P_err + k_i*I_err + k_d * D_err; |
|
63 |
prev_error = error; |
|
64 |
return k_p*P_err + k_i*I_err + k_d*D_err; |
|
45 | 65 |
} |
Also available in: Unified diff