Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

pid.c File Reference

#include <stdio.h>
#include "inetfile.hh"
#include "rcs_prnt.hh"
#include "inifile.h"
#include "pid.h"

Include dependency graph for pid.c:

Include dependency graph

Go to the source code of this file.

Defines

#define GAINS_SET   0x01
#define CYCLE_TIME_SET   0x02
#define ALL_SET   (GAINS_SET | CYCLE_TIME_SET)

Functions

char __attribute__ ((unused)) ident[]="$Id
int pidReset (PID_STRUCT *pid)
int pidSetCycleTime (PID_STRUCT *pid, double seconds)
int pidSetGains (PID_STRUCT *pid, PID_STRUCT parameters)
double pidRunCycle (PID_STRUCT *pid, double sample, double setpoint)
int pidIniLoad (PID_STRUCT *pid, const char *filename)

Define Documentation

#define ALL_SET   (GAINS_SET | CYCLE_TIME_SET)
 

#define CYCLE_TIME_SET   0x02
 

#define GAINS_SET   0x01
 

Function Documentation

char __attribute__ (  (unused)    )  [static]
 

Definition at line 58 of file pid.c.

00058                                                   : pid.c,v 1.7 2001/07/06 21:50:05 proctor Exp $";
00059 
00060 #define GAINS_SET 0x01
00061 #define CYCLE_TIME_SET 0x02
00062 #define ALL_SET (GAINS_SET | CYCLE_TIME_SET)
00063 
00064 int pidInit(PID_STRUCT * pid)
00065 {
00066 #ifdef SMOOTH_D_FACTOR
00067   int i;
00068 #endif
00069   if (0 == pid) {
00070     return -1;
00071   }
00072 
00073   pid->p = 0.0;
00074   pid->i = 0.0;
00075   pid->d = 0.0;
00076   pid->ff0 = 0.0;
00077   pid->ff1 = 0.0;
00078   pid->ff2 = 0.0;
00079   pid->backlash = 0.0;
00080   pid->bias = 0.0;
00081   pid->maxError = 0.0;
00082   pid->deadband = 0.0;
00083   pid->cycleTime = 1.0;
00084   pid->configured = 0;
00085 
00086 
00087 #ifdef SMOOTH_D_FACTOR
00088   for(i = 0; i < MAX_ERROR_WINDOW; i++)
00089     {
00090       pid->oldErrors[i] = 0.0;
00091       pid->errorWindowFactors[i] = 0.1;
00092     }
00093   pid->errorIndex = 0;
00094 #endif
00095 
00096   /* now clear out the state vars */
00097   return pidReset(pid);
00098 }

int pidIniLoad (  PID_STRUCT *    pid,
const char *    filename
) 
 

Definition at line 274 of file pid.c.

