blob: fabe1da91e7714faccc8cd3bdbd66c185685e840 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
|
#include "pid.h"
PID::PID(float P, float I, float D) {
A0 = P + I * dt;
A1 = -P;
A0d = D / dt;
A1d = -2 * D / dt;
A2d = D / dt;
tau = D / (P * N);
alpha = dt / (2 * tau);
reset(0.0);
}
// https://en.wikipedia.org/wiki/PID_controller#Pseudocode
float PID::iter(float target) {
error[2] = error[1];
error[1] = error[0];
error[0] = target - output;
output = output + A0 * error[0] + A1 * error[1];
d1 = d0;
d0 = A0d * error[0] + A1d * error[1] + A2d * error[2];
fd1 = fd0;
fd0 = ((alpha) / (alpha + 1)) * (d0 + d1) - ((alpha - 1) / (alpha + 1)) * fd1;
output = output + fd0;
if (output < -1) output = -1;
if (output > 1) output = 1;
return output;
}
void PID::reset(float value) {
error[0] = 0.0;
error[1] = 0.0;
error[2] = 0.0;
d0 = 0;
d1 = 0;
fd0 = 0;
fd1 = 0;
output = value;
}
PID speed_pid(0.1, 0.0, 0.0); // TODO: tune these (garbage values)
PID steer_pid(0.1, 0.0, 0.0);
PID speed_mod_pid(1, 1, 1);
void apply_pid(dui_state_t* target, dui_state_t* current) {
current->speed = speed_pid.iter(target->speed);
current->steer = steer_pid.iter(target->steer);
// current->speed_mod = speed_mod_pid.iter(current->speed_mod, target->speed_mod);
current->speed_mod = target->speed_mod;
}
|