-
Notifications
You must be signed in to change notification settings - Fork 5
/
simple_pid.hpp
61 lines (50 loc) · 1.3 KB
/
simple_pid.hpp
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
#ifndef _SIMPLE_PID_HPP_
#define _SIMPLE_PID_HPP_
#include <iostream>
#define SIMPLE_PID_INUM 10
class PID {
private:
double Kp;
double Ki;
double Kd;
double setpoint;
double integral;
double prev_error;
bool first_time;
int i_inx = 0;
int i_num = 0;
double i_values[SIMPLE_PID_INUM];
public:
PID(double Kp, double Ki, double Kd, double setpoint)
: Kp(Kp), Ki(Ki), Kd(Kd), setpoint(setpoint), integral(0.0), prev_error(0.0), first_time(true)
{
for (int i = 0; i < SIMPLE_PID_INUM; i++) {
i_values[i] = 0;
}
}
void set_setpoint(double sp) {
setpoint = sp;
}
double calculate(double input) {
double error = setpoint - input;
i_values[i_inx++] = error;
double derivative = (first_time) ? 0.0 : error - prev_error;
first_time = false;
prev_error = error;
if (i_num < SIMPLE_PID_INUM) {
i_num++;
}
if (i_inx >= SIMPLE_PID_INUM) {
i_inx = 0;
}
double _integral = 0;
for (int i = 0; i < i_num; i++) {
_integral += i_values[i];
}
return Kp * error + Ki * _integral + Kd * derivative;
}
void reset_integral() {
integral = 0.0;
}
};
#endif /* _SIMPLE_PID_HPP_ */