00275 {
00276 #ifndef NO_STDIO_SUPPORT
00277 
00278   PID_STRUCT params;
00279   double cycleTime;
00280   int retval = 0;
00281   const char *inistring;
00282 #ifndef UNDER_CE
00283   FILE *fp;
00284 
00285   if (NULL == (fp = fopen(filename, "r"))) {
00286     rcs_print_error( "can't open ini file %s\n", filename);
00287     return -1;
00288   }
00289 #else
00290   INET_FILE *fp;
00291 
00292   if (NULL == (fp = inet_file_open(filename, "r"))) {
00293     rcs_print_error( "can't open ini file %s\n", filename);
00294     return -1;
00295   }
00296 #endif
00297 
00298   /* forward gains */
00299 
00300   if (NULL != (inistring = iniFind(fp, "P", 0))) {
00301 #ifndef UNDER_CE
00302     if (1 != sscanf((char *) inistring, "%lf", &params.p)) {
00303       /* found, but invalid */
00304       rcs_print_error( "invalid P gain: %s\n", inistring);
00305       retval = -1;
00306     }
00307 #else
00308     params.p = RCS_CE_ATOF(inistring);
00309 #endif
00310   }
00311 
00312   if (NULL != (inistring = iniFind(fp, "I", 0))) {
00313 #ifndef UNDER_CE
00314     if (1 != sscanf((char *) inistring, "%lf", &params.i)) {
00315       /* found, but invalid */
00316       rcs_print_error( "invalid I gain: %s\n", inistring);
00317       retval = -1;
00318     }
00319 #else
00320     params.i = RCS_CE_ATOF(inistring);
00321 #endif
00322   }
00323 
00324   if (NULL != (inistring = iniFind(fp, "D", 0))) {
00325 #ifndef UNDER_CE
00326     if (1 != sscanf((char *) inistring, "%lf", &params.d)) {
00327       /* found, but invalid */
00328       rcs_print_error( "invalid D gain: %s\n", inistring);
00329       retval = -1;
00330     }
00331 #else
00332     params.d = RCS_CE_ATOF(inistring);
00333 #endif
00334   }
00335 
00336   if (NULL != (inistring = iniFind(fp, "FF0", 0))) {
00337 #ifndef UNDER_CE
00338     if (1 != sscanf((char *) inistring, "%lf", &params.ff0)) {
00339       /* found, but invalid */
00340       rcs_print_error( "invalid FF0 gain: %s\n", inistring);
00341       retval = -1;
00342     }
00343 #else
00344     params.ff0 = RCS_CE_ATOF(inistring);
00345 #endif
00346   }
00347 
00348   if (NULL != (inistring = iniFind(fp, "FF1", 0))) {
00349 #ifndef UNDER_CE
00350     if (1 != sscanf((char *) inistring, "%lf", &params.ff1)) {
00351       /* found, but invalid */
00352       rcs_print_error( "invalid FF1 gain: %s\n", inistring);
00353       retval = -1;
00354     }
00355 #else
00356     params.ff1 = RCS_CE_ATOF(inistring);
00357 #endif
00358   }
00359 
00360   if (NULL != (inistring = iniFind(fp, "FF2", 0))) {
00361 #ifndef UNDER_CE
00362     if (1 != sscanf((char *) inistring, "%lf", &params.ff2)) {
00363       /* found, but invalid */
00364       rcs_print_error( "invalid FF2 gain: %s\n", inistring);
00365       retval = -1;
00366     }
00367 #else
00368     params.ff2 = RCS_CE_ATOF(inistring);
00369 #endif
00370   }
00371 
00372   if (NULL != (inistring = iniFind(fp, "BACKLASH", 0))) {
00373 #ifndef UNDER_CE
00374     if (1 != sscanf((char *) inistring, "%lf", &params.backlash)) {
00375       /* found, but invalid */
00376       rcs_print_error( "invalid backlash: %s\n", inistring);
00377       retval = -1;
00378     }
00379 #else
00380     params.backlash = RCS_CE_ATOF(inistring);
00381 #endif
00382   }
00383 
00384   if (NULL != (inistring = iniFind(fp, "BIAS", 0))) {
00385 #ifndef UNDER_CE
00386     if (1 != sscanf((char *) inistring, "%lf", &params.bias)) {
00387       /* found, but invalid */
00388       rcs_print_error( "invalid bias: %s\n", inistring);
00389       retval = -1;
00390     }
00391 #else
00392     params.bias = RCS_CE_ATOF(inistring);
00393 #endif
00394   }
00395 
00396   if (NULL != (inistring = iniFind(fp, "MAX_ERROR", 0))) {
00397 #ifndef UNDER_CE
00398     if (1 != sscanf((char *) inistring, "%lf", &params.maxError)) {
00399       /* found, but invalid */
00400       rcs_print_error( "invalid max cum error: %s\n", inistring);
00401       retval = -1;
00402     }
00403 #else
00404     params.maxError = RCS_CE_ATOF(inistring);
00405 #endif
00406   }
00407 
00408   if (NULL != (inistring = iniFind(fp, "DEADBAND", 0))) {
00409 #ifndef UNDER_CE
00410     if (1 != sscanf((char *) inistring, "%lf", &params.deadband)) {
00411       /* found, but invalid */
00412       rcs_print_error( "invalid deadband: %s\n", inistring);
00413       retval = -1;
00414     }
00415 #else
00416     params.deadband = RCS_CE_ATOF(inistring);
00417 #endif
00418   }
00419 
00420   /* if they all read ok, stick them in pid */
00421   if (retval == 0) {
00422     pidSetGains(pid, params);
00423   }
00424 
00425   if (NULL != (inistring = iniFind(fp, "CYCLE_TIME", 0))) {
00426 #ifndef UNDER_CE
00427     if (1 != sscanf((char *) inistring, "%lf", &cycleTime)) {
00428       /* found, but invalid */
00429       rcs_print_error( "invalid CYCLE_TIME: %s\n", inistring);
00430       retval = -1;
00431     }
00432     else {
00433       pidSetCycleTime(pid, cycleTime);
00434     }
00435 #else
00436     cycleTime = RCS_CE_ATOF(inistring);
00437     pidSetCycleTime(pid, cycleTime);
00438 #endif
00439   }
00440 
00441   /* close inifile */
00442 #ifndef UNDER_CE
00443   fclose(fp);
00444 #else
00445   inet_file_close(fp);
00446 #endif
00447 
00448   return retval;
00449 
00450 #else
00451 
00452   return -1;
00453 
00454 #endif
00455 }

