forked from ManojBR105/ARDUINO-SMD-REWORK-STATION
-
Notifications
You must be signed in to change notification settings - Fork 1
/
FastPID.cpp
128 lines (106 loc) · 2.53 KB
/
FastPID.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
#include "FastPID.h"
#include <Arduino.h>
FastPID::~FastPID() {
}
void FastPID::clear() {
_last_sp = 0;
_last_out = 0;
_sum = 0;
_last_err = 0;
}
bool FastPID::setCoefficients(uint32_t kp, uint32_t ki, uint32_t kd) {
_p = kp;
_i = ki;
_d = kd;
return ! _cfg_err;
}
bool FastPID::setOutputConfig(int bits, bool sign) {
// Set output bits
if (bits > 16 || bits < 1) {
setCfgErr();
}
else {
if (bits == 16) {
_outmax = (0xFFFFULL >> (17 - bits)) * PARAM_MULT;
}
else{
_outmax = (0xFFFFULL >> (16 - bits)) * PARAM_MULT;
}
if (sign) {
_outmin = -((0xFFFFULL >> (17 - bits)) + 1) * PARAM_MULT;
}
else {
_outmin = 0;
}
}
return ! _cfg_err;
}
bool FastPID::setOutputRange(int16_t min, int16_t max)
{
if (min >= max) {
setCfgErr();
return ! _cfg_err;
}
_outmin = int64_t(min) * PARAM_MULT;
_outmax = int64_t(max) * PARAM_MULT;
return ! _cfg_err;
}
bool FastPID::configure(uint32_t kp, uint32_t ki, uint32_t kd, int bits, bool sign) {
clear();
_cfg_err = false;
setCoefficients(kp, ki, kd);
setOutputConfig(bits, sign);
return ! _cfg_err;
}
int16_t FastPID::step(int16_t sp, int16_t fb) {
// int16 + int16 = int17
int32_t err = int32_t(sp) - int32_t(fb);
// int32_t P = 0, I = 0, D = 0;
P = 0, I = 0, D = 0;
if (_p) {
// uint16 * int16 = int32
P = int32_t(_p) * int32_t(err);
}
if (_i) {
// int17 * int16 = int33
_sum += int64_t(err) * int64_t(_i);
// Limit sum to 32-bit signed value so that it saturates, never overflows.
if (_sum > INTEG_MAX)
_sum = INTEG_MAX;
else if (_sum < INTEG_MIN)
_sum = INTEG_MIN;
// int32
I = _sum;
}
if (_d) {
// (int17 - int16) - (int16 - int16) = int19
int32_t deriv = (err - _last_err) - int32_t(sp - _last_sp);
_last_sp = sp;
_last_err = err;
// Limit the derivative to 16-bit signed value.
if (deriv > DERIV_MAX)
deriv = DERIV_MAX;
else if (deriv < DERIV_MIN)
deriv = DERIV_MIN;
// int16 * int16 = int32
D = int32_t(_d) * int32_t(deriv);
}
// int32 (P) + int32 (I) + int32 (D) = int34
int64_t out = int64_t(P) + int64_t(I) + int64_t(D);
// Make the output saturate
if (out > _outmax)
out = _outmax;
else if (out < _outmin)
out = _outmin;
// Remove the integer scaling factor.
int16_t rval = out >> PARAM_SHIFT;
// Fair rounding.
if (out & (0x1ULL << (PARAM_SHIFT - 1))) {
rval++;
}
return rval;
}
void FastPID::setCfgErr() {
_cfg_err = true;
_p = _i = _d = 0;
}