Revision 7741ab09
ID | 7741ab09ee53490124c3fda719f2fc1f64efaabd |
Added a time thing to PID control
control/pid_control.cpp | ||
---|---|---|
30 | 30 |
I_err = 0; |
31 | 31 |
} |
32 | 32 |
|
33 |
float PID_control::pid(float input) |
|
33 |
float PID_control::pid(float input, double time)
|
|
34 | 34 |
{ |
35 | 35 |
prev_error = error; |
36 | 36 |
error = set_config - input; |
37 |
double dt = time - prev_time; |
|
37 | 38 |
|
38 | 39 |
P_err = error; |
39 |
I_err += prev_error;
|
|
40 |
D_err = error - prev_error;
|
|
40 |
I_err += (prev_error*dt);
|
|
41 |
D_err = (error - prev_error)/dt;
|
|
41 | 42 |
|
43 |
prev_time = time; |
|
42 | 44 |
return k_p*P_err + k_i*I_err + k_d * D_err; |
43 | 45 |
} |
Also available in: Unified diff