This graph shows which files directly or indirectly include this file:
Go to the source code of this file.
Defines | |
#define | __attribute__(x) |
Functions | |
char | __attribute__ ((unused)) pid_h[]="$Id |
int | pidInit (PID_STRUCT *pid) |
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) |
Variables | |
PID_STRUCT |
|
|
|
Definition at line 40 of file pid.h. 00040 : pid.h,v 1.6 2001/07/06 21:50:05 proctor Exp $"; 00041 00042 typedef struct 00043 { 00044 double p; /* proportional gain, volts/unit */ 00045 double i; /* integral gain, volts/unit */ 00046 double d; /* derivative gain, volts/unit per sec */ 00047 double ff0; /* 0th order feedforward, volts/unit */ 00048 double ff1; /* 1st order feedforward, volts/unit/sec */ 00049 double ff2; /* 2nd order feedforward, volts/unit/sec^2 */ 00050 double backlash; /* backlash */ 00051 double bias; /* bias for gravity comp, for example */ 00052 double maxError; /* ceiling for cumError */ 00053 double deadband; /* delta below which error is deemed to be 0 */ 00054 double cycleTime; /* copy of arg to setCycleTime() */ 00055 double error; /* error this cycle */ 00056 double lastError; /* last error computed */ 00057 double lastSetpoint; /* last setpoint received */ 00058 double lastOutput; /* last value output */ 00059 double lastSetpointDot; /* delta setpoint / delta t */ 00060 double cumError; /* cumulative error, all cycles */ 00061 int configured; /* configure flags */ 00062 #ifdef SMOOTH_D_FACTOR 00063 #define MAX_ERROR_WINDOW 4 00064 double oldErrors[MAX_ERROR_WINDOW]; /* This is a ring buffer that stores the 00065 last MAX_ERROR_WINDOW errors. */ 00066 double errorWindowFactors[MAX_ERROR_WINDOW];/* Weights to use in smoothing 00067 error factors. */ 00068 int errorIndex; /* Index into the oldErrors ring buffer. */ 00069 #endif 00070 int lastSetpoint_was_set; /* true when lastSetpoint is valid. */ 00071 00072 } PID_STRUCT; |
|
Definition at line 274 of file pid.c. Referenced by main().
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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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", ¶ms.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 } |
|
Referenced by init_module().
|
|
Definition at line 101 of file pid.c. Referenced by __attribute__(), and emcmotController().
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 } |
|
Definition at line 172 of file pid.c. Referenced by emcmotController().
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 } |
|
Definition at line 118 of file pid.c. Referenced by pidIniLoad(), and setServoCycleTime().
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 } |
|
Definition at line 131 of file pid.c. Referenced by emcmotCommandHandler(), init_module(), and pidIniLoad().
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 } |
|
|