-
Notifications
You must be signed in to change notification settings - Fork 1
/
MyPID.cpp
128 lines (99 loc) · 2.09 KB
/
MyPID.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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
/*
* MyPID.cpp
*
* Created on: Mar 17, 2012
* Author: jairo
*/
#include "wirish.h"
#include "MyPID.h"
MyPID::MyPID() {
// TODO Auto-generated constructor stub
_p = 0; _i = 0; _d = 0;
_kp = 0; _ki = 0; _kd = 0;
}
MyPID::MyPID(float kp, float ki, float kd) {
// TODO Auto-generated constructor stub
_kp = kp; _ki = ki; _kd = kd;
}
void MyPID::set_gain(char k, float val) {
switch(k) {
case 'p':
case 'P':
_kp = val;
break;
case 'i':
case 'I':
_ki = val;
break;
case 'd':
case 'D':
_kd = val;
break;
}
}
void MyPID::set_gains(float p, float i, float d) {
_kp = p; _ki = i; _kd = d;
}
float MyPID::get_gain(char k) {
switch(k) {
case 'p':
case 'P':
return _kp;
case 'i':
case 'I':
return _ki;
case 'd':
case 'D':
return _kd;
default:
return 0;
}
}
float MyPID::get_term(char k) {
switch(k) {
case 'p':
case 'P':
return _p;
case 'i':
case 'I':
return _i;
case 'd':
case 'D':
return _d;
default:
return 0;
}
}
float MyPID::get_error() {
return _error;
}
float MyPID::go(float desired, float actual, uint32 dt) {
float last_error;
float last_derivative;
/// Low pass filter cut frequency for derivative calculation.
///
static const float filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )";
// Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3
last_error = _error;
last_derivative = _derivative;
_error = desired - actual;
if ((_kd != 0) && (dt != 0)) {
_derivative = (_error - last_error)*1000000/(float)dt;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
_derivative = last_derivative +
(dt / ( filter + dt)) * (_derivative - last_derivative);
}
_p = _error * _kp;
_i += ( _error * _ki* dt)/1000000;
_d = _derivative * _kd;
#define MAX_I 0.010
if(_i > MAX_I) _i = MAX_I;
if(_i < -MAX_I) _i = -MAX_I;
return _p + _i + _d;
}