-
Notifications
You must be signed in to change notification settings - Fork 423
/
pid.cpp
72 lines (63 loc) · 1.63 KB
/
pid.cpp
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
66
67
68
69
70
71
72
#include "pid.h"
PID::PID(float point, float kp, float ki, float kd) {
_kp = kp;
_ki = ki;
_kd = kd;
_point = point;
output_max = 10000;
output_min = -10000;
integral_max = 10000;
integral_min = -10000;
error_integral = 0;
_dir = 1;
}
float PID::Update(float input) {
float output;
float dinput;
error = _point - input;
error_integral += _ki * error + error_integral_offset;
if (error_integral > integral_max) {
error_integral = integral_max;
}
if (error_integral < integral_min) {
error_integral = integral_min;
}
dinput = input - input_last;
output = _kp * error + error_integral - _kd * dinput;
input_last = input;
if (output < output_min) {
output = output_min;
}
if (output > output_max) {
output = output_max;
}
return output;
}
void PID::SetOutputLimits(float max_out, float min_out) {
output_max = max_out;
output_min = min_out;
}
void PID::SetIntegralLimits(float max_out, float min_out) {
integral_max = max_out;
integral_min = min_out;
}
void PID::SetDirection(int8_t dir) {
_dir = dir;
_kp = fabs(_kp) * _dir;
_ki = fabs(_ki) * _dir;
_kd = fabs(_kd) * _dir;
}
void PID::SetIntegral(float integral) {
error_integral = integral;
}
void PID::SetIntegralOffset(float offset) {
error_integral_offset = offset;
}
void PID::UpdateParam(float kp, float ki, float kd) {
_kp = fabs(kp) * _dir;
_ki = fabs(ki) * _dir;
_kd = fabs(kd) * _dir;
}
void PID::SetPoint(float point) {
_point = point;
}