int pidReset (  PID_STRUCT *    pid ) 
 

Definition at line 101 of file pid.c.

00102 {
00103   if (0 == pid) {
00104     return -1;
00105   }
00106 
00107   pid->error = 0.0;
00108   pid->lastError = 0.0;
00109   pid->cumError = 0.0;
00110   pid->lastSetpoint = 0.0;
00111   pid->lastSetpoint_was_set = 0; /* lastSetpoint is no longer valid. */
00112   pid->lastSetpointDot = 0.0;
00113   pid->lastOutput = 0.0;
00114 
00115   return 0;
00116 }

double pidRunCycle (  PID_STRUCT *    pid,
double    sample,
double    setpoint
) 
 

Definition at line 172 of file pid.c.

00173 {
00174   double setpointDot;
00175   double error;
00176 #ifdef SMOOTH_D_FACTOR
00177   double smooth_d;
00178   int i;
00179 #endif
00180 
00181   if( 0 == pid )
00182     {
00183       return 0;
00184     }
00185 
00186   if ( pid->cycleTime < 1e-7 || 
00187       pid->configured != ALL_SET ) {
00188     return 0.0;
00189   }
00190   if(! pid->lastSetpoint_was_set )
00191     {
00192       pid->lastSetpoint = setpoint;
00193       pid->lastSetpointDot = 0.0;
00194       pid->lastSetpoint_was_set = 1;
00195     }
00196 
00197 
00198   /* calculate error and cumError */
00199   error =  setpoint - sample;
00200   if (error > pid->deadband) {
00201     pid->error = error - pid->deadband;
00202   }
00203   else if (error < -pid->deadband) {
00204     pid->error = error + pid->deadband;
00205   }         
00206   else {
00207     pid->error = 0.0;
00208   }
00209   pid->cumError += pid->error * pid->cycleTime;
00210   setpointDot = (setpoint - pid->lastSetpoint) / pid->cycleTime;
00211   if (pid->cumError > pid->maxError) {
00212     pid->cumError = pid->maxError;
00213   }
00214   else if (pid->cumError < -pid->maxError) {
00215     pid->cumError = -pid->maxError;
00216   }
00217 #ifdef THROWAWAY_CUMULATIVE_ERROR_ON_SIGN_CHANGES
00218   if(
00219      (pid->error >= 0.0 && pid->lastError <= 0.0) ||
00220      (pid->error <= 0.0 && pid->lastError >= 0.0))
00221     {
00222       pid->cumError = pid->error * pid->cycleTime;
00223     }
00224 #endif
00225 
00226 #ifdef SMOOTH_D_FACTOR
00227   smooth_d =
00228     ( pid->oldErrors[(pid->errorIndex  + 2*MAX_ERROR_WINDOW)%MAX_ERROR_WINDOW]
00229       - pid->oldErrors[(pid->errorIndex +1 + 2*MAX_ERROR_WINDOW)%MAX_ERROR_WINDOW]) /MAX_ERROR_WINDOW;
00230 #if !defined(rtlinux) && !defined(rtai)
00231   if(error != 0.0)
00232     {
00233       for(i = 0; i < MAX_ERROR_WINDOW; i++)
00234         {
00235           printf("%8.8f ",( pid->oldErrors[(pid->errorIndex -i + 2*MAX_ERROR_WINDOW)%MAX_ERROR_WINDOW]
00236                             - pid->oldErrors[(pid->errorIndex -i-1 + 2*MAX_ERROR_WINDOW)%MAX_ERROR_WINDOW]));
00237         }
00238     }
00239 #endif
00240   pid->errorIndex++;
00241   pid->errorIndex %= MAX_ERROR_WINDOW;
00242   pid->oldErrors[pid->errorIndex] = pid->error;
00243 #if !defined(rtlinux)  && !defined(rtai)
00244   if(error != 0.0)
00245     {
00246       printf("\nerror=%8.8f,(error-lastError)=%8.8f,smooth_d=%8.8f\n",
00247              pid->error,(pid->error-pid->lastError),smooth_d);
00248     }
00249 #endif
00250 #endif
00251 
00252 
00253   /* do compensation calculations */
00254   pid->lastOutput = pid->bias +
00255     pid->p * pid->error +
00256     pid->i * pid->cumError +
00257 #ifdef SMOOTH_D_FACTOR
00258     pid->d * smooth_d /pid->cycleTime;
00259 #else
00260     pid->d * (pid->error - pid->lastError) / pid->cycleTime +
00261 #endif
00262     pid->ff0 * setpoint +
00263     pid->ff1 * setpointDot +
00264     pid->ff2 * (setpointDot - pid->lastSetpointDot) / pid->cycleTime;
00265 
00266   /* update history vars */
00267   pid->lastSetpoint = setpoint;
00268   pid->lastSetpointDot = setpointDot;
00269   pid->lastError = pid->error;
00270 
00271   return pid->lastOutput;
00272 }

