31 #include "TPIDController.h"
49 double dt = 0., out = 0., deltaT = 0., Kp = 0., Ki = 0., Kd = 0.;
51 dt = Time - fTime_ant;
53 if(dt > fPeriod && Time > fDwell) {
55 if(fSetPointControlled)
56 fSetPoint = fSetPointController->
Output(Time);
58 fError =
FSensor[0]->Output() - fSetPoint;
84 fdact = Kd * (fError - fError_ant) / dt;
85 fiact = fiact + Ki * fError * dt;
90 fOutput = fpact + fdact + fiact;
92 if(fOutput > fMax_out) {
97 }
else if(fOutput < fMin_out) {
112 deltaT = Time - fTime_ant_filt;
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;
130 FILE *fich = fopen(FileWAM,
"r");
131 fsetpos(fich, &filepos);
133 fscanf(fich,
"%lf %lf %lf ", &fKP_pos, &fKI_pos, &fKD_pos);
134 fscanf(fich,
"%lf %lf %lf ", &fKP_neg, &fKI_neg, &fKD_neg);
136 fscanf(fich,
"%lf %lf %lf %lf ", &fOutput, &fOutput0, &fMax_out, &fMin_out);
138 fscanf(fich,
"%lf %lf %lf ", &fPeriod, &fDelay, &fGain);
142 fOutput_ant = fOutput;
143 fOutput_filt = fOutput;
144 fOutput_filt_ant = fOutput;
148 fscanf(fich,
"%lf ", &fSetPoint);
149 fscanf(fich,
"%d ", &ctrl);
151 fSetPointControlled =
false;
153 fSetPointControlled =
true;
154 fSetPointControllerID = ctrl;
160 fgetpos(fich, &filepos);
169 if(fSetPointControlled)
170 fSetPointController = Controller[fSetPointControllerID - 1];
176 int nvars = 0, var = 0;
178 FILE *fich = fopen(FileWAM,
"r");
179 fsetpos(fich, &filepos);
181 fscanf(fich,
"%d ", &nvars);
182 for(
int i = 0; i < nvars; i++) {
183 fscanf(fich,
"%d ", &var);
204 std::cout <<
"Resultados medios en Controlador " << fID <<
" no implementados " << std::endl;
208 fgetpos(fich, &filepos);
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;
219 int nvars = 0, var = 0;
221 FILE *fich = fopen(FileWAM,
"r");
222 fsetpos(fich, &filepos);
224 fscanf(fich,
"%d ", &nvars);
225 for(
int i = 0; i < nvars; i++) {
226 fscanf(fich,
"%d ", &var);
247 std::cout <<
"Resultados instantaneos en Controlador " << fID <<
" no implementados " << std::endl;
251 fgetpos(fich, &filepos);
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;
266 medoutput << Label.c_str();
270 medoutput << Label.c_str();
274 medoutput << Label.c_str();
278 medoutput << Label.c_str();
282 medoutput << Label.c_str();
286 medoutput << Label.c_str();
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;
302 insoutput << Label.c_str();
306 insoutput << Label.c_str();
310 insoutput << Label.c_str();
314 insoutput << Label.c_str();
318 insoutput << Label.c_str();
322 insoutput << Label.c_str();
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;
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;
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;
401 }
catch(exception & N) {
402 std::cout <<
"ERROR: TPIDController::IniciaMedias en el controlador: " << fID << std::endl;
437 }
catch(exception & N) {
438 std::cout <<
"ERROR: TPIDController::ResultadosMediosController en el eje: " << fID << std::endl;
471 }
catch(exception & N) {
472 std::cout <<
"ERROR: TPIDController::AcumulaResultadosMediosController en el eje: " << fID << std::endl;
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;
500 #pragma package(smart_init)