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