OpenWAM
TPIDController.cpp
1 /* --------------------------------------------------------------------------------*\
2 ==========================|
3  \\ /\ /\ // O pen | OpenWAM: The Open Source 1D Gas-Dynamic Code
4  \\ | X | // W ave |
5  \\ \/_\/ // A ction | CMT-Motores Termicos / Universidad Politecnica Valencia
6  \\/ \// M odel |
7  ----------------------------------------------------------------------------------
8  License
9 
10  This file is part of OpenWAM.
11 
12  OpenWAM is free software: you can redistribute it and/or modify
13  it under the terms of the GNU General Public License as published by
14  the Free Software Foundation, either version 3 of the License, or
15  (at your option) any later version.
16 
17  OpenWAM is distributed in the hope that it will be useful,
18  but WITHOUT ANY WARRANTY; without even the implied warranty of
19  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  GNU General Public License for more details.
21 
22  You should have received a copy of the GNU General Public License
23  along with OpenWAM. If not, see <http://www.gnu.org/licenses/>.
24 
25 
26  \*-------------------------------------------------------------------------------- */
27 
28 // ---------------------------------------------------------------------------
29 #pragma hdrstop
30 
31 #include "TPIDController.h"
32 
33 // ---------------------------------------------------------------------------
34 
36  TController(nmCtlPID, i) {
37  fID = i + 1;
38  fError_ant = 0;
39  fTime_ant = 0;
40  fInicio = true;
41  fDwell = 0.5;
42 }
43 
45 
46 }
47 
48 double TPIDController::Output(double Time) {
49  double dt = 0., out = 0., deltaT = 0., Kp = 0., Ki = 0., Kd = 0.;
50 
51  dt = Time - fTime_ant;
52 
53  if(dt > fPeriod && Time > fDwell) {
54 
55  if(fSetPointControlled)
56  fSetPoint = fSetPointController->Output(Time);
57 
58  fError = FSensor[0]->Output() - fSetPoint;
59 
60  if(fInicio) {
61  fError_ant = fError;
62  fiact = 0;
63  fInicio = false;
64  }
65 
66  if(fError > 0) {
67  Kp = fKP_pos;
68  Ki = fKI_pos;
69  Kd = fKD_pos;
70 // if(fError_ant < 0){
71 // fiact=0.;
72 // }
73 
74  } else {
75  Kp = fKP_neg;
76  Ki = fKI_neg;
77  Kd = fKD_neg;
78 // if(fError_ant > 0){
79 // fiact=0.;
80 // }
81  }
82 
83  fpact = Kp * fError;
84  fdact = Kd * (fError - fError_ant) / dt;
85  fiact = fiact + Ki * fError * dt;
86  //fiact = Ki * fError * dt;
87 
88  //fOutput = fOutput_ant + fpact + fdact + fiact;
89 
90  fOutput = fpact + fdact + fiact;
91 
92  if(fOutput > fMax_out) {
93  fOutput = fMax_out;
94  fiact = fI_ant;
95 // if (fError < 0)
96 // fI_ant = fI_ant + Ki * fError * dt;
97  } else if(fOutput < fMin_out) {
98  fOutput = fMin_out;
99  fiact = fI_ant;
100 // if (fError > 0)
101 // fI_ant = fI_ant + Ki * fError * dt;
102  }
103 // else {
104 // fI_ant = fI_ant + Ki * fError * dt;
105 // }
106 
107  fI_ant = fiact;
108  fError_ant = fError;
109 
110  fTime_ant = Time;
111  }
112  deltaT = Time - fTime_ant_filt;
113  if(deltaT > 0) {
114  fOutput_filt = ((2 * fDelay - deltaT) * fOutput_filt_ant + deltaT * fGain * (fOutput + fOutput_ant)) /
115  (2 * fDelay + deltaT);
116  fOutput_filt_ant = fOutput_filt;
117  fOutput_ant = fOutput;
118  fTime_ant_filt = Time;
119  }
120 
122 
123  return fOutput_filt;
124 }
125 
126 void TPIDController::LeeController(const char *FileWAM, fpos_t &filepos) {
127 
128  int ctrl = 0;
129 
130  FILE *fich = fopen(FileWAM, "r");
131  fsetpos(fich, &filepos);
132 
133  fscanf(fich, "%lf %lf %lf ", &fKP_pos, &fKI_pos, &fKD_pos);
134  fscanf(fich, "%lf %lf %lf ", &fKP_neg, &fKI_neg, &fKD_neg);
135 
136  fscanf(fich, "%lf %lf %lf %lf ", &fOutput, &fOutput0, &fMax_out, &fMin_out);
137 
138  fscanf(fich, "%lf %lf %lf ", &fPeriod, &fDelay, &fGain);
139 
140  fI_ant = fOutput;
141  fiact = fOutput;
142  fOutput_ant = fOutput;
143  fOutput_filt = fOutput;
144  fOutput_filt_ant = fOutput;
145 
146  int tmp = 0;
147 
148  fscanf(fich, "%lf ", &fSetPoint);
149  fscanf(fich, "%d ", &ctrl);
150  if(ctrl == 0)
151  fSetPointControlled = false;
152  else {
153  fSetPointControlled = true;
154  fSetPointControllerID = ctrl;
155  }
156 
157  FSensorID.resize(1);
158  fscanf(fich, "%d ", &FSensorID[0]);
159 
160  fgetpos(fich, &filepos);
161  fclose(fich);
162 
163 }
164 
165 void TPIDController::AsignaObjetos(TSensor **Sensor, TController **Controller) {
166 
167  FSensor.push_back(Sensor[FSensorID[0] - 1]);
168 
169  if(fSetPointControlled)
170  fSetPointController = Controller[fSetPointControllerID - 1];
171 
172 }
173 
174 void TPIDController::LeeResultadosMedControlador(const char *FileWAM, fpos_t &filepos) {
175  try {
176  int nvars = 0, var = 0;
177 
178  FILE *fich = fopen(FileWAM, "r");
179  fsetpos(fich, &filepos);
180 
181  fscanf(fich, "%d ", &nvars);
182  for(int i = 0; i < nvars; i++) {
183  fscanf(fich, "%d ", &var);
184  switch(var) {
185  case 0:
186  FResMediosCtrl.Output = true;
187  break;
188  case 1:
189  FResMediosCtrl.Error = true;
190  break;
191  case 2:
192  FResMediosCtrl.POutput = true;
193  break;
194  case 3:
195  FResMediosCtrl.IOutput = true;
196  break;
197  case 4:
198  FResMediosCtrl.DOutput = true;
199  break;
200  case 5:
201  FResMediosCtrl.Output_filt = true;
202  break;
203  default:
204  std::cout << "Resultados medios en Controlador " << fID << " no implementados " << std::endl;
205  }
206  }
207 
208  fgetpos(fich, &filepos);
209  fclose(fich);
210  } catch(exception & N) {
211  std::cout << "ERROR: TPIDController::LeeResultadosControlador en el controlador " << fID << std::endl;
212  std::cout << "Tipo de error: " << N.what() << std::endl;
213  throw Exception(N.what());
214  }
215 }
216 
217 void TPIDController::LeeResultadosInsControlador(const char *FileWAM, fpos_t &filepos) {
218  try {
219  int nvars = 0, var = 0;
220 
221  FILE *fich = fopen(FileWAM, "r");
222  fsetpos(fich, &filepos);
223 
224  fscanf(fich, "%d ", &nvars);
225  for(int i = 0; i < nvars; i++) {
226  fscanf(fich, "%d ", &var);
227  switch(var) {
228  case 0:
229  FResInstantCtrl.Output = true;
230  break;
231  case 1:
232  FResInstantCtrl.Error = true;
233  break;
234  case 2:
235  FResInstantCtrl.POutput = true;
236  break;
237  case 3:
238  FResInstantCtrl.IOutput = true;
239  break;
240  case 4:
241  FResInstantCtrl.DOutput = true;
242  break;
243  case 5:
244  FResInstantCtrl.Output_filt = true;
245  break;
246  default:
247  std::cout << "Resultados instantaneos en Controlador " << fID << " no implementados " << std::endl;
248  }
249  }
250 
251  fgetpos(fich, &filepos);
252  fclose(fich);
253  } catch(exception & N) {
254  std::cout << "ERROR: TPIDController::LeeResultadosInsControlador en el controlador " << fID << std::endl;
255  std::cout << "Tipo de error: " << N.what() << std::endl;
256  throw Exception(N.what());
257  }
258 }
259 
261  try {
262  std::string Label;
263 
264  if(FResMediosCtrl.Output) {
265  Label = "\t" + PutLabel(705) + std::to_string(fID) + PutLabel(901);
266  medoutput << Label.c_str();
267  }
268  if(FResMediosCtrl.Error) {
269  Label = "\t" + PutLabel(706) + std::to_string(fID) + PutLabel(901);
270  medoutput << Label.c_str();
271  }
272  if(FResMediosCtrl.POutput) {
273  Label = "\t" + PutLabel(709) + std::to_string(fID) + PutLabel(901);
274  medoutput << Label.c_str();
275  }
276  if(FResMediosCtrl.IOutput) {
277  Label = "\t" + PutLabel(710) + std::to_string(fID) + PutLabel(901);
278  medoutput << Label.c_str();
279  }
280  if(FResMediosCtrl.DOutput) {
281  Label = "\t" + PutLabel(711) + std::to_string(fID) + PutLabel(901);
282  medoutput << Label.c_str();
283  }
284  if(FResMediosCtrl.Output_filt) {
285  Label = "\t" + PutLabel(712) + std::to_string(fID) + PutLabel(901);
286  medoutput << Label.c_str();
287  }
288 
289  } catch(exception & N) {
290  std::cout << "ERROR: TPIDController::CabeceraResultadosMedControlador en el controlador " << fID << std::endl;
291  std::cout << "Tipo de error: " << N.what() << std::endl;
292  throw Exception(N.what());
293  }
294 }
295 
297  try {
298  std::string Label;
299 
300  if(FResInstantCtrl.Output) {
301  Label = "\t" + PutLabel(705) + std::to_string(fID) + PutLabel(901);
302  insoutput << Label.c_str();
303  }
304  if(FResInstantCtrl.Error) {
305  Label = "\t" + PutLabel(706) + std::to_string(fID) + PutLabel(901);
306  insoutput << Label.c_str();
307  }
308  if(FResInstantCtrl.POutput) {
309  Label = "\t" + PutLabel(709) + std::to_string(fID) + PutLabel(901);
310  insoutput << Label.c_str();
311  }
312  if(FResInstantCtrl.IOutput) {
313  Label = "\t" + PutLabel(710) + std::to_string(fID) + PutLabel(901);
314  insoutput << Label.c_str();
315  }
316  if(FResInstantCtrl.DOutput) {
317  Label = "\t" + PutLabel(711) + std::to_string(fID) + PutLabel(901);
318  insoutput << Label.c_str();
319  }
320  if(FResInstantCtrl.Output_filt) {
321  Label = "\t" + PutLabel(712) + std::to_string(fID) + PutLabel(901);
322  insoutput << Label.c_str();
323  }
324  } catch(exception & N) {
325  std::cout << "ERROR: TPIDController::CabeceraResultadosInsControlador en el controlador " << fID << std::endl;
326  std::cout << "Tipo de error: " << N.what() << std::endl;
327  throw Exception(N.what());
328  }
329 }
330 
331 void TPIDController::ImprimeResultadosMedControlador(stringstream& medoutput) {
332  try {
333  std::string Label;
334 
335  if(FResMediosCtrl.Output) {
336  medoutput << "\t" << FResMediosCtrl.OutputMED;
337  }
338  if(FResMediosCtrl.Error) {
339  medoutput << "\t" << FResMediosCtrl.ErrorMED;
340  }
341  if(FResMediosCtrl.POutput) {
342  medoutput << "\t" << FResMediosCtrl.POutputMED;
343  }
344  if(FResMediosCtrl.IOutput) {
345  medoutput << "\t" << FResMediosCtrl.IOutputMED;
346  }
347  if(FResMediosCtrl.DOutput) {
348  medoutput << "\t" << FResMediosCtrl.DOutputMED;
349  }
350  if(FResMediosCtrl.Output_filt) {
351  medoutput << "\t" << FResMediosCtrl.Output_filtMED;
352  }
353  } catch(exception & N) {
354  std::cout << "ERROR: TPIDController::ImprimeResultadosMedControlador en el controlador " << fID << std::endl;
355  std::cout << "Tipo de error: " << N.what() << std::endl;
356  throw Exception(N.what());
357  }
358 }
359 
360 void TPIDController::ImprimeResultadosInsControlador(stringstream& insoutput) {
361  try {
362  std::string Label;
363 
364  if(FResInstantCtrl.Output) {
365  insoutput << "\t" << FResInstantCtrl.OutputINS;
366  }
367  if(FResInstantCtrl.Error) {
368  insoutput << "\t" << FResInstantCtrl.ErrorINS;
369  }
370  if(FResInstantCtrl.POutput) {
371  insoutput << "\t" << FResInstantCtrl.POutputINS;
372  }
373  if(FResInstantCtrl.IOutput) {
374  insoutput << "\t" << FResInstantCtrl.IOutputINS;
375  }
376  if(FResInstantCtrl.DOutput) {
377  insoutput << "\t" << FResInstantCtrl.DOutputINS;
378  }
379  if(FResInstantCtrl.Output_filt) {
380  insoutput << "\t" << FResInstantCtrl.Output_filtINS;
381  }
382  } catch(exception & N) {
383  std::cout << "ERROR: TPIDController::CabeceraResultadosInsControlador en el controlador " << fID << std::endl;
384  std::cout << "Tipo de error: " << N.what() << std::endl;
385  throw Exception(N.what());
386  }
387 }
388 
390  try {
391 
392  FResMediosCtrl.OutputSUM = 0.;
393  FResMediosCtrl.ErrorSUM = 0.;
394  FResMediosCtrl.POutputSUM = 0.;
395  FResMediosCtrl.IOutputSUM = 0.;
396  FResMediosCtrl.DOutputSUM = 0.;
397  FResMediosCtrl.Output_filtSUM = 0.;
398  FResMediosCtrl.TiempoSUM = 0.;
399  FResMediosCtrl.Tiempo0 = 0.;
400 
401  } catch(exception & N) {
402  std::cout << "ERROR: TPIDController::IniciaMedias en el controlador: " << fID << std::endl;
403  // std::cout << "Tipo de error: " << N.what() << std::endl;
404  throw Exception(N.what());
405  }
406 }
407 
409  try {
410 
411  if(FResMediosCtrl.Output) {
412  FResMediosCtrl.OutputMED = FResMediosCtrl.OutputSUM / FResMediosCtrl.TiempoSUM;
413  FResMediosCtrl.OutputSUM = 0.;
414  }
415  if(FResMediosCtrl.Error) {
416  FResMediosCtrl.ErrorMED = FResMediosCtrl.ErrorSUM / FResMediosCtrl.TiempoSUM;
417  FResMediosCtrl.ErrorSUM = 0.;
418  }
419  if(FResMediosCtrl.POutput) {
420  FResMediosCtrl.POutputMED = FResMediosCtrl.POutputSUM / FResMediosCtrl.TiempoSUM;
421  FResMediosCtrl.POutputSUM = 0.;
422  }
423  if(FResMediosCtrl.IOutput) {
424  FResMediosCtrl.IOutputMED = FResMediosCtrl.IOutputSUM / FResMediosCtrl.TiempoSUM;
425  FResMediosCtrl.IOutputSUM = 0.;
426  }
427  if(FResMediosCtrl.DOutput) {
428  FResMediosCtrl.DOutputMED = FResMediosCtrl.DOutputSUM / FResMediosCtrl.TiempoSUM;
429  FResMediosCtrl.DOutputSUM = 0.;
430  }
431  if(FResMediosCtrl.Output_filt) {
432  FResMediosCtrl.Output_filtMED = FResMediosCtrl.Output_filtSUM / FResMediosCtrl.TiempoSUM;
433  FResMediosCtrl.Output_filtSUM = 0.;
434  }
435  FResMediosCtrl.TiempoSUM = 0;
436 
437  } catch(exception & N) {
438  std::cout << "ERROR: TPIDController::ResultadosMediosController en el eje: " << fID << std::endl;
439  // std::cout << "Tipo de error: " << N.what() << std::endl;
440  throw Exception(N.what());
441  }
442 }
443 
445  try {
446  /* Lo que se hace en esta funcion se realiza dentro del calculo del eje, para asi poder
447  llevar a cabo la salida de resultados medios por pantalla. */
448  double Delta = Actual - FResMediosCtrl.Tiempo0;
449 
450  if(FResMediosCtrl.Output) {
451  FResMediosCtrl.OutputSUM += fOutput * Delta;
452  }
453  if(FResMediosCtrl.Error) {
454  FResMediosCtrl.ErrorSUM += fError * Delta;
455  }
456  if(FResMediosCtrl.POutput) {
457  FResMediosCtrl.POutputSUM += fpact * Delta;
458  }
459  if(FResMediosCtrl.IOutput) {
460  FResMediosCtrl.IOutputSUM += fiact * Delta;
461  }
462  if(FResMediosCtrl.DOutput) {
463  FResMediosCtrl.DOutputSUM += fdact * Delta;
464  }
465  if(FResMediosCtrl.Output_filt) {
466  FResMediosCtrl.Output_filtSUM += fOutput_filt * Delta;
467  }
468  FResMediosCtrl.TiempoSUM += Delta;
469  FResMediosCtrl.Tiempo0 = Actual;
470 
471  } catch(exception & N) {
472  std::cout << "ERROR: TPIDController::AcumulaResultadosMediosController en el eje: " << fID << std::endl;
473  // std::cout << "Tipo de error: " << N.what() << std::endl;
474  throw Exception(N.what());
475  }
476 }
477 
479  try {
480  if(FResInstantCtrl.Output)
481  FResInstantCtrl.OutputINS = fOutput;
482  if(FResInstantCtrl.Error)
483  FResInstantCtrl.ErrorINS = fError;
484  if(FResInstantCtrl.POutput)
485  FResInstantCtrl.POutputINS = fpact;
486  if(FResInstantCtrl.IOutput)
487  FResInstantCtrl.IOutputINS = fiact;
488  if(FResInstantCtrl.DOutput)
489  FResInstantCtrl.DOutputINS = fdact;
490  if(FResInstantCtrl.Output_filt)
491  FResInstantCtrl.Output_filtINS = fOutput_filt;
492 
493  } catch(exception & N) {
494  std::cout << "ERROR: TPIDController::ResultadosInstantController en el eje " << fID << std::endl;
495  std::cout << "Tipo de error: " << N.what() << std::endl;
496  throw Exception(N.what());
497  }
498 }
499 
500 #pragma package(smart_init)
TPIDController::TPIDController
TPIDController(int i)
Definition: TPIDController.cpp:35
TPIDController::CabeceraResultadosInsControlador
void CabeceraResultadosInsControlador(stringstream &insoutput)
Definition: TPIDController.cpp:296
TController
Definition: TController.h:37
TPIDController::~TPIDController
~TPIDController()
Definition: TPIDController.cpp:44
TPIDController::LeeResultadosMedControlador
void LeeResultadosMedControlador(const char *FileWAM, fpos_t &filepos)
Definition: TPIDController.cpp:174
TPIDController::ResultadosMediosController
void ResultadosMediosController()
Definition: TPIDController.cpp:408
TPIDController::IniciaMedias
void IniciaMedias()
Definition: TPIDController.cpp:389
TPIDController::ImprimeResultadosMedControlador
void ImprimeResultadosMedControlador(stringstream &medoutput)
Definition: TPIDController.cpp:331
TController::FSensorID
iVector FSensorID
Array with the ID of the sensor inputs.
Definition: TController.h:43
TController::FResMediosCtrl
stResMediosCtrl FResMediosCtrl
Struct with the average results of the controllers.
Definition: TController.h:49
TPIDController::CabeceraResultadosMedControlador
void CabeceraResultadosMedControlador(stringstream &medoutput)
Definition: TPIDController.cpp:260
TPIDController::ResultadosInstantController
void ResultadosInstantController()
Definition: TPIDController.cpp:478
PutLabel
std::string PutLabel(int idx)
Returns an integer.
Definition: labels.cpp:475
TPIDController::ImprimeResultadosInsControlador
void ImprimeResultadosInsControlador(stringstream &insoutput)
Definition: TPIDController.cpp:360
Exception
Custom exception class.
Definition: Exception.hpp:39
TPIDController::LeeController
void LeeController(const char *FileWAM, fpos_t &filepos)
Definition: TPIDController.cpp:126
TPIDController::AcumulaResultadosMediosController
void AcumulaResultadosMediosController(double Actual)
Definition: TPIDController.cpp:444
TController::FResInstantCtrl
stResInstantCtrl FResInstantCtrl
Struct with the average results of the controllers.
Definition: TController.h:50
TSensor
Definition: TSensor.h:42
TPIDController::LeeResultadosInsControlador
void LeeResultadosInsControlador(const char *FileWAM, fpos_t &filepos)
Definition: TPIDController.cpp:217
TController::Output
virtual double Output(double Time)=0
TPIDController::AsignaObjetos
void AsignaObjetos(TSensor **Sensor, TController **Controller)
Definition: TPIDController.cpp:165
TController::FSensor
std::vector< TSensor * > FSensor
Array with the pointers of the sensor inputs.
Definition: TController.h:40
TPIDController::Output
double Output(double Time)
Definition: TPIDController.cpp:48