int pidSetCycleTime (  PID_STRUCT *    pid,
double    seconds
) 
 

Definition at line 118 of file pid.c.

00119 {
00120   if (0 == pid ||
00121       seconds <= 0.0) {
00122     return -1;
00123   }
00124 
00125   pid->cycleTime = seconds;
00126   pid->configured |= CYCLE_TIME_SET;
00127 
00128   return 0;
00129 }

int pidSetGains (  PID_STRUCT *    pid,
PID_STRUCT    parameters
) 
 

Definition at line 131 of file pid.c.

00132 {
00133 #ifdef SMOOTH_D_FACTOR
00134   int i;
00135 #endif
00136 
00137   if (0 == pid) {
00138     return -1;
00139   }
00140 
00141   pid->p = parameters.p;
00142   pid->i = parameters.i;
00143   pid->d = parameters.d;
00144   pid->ff0 = parameters.ff0;
00145   pid->ff1 = parameters.ff1;
00146   pid->ff2 = parameters.ff2;
00147   pid->backlash = parameters.backlash;
00148   pid->bias = parameters.bias;
00149   pid->maxError = parameters.maxError;
00150   pid->deadband = parameters.deadband;
00151 
00152 
00153 #ifdef SMOOTH_D_FACTOR
00154   for(i = 0; i < MAX_ERROR_WINDOW; i++)
00155     {
00156       pid->oldErrors[i] = 0.0;
00157 #if 0
00158       if(parameters.errorWindowFactors[0] > 0.0)
00159         {
00160           pid->errorWindowFactors[i] = parameters.errorWindowFactors[i];
00161         }
00162 #endif
00163     }
00164   pid->errorIndex = 0;
00165 #endif
00166 
00167   pid->configured |= GAINS_SET;
00168 
00169   return 0;
00170 }

Generated on Sun Dec 2 15:28:11 2001 for EMC by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001