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
54
55
56
57
58
59
60
61
62
63
64
65
|
#include "PID.h"
#include "util.h"
PID createPID(PIDConfig config) {
PID pid = (PID){
.p = 0.0,
.i = 0.0,
.d = 0.0,
.kP = config.kP,
.kI = config.kI,
.kD = config.kD,
.angleMode = config.angleMode,
.doClamp = config.doClamp,
.min = config.min,
.max = config.max,
.error = 0.0,
.pastError = 0.0,
.output = 0.0
};
return pid;
}
float runPID(float setpoint, float processValue, PID * pid) {
// Get error.
if (pid->angleMode)
pid->error = angleDis(setpoint, processValue);
else
pid->error = setpoint - processValue;
// Set p, i and d.
pid->p = pid->error * pid->kP;
pid->i += pid->error * pid->kI;
pid->d = (pid->error - pid->pastError) * pid->kD;
// Update error.
pid->pastError = pid->error;
// Set output.
pid->output = pid->p + pid->i + pid->d;
// Clamp.
if (pid->doClamp)
pid->output = Clamp(pid->output, pid->min, pid->max);
return pid->output;
}
void resetPID(PID * pid) {
pid->p = 0.0;
pid->i = 0.0;
pid->d = 0.0;
pid->error = 0.0;
pid->pastError = 0.0;
pid->output = 0.0;
}
float angleDis(float a, float b) {
float dir = b - a;
if (fabsf(dir) > (PI/2))
dir = -(signum(dir) * PI) + dir;
return dir;
}
|
