|
1 |
|
2 #include <stddef.h> |
|
3 #include <string.h> |
|
4 #include <stdlib.h> |
|
5 #include <stdbool.h> |
|
6 #include <stdint.h> |
|
7 #include <math.h> |
|
8 |
|
9 #include "freertos/FreeRTOS.h" |
|
10 #include "freertos/task.h" |
|
11 #include "esp_system.h" |
|
12 #include "esp_log.h" |
|
13 |
|
14 #include "PID_v1.h" |
|
15 |
|
16 double dispKp; // we'll hold on to the tuning parameters in user-entered |
|
17 double dispKi; // format for display purposes |
|
18 double dispKd; |
|
19 |
|
20 double kp; // (P)roportional Tuning Parameter |
|
21 double ki; // (I)ntegral Tuning Parameter |
|
22 double kd; // (D)erivative Tuning Parameter |
|
23 |
|
24 int controllerDirection; |
|
25 int pOn; |
|
26 |
|
27 double *myInput; // Pointers to the Input, Output, and Setpoint variables |
|
28 double *myOutput; // This creates a hard link between the variables and the |
|
29 double *mySetpoint; // PID, freeing the user from having to constantly tell us |
|
30 // what these values are. with pointers we'll just know. |
|
31 |
|
32 unsigned long lastTime; |
|
33 double outputSum, lastInput; |
|
34 |
|
35 unsigned long SampleTime; |
|
36 double outMin, outMax; |
|
37 bool inAuto, pOnE; |
|
38 |
|
39 |
|
40 static const char *TAG = "pid"; |
|
41 |
|
42 void PID_Initialize(void); |
|
43 |
|
44 |
|
45 |
|
46 void PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, PID_PON POn, PID_DIRECTION Direction) { |
|
47 myOutput = Output; |
|
48 myInput = Input; |
|
49 mySetpoint = Setpoint; |
|
50 inAuto = false; |
|
51 |
|
52 PID_SetOutputLimits(0, 255); |
|
53 |
|
54 SampleTime = 100; |
|
55 |
|
56 PID_SetControllerDirection(Direction); |
|
57 PID_SetTunings(Kp, Ki, Kd, POn); |
|
58 |
|
59 lastTime = (xTaskGetTickCount() * portTICK_PERIOD_MS ) - SampleTime; |
|
60 } |
|
61 |
|
62 |
|
63 |
|
64 /* |
|
65 * This, as they say, is where the magic happens. this function should be called |
|
66 * every time "void loop()" executes. the function will decide for itself whether a new |
|
67 * pid Output needs to be computed. returns true when the output is computed, |
|
68 * false when nothing has been done. |
|
69 */ |
|
70 bool PID_Compute(void) |
|
71 { |
|
72 if (!inAuto) |
|
73 return false; |
|
74 |
|
75 unsigned long now = xTaskGetTickCount() * portTICK_PERIOD_MS; |
|
76 unsigned long timeChange = (now - lastTime); |
|
77 if (timeChange >= SampleTime) { |
|
78 /*Compute all the working error variables*/ |
|
79 double input = *myInput; |
|
80 double error = *mySetpoint - input; |
|
81 double dInput = (input - lastInput); |
|
82 outputSum+= (ki * error); |
|
83 |
|
84 /*Add Proportional on Measurement, if P_ON_M is specified*/ |
|
85 if (!pOnE) |
|
86 outputSum-= kp * dInput; |
|
87 |
|
88 if (outputSum > outMax) |
|
89 outputSum= outMax; |
|
90 else if (outputSum < outMin) |
|
91 outputSum= outMin; |
|
92 |
|
93 /*Add Proportional on Error, if P_ON_E is specified*/ |
|
94 double output; |
|
95 if (pOnE) |
|
96 output = kp * error; |
|
97 else |
|
98 output = 0; |
|
99 |
|
100 /*Compute Rest of PID Output*/ |
|
101 output += outputSum - kd * dInput; |
|
102 |
|
103 if (output > outMax) |
|
104 output = outMax; |
|
105 else if (output < outMin) |
|
106 output = outMin; |
|
107 *myOutput = output; |
|
108 |
|
109 /*Remember some variables for next time*/ |
|
110 lastInput = input; |
|
111 lastTime = now; |
|
112 return true; |
|
113 } else |
|
114 return false; |
|
115 } |
|
116 |
|
117 |
|
118 |
|
119 /* |
|
120 * This function allows the controller's dynamic performance to be adjusted. |
|
121 * it's called automatically from the constructor, but tunings can also |
|
122 * be adjusted on the fly during normal operation |
|
123 */ |
|
124 void PID_SetTunings(double Kp, double Ki, double Kd, PID_PON POn) |
|
125 { |
|
126 if (Kp<0 || Ki<0 || Kd<0) { |
|
127 ESP_LOGE(TAG, "SetTunings negative input"); |
|
128 return; |
|
129 } |
|
130 |
|
131 pOn = POn; |
|
132 pOnE = POn == PID_P_ON_E; |
|
133 |
|
134 dispKp = Kp; dispKi = Ki; dispKd = Kd; |
|
135 |
|
136 ESP_LOGI(TAG, "SetTunings(%.3f, %.3f, %.3f, %s)", Kp, Ki, Kd, (POn) ? "P_ON_E":"P_ON_M"); |
|
137 double SampleTimeInSec = ((double)SampleTime)/1000; |
|
138 kp = Kp; |
|
139 ki = Ki * SampleTimeInSec; |
|
140 kd = Kd / SampleTimeInSec; |
|
141 |
|
142 if (controllerDirection == PID_REVERSE) { |
|
143 kp = (0 - kp); |
|
144 ki = (0 - ki); |
|
145 kd = (0 - kd); |
|
146 } |
|
147 } |
|
148 |
|
149 |
|
150 |
|
151 /* |
|
152 * sets the period, in Milliseconds, at which the calculation is performed |
|
153 */ |
|
154 void PID_SetSampleTime(int NewSampleTime) |
|
155 { |
|
156 ESP_LOGI(TAG, "SetSampleTime(%d)", NewSampleTime); |
|
157 |
|
158 if (NewSampleTime > 0) { |
|
159 double ratio = (double)NewSampleTime / (double)SampleTime; |
|
160 ki *= ratio; |
|
161 kd /= ratio; |
|
162 SampleTime = (unsigned long)NewSampleTime; |
|
163 } |
|
164 } |
|
165 |
|
166 |
|
167 |
|
168 /* |
|
169 * This function will be used far more often than SetInputLimits. while |
|
170 * the input to the controller will generally be in the 0-1023 range (which is |
|
171 * the default already,) the output will be a little different. maybe they'll |
|
172 * be doing a time window and will need 0-8000 or something. or maybe they'll |
|
173 * want to clamp it from 0-125. who knows. at any rate, that can all be done |
|
174 * here. |
|
175 */ |
|
176 void PID_SetOutputLimits(double Min, double Max) |
|
177 { |
|
178 if(Min >= Max) { |
|
179 ESP_LOGE(TAG, "SetOutputLimits Min >= Max"); |
|
180 return; |
|
181 } |
|
182 |
|
183 ESP_LOGI(TAG, "SetOutputLimits(%.0f, %.0f)", Min, Max); |
|
184 outMin = Min; |
|
185 outMax = Max; |
|
186 |
|
187 if(inAuto) { |
|
188 if(*myOutput > outMax) |
|
189 *myOutput = outMax; |
|
190 else if(*myOutput < outMin) |
|
191 *myOutput = outMin; |
|
192 |
|
193 if(outputSum > outMax) |
|
194 outputSum= outMax; |
|
195 else if(outputSum < outMin) |
|
196 outputSum= outMin; |
|
197 } |
|
198 } |
|
199 |
|
200 |
|
201 |
|
202 /* |
|
203 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) |
|
204 * when the transition from manual to auto occurs, the controller is |
|
205 * automatically initialized |
|
206 */ |
|
207 void PID_SetMode(PID_MODE Mode) |
|
208 { |
|
209 bool newAuto = (Mode == PID_AUTOMATIC); |
|
210 |
|
211 ESP_LOGI(TAG, "SetMode(%s)", (Mode) ? "AUTOMATIC":"MANUAL"); |
|
212 if(newAuto && !inAuto) { /*we just went from manual to auto*/ |
|
213 PID_Initialize(); |
|
214 } |
|
215 inAuto = newAuto; |
|
216 } |
|
217 |
|
218 |
|
219 |
|
220 /* |
|
221 * does all the things that need to happen to ensure a bumpless transfer |
|
222 * from manual to automatic mode. |
|
223 */ |
|
224 void PID_Initialize() |
|
225 { |
|
226 outputSum = *myOutput; |
|
227 lastInput = *myInput; |
|
228 if(outputSum > outMax) |
|
229 outputSum = outMax; |
|
230 else if(outputSum < outMin) |
|
231 outputSum = outMin; |
|
232 } |
|
233 |
|
234 |
|
235 |
|
236 /* |
|
237 * The PID will either be connected to a DIRECT acting process (+Output leads |
|
238 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to |
|
239 * know which one, because otherwise we may increase the output when we should |
|
240 * be decreasing. This is called from the constructor. |
|
241 */ |
|
242 void PID_SetControllerDirection(PID_DIRECTION Direction) |
|
243 { |
|
244 ESP_LOGI(TAG, "SetControllerDirection(%s)", (Direction) ? "REVERSE":"DIRECT"); |
|
245 |
|
246 if(inAuto && Direction !=controllerDirection) { |
|
247 kp = (0 - kp); |
|
248 ki = (0 - ki); |
|
249 kd = (0 - kd); |
|
250 } |
|
251 controllerDirection = Direction; |
|
252 } |
|
253 |
|
254 |
|
255 |
|
256 /* |
|
257 * Just because you set the Kp=-1 doesn't mean it actually happened. these |
|
258 * functions query the internal state of the PID. they're here for display |
|
259 * purposes. this are the functions the PID Front-end uses for example |
|
260 */ |
|
261 double PID_GetKp() |
|
262 { |
|
263 return dispKp; |
|
264 } |
|
265 |
|
266 double PID_GetKi() |
|
267 { |
|
268 return dispKi; |
|
269 } |
|
270 |
|
271 double PID_GetKd() |
|
272 { |
|
273 return dispKd; |
|
274 } |
|
275 |
|
276 PID_MODE PID_GetMode() |
|
277 { |
|
278 return inAuto ? PID_AUTOMATIC : PID_MANUAL; |
|
279 } |
|
280 |
|
281 PID_DIRECTION PID_GetDirection() |
|
282 { |
|
283 return controllerDirection; |
|
284 } |
|
285 |
|
286 |