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