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 |
} |
control/pid_control.h | ||
---|---|---|
9 | 9 |
|
10 | 10 |
void change_goal(float goal); |
11 | 11 |
|
12 |
float pid(float input); |
|
12 |
float pid(float input, double time);
|
|
13 | 13 |
|
14 | 14 |
private: |
15 | 15 |
float k_p; |
... | ... | |
19 | 19 |
float set_config; |
20 | 20 |
float error; |
21 | 21 |
float prev_error; |
22 |
double prev_time; |
|
22 | 23 |
|
23 | 24 |
float P_err; |
24 | 25 |
float I_err; |
control/pid_test.cpp | ||
---|---|---|
7 | 7 |
float result = 1; |
8 | 8 |
for(int counter = 0; counter < 100; counter++) |
9 | 9 |
{ |
10 |
result = meow->pid(result); |
|
10 |
result = meow->pid(result, counter+1);
|
|
11 | 11 |
printf("%f\t", result); |
12 | 12 |
} |
13 | 13 |
return 0; |
Also available in: Unified diff