root / control / pid_control.cpp @ master
History | View | Annotate | Download (983 Bytes)
1 | a6ca8c2b | Priya | #include "pid_control.h" |
---|---|---|---|
2 | |||
3 | 71780e9c | Priya | PID_control::PID_control(float p_term, float d_term, float i_term, float goal) |
4 | a6ca8c2b | Priya | { |
5 | k_p = p_term; |
||
6 | k_d = d_term; |
||
7 | k_i = i_term; |
||
8 | set_config = goal; |
||
9 | c7e517ce | Tom Mullins | reset(); |
10 | a6ca8c2b | Priya | } |
11 | |||
12 | 71780e9c | Priya | PID_control::PID_control(float p_term, float d_term, float i_term) |
13 | a6ca8c2b | Priya | { |
14 | k_p = p_term; |
||
15 | k_d = d_term; |
||
16 | k_i = i_term; |
||
17 | c7e517ce | Tom Mullins | set_config = 0;
|
18 | reset(); |
||
19 | a6ca8c2b | Priya | } |
20 | |||
21 | c7e517ce | Tom Mullins | void PID_control::reset()
|
22 | a6ca8c2b | Priya | { |
23 | c7e517ce | Tom Mullins | P_err = 0;
|
24 | a6ca8c2b | Priya | I_err = 0;
|
25 | c7e517ce | Tom Mullins | 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(); |
||
51 | a6ca8c2b | Priya | } |
52 | |||
53 | 7741ab09 | Priya | float PID_control::pid(float input, double time) |
54 | a6ca8c2b | Priya | { |
55 | c7e517ce | Tom Mullins | float error = set_config - input;
|
56 | 7741ab09 | Priya | double dt = time - prev_time;
|
57 | a6ca8c2b | Priya | |
58 | P_err = error; |
||
59 | c7e517ce | Tom Mullins | I_err += prev_error*dt; |
60 | 7741ab09 | Priya | D_err = (error - prev_error)/dt; |
61 | a6ca8c2b | Priya | |
62 | 7741ab09 | Priya | prev_time = time; |
63 | c7e517ce | Tom Mullins | prev_error = error; |
64 | return k_p*P_err + k_i*I_err + k_d*D_err;
|
||
65 | a6ca8c2b | Priya | } |