root / control / pid_control.cpp @ 71780e9c
History | View | Annotate | Download (673 Bytes)
1 |
#include "pid_control.h" |
---|---|
2 |
|
3 |
PID_control::PID_control(float p_term, float d_term, float i_term, float goal) |
4 |
{ |
5 |
P_err = 0;
|
6 |
I_err = 0;
|
7 |
D_err = 0;
|
8 |
|
9 |
k_p = p_term; |
10 |
k_d = d_term; |
11 |
k_i = i_term; |
12 |
|
13 |
set_config = goal; |
14 |
} |
15 |
|
16 |
PID_control::PID_control(float p_term, float d_term, float i_term) |
17 |
{ |
18 |
P_err = 0;
|
19 |
I_err = 0;
|
20 |
D_err = 0;
|
21 |
|
22 |
k_p = p_term; |
23 |
k_d = d_term; |
24 |
k_i = i_term; |
25 |
} |
26 |
|
27 |
void PID_control::change_goal(float goal) |
28 |
{ |
29 |
set_config == goal; |
30 |
I_err = 0;
|
31 |
} |
32 |
|
33 |
float PID_control::pid(float input) |
34 |
{ |
35 |
prev_error = error; |
36 |
error = set_config - input; |
37 |
|
38 |
P_err = error; |
39 |
I_err += prev_error; |
40 |
D_err = error - prev_error; |
41 |
|
42 |
return k_p*P_err + k_i*I_err + k_d * D_err;
|
43 |
} |