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