at_pid.c
1 /****************************************************************************** 2 * 3 * Copyright (C) 2003 John Kasunich <jmkasunich AT users DOT sourceforge DOT net> 4 * Copyright (C) 2007 Peter G. Vavaroutsos <pete AT vavaroutsos DOT com> 5 * 6 * 7 ****************************************************************************** 8 * 9 * This file, 'pid.c', is a HAL component that provides Proportional/ 10 * Integeral/Derivative control loops. It is a realtime component. 11 * 12 * It supports a maximum of 16 PID loops, as set by the insmod parameter 13 * 'num_chan=' or the 'names=" specifier. The 'num_chan=' and 'names=' 14 * specifiers are mutually exclusive. 15 * 16 * In this documentation, it is assumed that we are discussing position 17 * loops. However this component can be used to implement other loops 18 * such as speed loops, torch height control, and others. 19 * 20 * Each loop has a number of pins and parameters 21 * When the 'num_chan=' method is used, names begin with 'pid.x.', where 22 * 'x' is the channel number. Channel numbers start at zero. 23 * When the 'names=' method is used, names begin with the specified names, 24 * e.g., for 'names=PID', the pin/parameter begin with "PID." 25 * 26 * The three most important pins are 'command', 'feedback', and 27 * 'output'. For a position loop, 'command' and 'feedback' are 28 * in position units. For a linear axis, this could be inches, 29 * mm, metres, or whatever is relavent. Likewise, for a angular 30 * axis, it could be degrees, radians, etc. The units of the 31 * 'output' pin represent the change needed to make the feedback 32 * match the command. As such, for a position loop 'Output' is 33 * a velocity, in inches/sec, mm/sec, degrees/sec, etc. 34 * 35 * Each loop has several other pins as well. 'error' is equal to 36 * 'command' minus 'feedback'. 'enable' is a bit that enables 37 * the loop. If 'enable' is false, all integrators are reset, 38 * and the output is forced to zero. If 'enable' is true, the 39 * loop operates normally. 40 * 41 * The PID gains, limits, and other 'tunable' features of the 42 * loop are implemented as parameters. These are as follows: 43 * 44 * Pgain Proportional gain 45 * Igain Integral gain 46 * Dgain Derivative gain 47 * bias Constant offset on output 48 * FF0 Zeroth order Feedforward gain 49 * FF1 First order Feedforward gain 50 * FF2 Second order Feedforward gain 51 * deadband Amount of error that will be ignored 52 * maxerror Limit on error 53 * maxerrorI Limit on error integrator 54 * maxerrorD Limit on error differentiator 55 * maxcmdD Limit on command differentiator 56 * maxcmdDD Limit on command 2nd derivative 57 * maxoutput Limit on output value 58 * 59 * All of the limits (max____) are implemented such that if the 60 * parameter value is zero, there is no limit. 61 * 62 * A number of internal values which may be useful for testing 63 * and tuning are also available as parameters. To avoid cluttering 64 * the parameter list, these are only exported if "debug=1" is 65 * specified on the insmod command line. 66 * 67 * errorI Integral of error 68 * errorD Derivative of error 69 * commandD Derivative of the command 70 * commandDD 2nd derivative of the command 71 * 72 * The PID loop calculations are as follows (see the code for 73 * all the nitty gritty details): 74 * 75 * error = command - feedback 76 * if ( fabs(error) < deadband ) then error = 0 77 * limit error to +/- maxerror 78 * errorI += error * period 79 * limit errorI to +/- maxerrorI 80 * errorD = (error - previouserror) / period 81 * limit errorD to +/- paxerrorD 82 * commandD = (command - previouscommand) / period 83 * limit commandD to +/- maxcmdD 84 * commandDD = (commandD - previouscommandD) / period 85 * limit commandDD to +/- maxcmdDD 86 * output = bias + error * Pgain + errorI * Igain + 87 * errorD * Dgain + command * FF0 + commandD * FF1 + 88 * commandDD * FF2 89 * limit output to +/- maxoutput 90 * 91 * This component exports one function called 'pid.x.do-pid-calcs' 92 * for each PID loop. This allows loops to be included in different 93 * threads and execute at different rates. 94 * 95 ****************************************************************************** 96 * 97 * This program is free software; you can redistribute it and/or 98 * modify it under the terms of version 2 of the GNU General 99 * Public License as published by the Free Software Foundation. 100 * This library is distributed in the hope that it will be useful, 101 * but WITHOUT ANY WARRANTY; without even the implied warranty of 102 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 103 * GNU General Public License for more details. 104 * 105 * You should have received a copy of the GNU General Public 106 * License along with this library; if not, write to the Free Software 107 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 108 * 109 * THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR 110 * ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE 111 * TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of 112 * harming persons must have provisions for completely removing power 113 * from all motors, etc, before persons enter any danger area. All 114 * machinery must be designed to comply with local and national safety 115 * codes, and the authors of this software can not, and do not, take 116 * any responsibility for such compliance. 117 * 118 * This code was written as part of the EMC HAL project. For more 119 * information, go to www.linuxcnc.org. 120 * 121 ******************************************************************************/ 122 123 #include "rtapi.h" // RTAPI realtime OS API. 124 #include "rtapi_app.h" // RTAPI realtime module decls. 125 #include "rtapi_math.h" 126 #include "hal.h" // HAL public API decls. 127 128 129 // Module information. 130 MODULE_AUTHOR("Pete Vavaroutsos"); 131 MODULE_DESCRIPTION("Auto-Tune PID Loop Component for EMC HAL"); 132 MODULE_LICENSE("GPL"); 133 static int num_chan; // Number of channels. 134 static int default_num_chan = 3; 135 static int howmany; 136 RTAPI_MP_INT(num_chan, "number of channels"); 137 static int debug = 0; // Flag to export optional params. 138 RTAPI_MP_INT(debug, "enables optional params"); 139 140 #define MAX_CHAN 16 141 static char *names[MAX_CHAN] = {0,}; 142 RTAPI_MP_ARRAY_STRING(names, MAX_CHAN, "names of at_pid components"); 143 144 #define PI 3.141592653589 145 146 147 /****************************************************************************** 148 * PID OBJECT 149 * 150 * This structure contains the runtime data for a single PID loop. 151 ******************************************************************************/ 152 153 typedef enum { 154 STATE_PID, 155 STATE_TUNE_IDLE, 156 STATE_TUNE_START, 157 STATE_TUNE_POS, 158 STATE_TUNE_NEG, 159 STATE_TUNE_ABORT, 160 } State; 161 162 163 // Values for tune-type parameter. 164 typedef enum { 165 TYPE_PID, 166 TYPE_PI_FF1, 167 } Mode; 168 169 170 typedef struct { 171 // Pins. (former params) 172 hal_float_t *deadband; 173 hal_float_t *maxError; // Limit for error. 174 hal_float_t *maxErrorI; // Limit for integrated error. 175 hal_float_t *maxErrorD; // Limit for differentiated error. 176 hal_float_t *maxCmdD; // Limit for differentiated cmd. 177 hal_float_t *maxCmdDd; // Limit for 2nd derivative of cmd. 178 179 hal_float_t *bias; // Steady state offset. 180 hal_float_t *pGain; // Proportional gain. 181 hal_float_t *iGain; // Integral gain. 182 hal_float_t *dGain; // Derivative gain. 183 hal_float_t *ff0Gain; // Feedforward proportional. 184 hal_float_t *ff1Gain; // Feedforward derivative. 185 hal_float_t *ff2Gain; // Feedforward 2nd derivative. 186 hal_float_t *maxOutput; // Limit for PID output. 187 hal_float_t *tuneEffort; // Control effort for limit cycle. 188 hal_u32_t *tuneCycles; 189 hal_u32_t *tuneType; 190 191 hal_float_t *errorI; // Integrated error. 192 hal_float_t *errorD; // Differentiated error. 193 hal_float_t *cmdD; // Differentiated command. 194 hal_float_t *cmdDd; // 2nd derivative of command. 195 hal_float_t *ultimateGain; // Calc by auto-tune from limit cycle. 196 hal_float_t *ultimatePeriod; // Calc by auto-tune from limit cycle. 197 198 // Pins. 199 hal_bit_t *pEnable; 200 hal_float_t *pCommand; // Commanded value. 201 hal_float_t *pFeedback; // Feedback value. 202 hal_float_t *pError; // Command - feedback. 203 hal_float_t *pOutput; // The output value. 204 hal_bit_t *pTuneMode; // 0=PID, 1=tune. 205 hal_bit_t *pTuneStart; // Set to 1 to start an auto-tune cycle. 206 // Clears automatically when the cycle 207 // has finished. 208 209 // Private data. 210 hal_float_t prevError; // previous error for differentiator. 211 hal_float_t prevCmd; // previous command for differentiator. 212 hal_float_t limitState; // +1 or -1 if in limit, else 0. 213 State state; 214 hal_u32_t cycleCount; 215 hal_u32_t cyclePeriod; 216 hal_float_t cycleAmplitude; 217 hal_float_t totalTime; 218 hal_float_t avgAmplitude; 219 } Pid; 220 221 222 // These methods are used for initialization. 223 static int Pid_Init(Pid *this); 224 static int Pid_Export(Pid *this, int compId, char* prefix); 225 static void Pid_AutoTune(Pid *this, long period); 226 static void Pid_CycleEnd(Pid *this); 227 228 // These methods are exported to the HAL. 229 static void Pid_Refresh(void *this, long period); 230 231 /****************************************************************************** 232 * COMPONENT OBJECT 233 * 234 * This object contains all the data for this HAL component. 235 * 236 ******************************************************************************/ 237 238 239 typedef struct { 240 int id; // HAL component ID. 241 Pid *pidTable; 242 } Component; 243 244 static Component component; 245 246 247 /****************************************************************************** 248 * INIT AND EXIT CODE 249 ******************************************************************************/ 250 251 int 252 rtapi_app_main(void) 253 { 254 int retval,n,i; 255 Pid *pComp; 256 257 if(num_chan && names[0]) { 258 rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n"); 259 return -EINVAL; 260 } 261 if(!num_chan && !names[0]) num_chan = default_num_chan; 262 263 if(num_chan) { 264 howmany = num_chan; 265 } else { 266 howmany = 0; 267 for (i = 0; i < MAX_CHAN; i++) { 268 if ( (names[i] == NULL) || (*names[i] == 0) ){ 269 break; 270 } 271 howmany = i + 1; 272 } 273 } 274 275 // Check number of channels. 276 if((howmany <= 0) || (howmany > MAX_CHAN)){ 277 rtapi_print_msg(RTAPI_MSG_ERR, 278 "AT_PID: ERROR: invalid number of channels: %d\n", howmany); 279 return(-1); 280 } 281 282 // Connect to the HAL. 283 component.id = hal_init("at_pid"); 284 if(component.id < 0){ 285 rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_init() failed\n"); 286 return(-1); 287 } 288 289 // Allocate memory for pid objects. 290 component.pidTable = hal_malloc(howmany * sizeof(*pComp)); 291 292 if(component.pidTable == NULL){ 293 rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_malloc() failed\n"); 294 hal_exit(component.id); 295 return(-1); 296 } 297 298 pComp = component.pidTable; 299 i = 0; // for names= items 300 for(n = 0; n < howmany; n++, pComp++){ 301 302 // Export pins, parameters, and functions. 303 if(num_chan) { 304 char buf[HAL_NAME_LEN + 1]; 305 // note name is pid not at_pid 306 rtapi_snprintf(buf, sizeof(buf), "pid.%d", n); 307 retval = Pid_Export(pComp, component.id, buf); 308 } else { 309 retval = Pid_Export(pComp, component.id, names[i++]); 310 } 311 if (retval != 0) { 312 rtapi_print_msg(RTAPI_MSG_ERR, 313 "AT_PID: ERROR: at_pid %d var export failed\n", n); 314 hal_exit(component.id); 315 return -1; 316 } 317 318 // Initialize pid. 319 if(Pid_Init(pComp)){ 320 hal_exit(component.id); 321 return(-1); 322 } 323 } 324 325 hal_ready(component.id); 326 return(0); 327 } 328 329 330 void 331 rtapi_app_exit(void) 332 { 333 Pid *pComp; 334 335 hal_exit(component.id); 336 337 if((pComp = component.pidTable) != NULL){ 338 // TODO: Free pid objects when HAL supports free. 339 // hal_free(pComp); 340 } 341 } 342 343 344 /****************************************************************************** 345 * PID OBJECT FUNCTION DEFINITIONS 346 ******************************************************************************/ 347 348 /* 349 * LOCAL FUNCTIONS 350 */ 351 352 static int 353 Pid_Init(Pid *this) 354 { 355 // Init all structure members. 356 *(this->deadband) = 0; 357 *(this->maxError) = 0; 358 *(this->maxErrorI) = 0; 359 *(this->maxErrorD) = 0; 360 *(this->maxCmdD) = 0; 361 *(this->maxCmdDd) = 0; 362 *(this->errorI) = 0; 363 this->prevError = 0; 364 *(this->errorD) = 0; 365 this->prevCmd = 0; 366 this->limitState = 0; 367 *(this->cmdD) = 0; 368 *(this->cmdDd) = 0; 369 *(this->bias) = 0; 370 *(this->pGain) = 1; 371 *(this->iGain) = 0; 372 *(this->dGain) = 0; 373 *(this->ff0Gain) = 0; 374 *(this->ff1Gain) = 0; 375 *(this->ff2Gain) = 0; 376 *(this->maxOutput) = 0; 377 this->state = STATE_PID; 378 *(this->tuneCycles) = 50; 379 *(this->tuneEffort) = 0.5; 380 *(this->tuneType) = TYPE_PID; 381 382 return(0); 383 } 384 385 386 static int 387 Pid_Export(Pid *this, int compId,char* prefix) 388 { 389 int error, msg; 390 char buf[HAL_NAME_LEN + 1]; 391 392 // This function exports a lot of stuff, which results in a lot of 393 // logging if msg_level is at INFO or ALL. So we save the current value 394 // of msg_level and restore it later. If you actually need to log this 395 // function's actions, change the second line below. 396 msg = rtapi_get_msg_level(); 397 rtapi_set_msg_level(RTAPI_MSG_WARN); 398 399 // Export pins. 400 error = hal_pin_bit_newf(HAL_IN, &(this->pEnable), compId, "%s.enable", prefix); 401 402 if(!error){ 403 error = hal_pin_float_newf(HAL_IN, &(this->pCommand), compId, "%s.command", prefix); 404 } 405 406 if(!error){ 407 error = hal_pin_float_newf(HAL_IN, &(this->pFeedback), compId, "%s.feedback", prefix); 408 } 409 410 if(!error){ 411 error = hal_pin_float_newf(HAL_OUT, &(this->pError), compId, "%s.error", prefix); 412 } 413 414 if(!error){ 415 error = hal_pin_float_newf(HAL_OUT, &(this->pOutput), compId, "%s.output", prefix); 416 } 417 418 if(!error){ 419 error = hal_pin_bit_newf(HAL_IN, &(this->pTuneMode), compId, "%s.tune-mode", prefix); 420 } 421 422 if(!error){ 423 error = hal_pin_bit_newf(HAL_IO, &(this->pTuneStart), compId, "%s.tune-start", prefix); 424 } 425 426 // Export pins. (former parameters). 427 if(!error){ 428 error = hal_pin_float_newf(HAL_IO, &(this->deadband), compId, "%s.deadband", prefix); 429 } 430 431 if(!error){ 432 error = hal_pin_float_newf(HAL_IO, &(this->maxError), compId, "%s.maxerror", prefix); 433 } 434 435 if(!error){ 436 error = hal_pin_float_newf(HAL_IO, &(this->maxErrorI), compId, "%s.maxerrorI", prefix); 437 } 438 439 if(!error){ 440 error = hal_pin_float_newf(HAL_IO, &(this->maxErrorD), compId, "%s.maxerrorD", prefix); 441 } 442 443 if(!error){ 444 error = hal_pin_float_newf(HAL_IO, &(this->maxCmdD), compId, "%s.maxcmdD", prefix); 445 } 446 447 if(!error){ 448 error = hal_pin_float_newf(HAL_IO, &(this->maxCmdDd), compId, "%s.maxcmdDD", prefix); 449 } 450 451 if(!error){ 452 error = hal_pin_float_newf(HAL_IO, &(this->bias), compId, "%s.bias", prefix); 453 } 454 455 if(!error){ 456 error = hal_pin_float_newf(HAL_IO, &(this->pGain), compId, "%s.Pgain", prefix); 457 } 458 459 if(!error){ 460 error = hal_pin_float_newf(HAL_IO, &(this->iGain), compId, "%s.Igain", prefix); 461 } 462 463 if(!error){ 464 error = hal_pin_float_newf(HAL_IO, &(this->dGain), compId, "%s.Dgain", prefix); 465 } 466 467 if(!error){ 468 error = hal_pin_float_newf(HAL_IO, &(this->ff0Gain), compId, "%s.FF0", prefix); 469 } 470 471 if(!error){ 472 error = hal_pin_float_newf(HAL_IO, &(this->ff1Gain), compId, "%s.FF1", prefix); 473 } 474 475 if(!error){ 476 error = hal_pin_float_newf(HAL_IO, &(this->ff2Gain), compId, "%s.FF2", prefix); 477 } 478 479 if(!error){ 480 error = hal_pin_float_newf(HAL_IO, &(this->maxOutput), compId, "%s.maxoutput", prefix); 481 } 482 483 if(!error){ 484 error = hal_pin_float_newf(HAL_IO, &(this->tuneEffort), compId, "%s.tune-effort", prefix); 485 } 486 487 if(!error){ 488 error = hal_pin_u32_newf(HAL_IO, &(this->tuneCycles), compId, "%s.tune-cycles", prefix); 489 } 490 491 if(!error){ 492 error = hal_pin_u32_newf(HAL_IO, &(this->tuneType), compId, "%s.tune-type", prefix); 493 } 494 495 // Export optional parameters. 496 if(debug > 0){ 497 if(!error){ 498 error = hal_pin_float_newf(HAL_OUT, &(this->errorI), compId, "%s.errorI", prefix); 499 } 500 501 if(!error){ 502 error = hal_pin_float_newf(HAL_OUT, &(this->errorD), compId, "%s.errorD", prefix); 503 } 504 505 if(!error){ 506 error = hal_pin_float_newf(HAL_OUT, &(this->cmdD), compId, "%s.commandD", prefix); 507 } 508 509 if(!error){ 510 error = hal_pin_float_newf(HAL_OUT, &(this->cmdDd), compId, "%s.commandDD", prefix); 511 } 512 513 if(!error){ 514 error = hal_pin_float_newf(HAL_OUT, &(this->ultimateGain), compId, "%s.ultimate-gain", prefix); 515 } 516 517 if(!error){ 518 error = hal_pin_float_newf(HAL_OUT, &(this->ultimatePeriod), compId, "%s.ultimate-period", prefix); 519 } 520 } else { 521 this->errorI = (hal_float_t *) hal_malloc(sizeof(hal_float_t)); 522 this->errorD = (hal_float_t *) hal_malloc(sizeof(hal_float_t)); 523 this->cmdD = (hal_float_t *) hal_malloc(sizeof(hal_float_t)); 524 this->cmdDd = (hal_float_t *) hal_malloc(sizeof(hal_float_t)); 525 this->ultimateGain = (hal_float_t *) hal_malloc(sizeof(hal_float_t)); 526 this->ultimatePeriod = (hal_float_t *) hal_malloc(sizeof(hal_float_t)); 527 } 528 529 // Export functions. 530 if(!error){ 531 rtapi_snprintf(buf, sizeof(buf), "%s.do-pid-calcs", prefix); 532 error = hal_export_funct(buf, Pid_Refresh, this, 1, 0, compId); 533 } 534 535 if(!error){ 536 *this->pEnable = 0; 537 *this->pCommand = 0; 538 *this->pFeedback = 0; 539 *this->pError = 0; 540 *this->pOutput = 0; 541 *this->pTuneMode = 0; 542 *this->pTuneStart = 0; 543 } 544 545 // Restore saved message level. 546 rtapi_set_msg_level(msg); 547 return(error); 548 } 549 550 551 /* 552 * Perform an auto-tune operation. Sets up a limit cycle using the specified 553 * tune effort. Averages the amplitude and period over the specified number of 554 * cycles. This characterizes the process and determines the ultimate gain 555 * and period, which are then used to calculate PID. 556 * 557 * CO(t) = P * [ e(t) + 1/Ti * (f e(t)dt) - Td * (d/dt PV(t)) ] 558 * 559 * Pu = 4/PI * tuneEffort / responseAmplitude 560 * Ti = 0.5 * responsePeriod 561 * Td = 0.125 * responsePeriod 562 * 563 * P = 0.6 * Pu 564 * I = P * 1/Ti 565 * D = P * Td 566 */ 567 static void 568 Pid_AutoTune(Pid *this, long period) 569 { 570 hal_float_t error; 571 572 // Calculate the error. 573 error = *this->pCommand - *this->pFeedback; 574 *this->pError = error; 575 576 // Check if enabled and if still in tune mode. 577 if(!*this->pEnable || !*this->pTuneMode){ 578 this->state = STATE_TUNE_ABORT; 579 } 580 581 switch(this->state){ 582 case STATE_TUNE_IDLE: 583 // Wait for tune start command. 584 if(*this->pTuneStart) 585 this->state = STATE_TUNE_START; 586 break; 587 588 case STATE_TUNE_START: 589 // Initialize tuning variables and start limit cycle. 590 this->state = STATE_TUNE_POS; 591 this->cycleCount = 0; 592 this->cyclePeriod = 0; 593 this->cycleAmplitude = 0; 594 this->totalTime = 0; 595 this->avgAmplitude = 0; 596 *(this->ultimateGain) = 0; 597 *(this->ultimatePeriod) = 0; 598 *this->pOutput = *(this->bias) + fabs(*(this->tuneEffort)); 599 break; 600 601 case STATE_TUNE_POS: 602 case STATE_TUNE_NEG: 603 this->cyclePeriod += period; 604 605 if(error < 0.0){ 606 // Check amplitude. 607 if(-error > this->cycleAmplitude) 608 this->cycleAmplitude = -error; 609 610 // Check for end of cycle. 611 if(this->state == STATE_TUNE_POS){ 612 this->state = STATE_TUNE_NEG; 613 Pid_CycleEnd(this); 614 } 615 616 // Update output so user can ramp effort until movement occurs. 617 *this->pOutput = *(this->bias) - fabs(*(this->tuneEffort)); 618 }else{ 619 // Check amplitude. 620 if(error > this->cycleAmplitude) 621 this->cycleAmplitude = error; 622 623 // Check for end of cycle. 624 if(this->state == STATE_TUNE_NEG){ 625 this->state = STATE_TUNE_POS; 626 Pid_CycleEnd(this); 627 } 628 629 // Update output so user can ramp effort until movement occurs. 630 *this->pOutput = *(this->bias) + fabs(*(this->tuneEffort)); 631 } 632 633 // Check if the last cycle just ended. This is really the number 634 // of half cycles. 635 if(this->cycleCount < *(this->tuneCycles)) 636 break; 637 638 // Calculate PID. 639 *(this->ultimateGain) = (4.0 * fabs(*(this->tuneEffort)))/(PI * this->avgAmplitude); 640 *(this->ultimatePeriod) = 2.0 * this->totalTime / *(this->tuneCycles); 641 *(this->ff0Gain) = 0; 642 *(this->ff2Gain) = 0; 643 644 if(*(this->tuneType) == TYPE_PID){ 645 // PID. 646 *(this->pGain) = 0.6 * *(this->ultimateGain); 647 *(this->iGain) = *(this->pGain) / (*(this->ultimatePeriod) / 2.0); 648 *(this->dGain) = *(this->pGain) * (*(this->ultimatePeriod) / 8.0); 649 *(this->ff1Gain) = 0; 650 }else{ 651 // PI FF1. 652 *(this->pGain) = 0.45 * *(this->ultimateGain); 653 *(this->iGain) = *(this->pGain) / (*(this->ultimatePeriod) / 1.2); 654 *(this->dGain) = 0; 655 656 // Scaling must be set so PID output is in user units per second. 657 *(this->ff1Gain) = 1; 658 } 659 660 // Fall through. 661 662 case STATE_TUNE_ABORT: 663 default: 664 // Force output to zero. 665 *this->pOutput = 0; 666 667 // Abort any tuning cycle in progress. 668 *this->pTuneStart = 0; 669 this->state = (*this->pTuneMode)? STATE_TUNE_IDLE: STATE_PID; 670 } 671 } 672 673 674 static void 675 Pid_CycleEnd(Pid *this) 676 { 677 this->cycleCount++; 678 this->avgAmplitude += this->cycleAmplitude / *(this->tuneCycles); 679 this->cycleAmplitude = 0; 680 this->totalTime += this->cyclePeriod * 0.000000001; 681 this->cyclePeriod = 0; 682 } 683 684 685 /* 686 * HAL EXPORTED FUNCTIONS 687 */ 688 689 static void 690 Pid_Refresh(void *arg, long periodNs) 691 { 692 Pid *this = (Pid *)arg; 693 hal_float_t period, periodRecip; 694 hal_float_t prevCmdD, error, output; 695 696 if(this->state != STATE_PID){ 697 Pid_AutoTune(this, periodNs); 698 return; 699 } 700 701 // Calculate the error. 702 error = *this->pCommand - *this->pFeedback; 703 704 // Store error to error pin. 705 *this->pError = error; 706 707 // Check for tuning mode request. 708 if(*this->pTuneMode){ 709 *(this->errorI) = 0; 710 this->prevError = 0; 711 *(this->errorD) = 0; 712 this->prevCmd = 0; 713 this->limitState = 0; 714 *(this->cmdD) = 0; 715 *(this->cmdDd) = 0; 716 717 // Force output to zero. 718 *(this->pOutput) = 0; 719 720 // Switch to tuning mode. 721 this->state = STATE_TUNE_IDLE; 722 723 return; 724 } 725 726 // Precalculate some timing constants. 727 period = periodNs * 0.000000001; 728 periodRecip = 1.0 / period; 729 730 // Apply error limits. 731 if(*(this->maxError) != 0.0){ 732 if(error > *(this->maxError)){ 733 error = *(this->maxError); 734 }else if(error < -*(this->maxError)){ 735 error = -*(this->maxError); 736 } 737 } 738 739 // Apply the deadband. 740 if(error > *(this->deadband)){ 741 error -= *(this->deadband); 742 }else if(error < -*(this->deadband)){ 743 error += *(this->deadband); 744 }else{ 745 error = 0; 746 } 747 748 // Calculate derivative term. 749 *(this->errorD) = (error - this->prevError) * periodRecip; 750 this->prevError = error; 751 752 // Apply derivative limits. 753 if(*(this->maxErrorD) != 0.0){ 754 if(*(this->errorD) > *(this->maxErrorD)){ 755 *(this->errorD) = *(this->maxErrorD); 756 }else if(*(this->errorD) < -*(this->maxErrorD)){ 757 *(this->errorD) = -*(this->maxErrorD); 758 } 759 } 760 761 // Calculate derivative of command. 762 // Save old value for 2nd derivative calc later. 763 prevCmdD = *(this->cmdD); 764 *(this->cmdD) = (*this->pCommand - this->prevCmd) * periodRecip; 765 this->prevCmd = *this->pCommand; 766 767 // Apply derivative limits. 768 if(*(this->maxCmdD) != 0.0){ 769 if(*(this->cmdD) > *(this->maxCmdD)){ 770 *(this->cmdD) = *(this->maxCmdD); 771 }else if(*(this->cmdD) < -*(this->maxCmdD)){ 772 *(this->cmdD) = -*(this->maxCmdD); 773 } 774 } 775 776 // Calculate 2nd derivative of command. 777 *(this->cmdDd) = (*(this->cmdD) - prevCmdD) * periodRecip; 778 779 // Apply 2nd derivative limits. 780 if(*(this->maxCmdDd) != 0.0){ 781 if(*(this->cmdDd) > *(this->maxCmdDd)){ 782 *(this->cmdDd) = *(this->maxCmdDd); 783 }else if(*(this->cmdDd) < -*(this->maxCmdDd)){ 784 *(this->cmdDd) = -*(this->maxCmdDd); 785 } 786 } 787 788 // Check if enabled. 789 if(!*this->pEnable){ 790 // Reset integrator. 791 *(this->errorI) = 0; 792 793 // Force output to zero. 794 *this->pOutput = 0; 795 this->limitState = 0; 796 797 return; 798 } 799 800 // If output is in limit, don't let integrator wind up. 801 if(error * this->limitState <= 0.0){ 802 // Compute integral term. 803 *(this->errorI) += error * period; 804 } 805 806 // Apply integrator limits. 807 if(*(this->maxErrorI) != 0.0){ 808 if(*(this->errorI) > *(this->maxErrorI)){ 809 *(this->errorI) = *(this->maxErrorI); 810 }else if(*(this->errorI) < -*(this->maxErrorI)){ 811 *(this->errorI) = -*(this->maxErrorI); 812 } 813 } 814 815 // Calculate the output value. 816 output = 817 *(this->bias) + *(this->pGain) * error + *(this->iGain) * *(this->errorI) + 818 *(this->dGain) * *(this->errorD); 819 output += *this->pCommand * *(this->ff0Gain) + *(this->cmdD) * *(this->ff1Gain) + 820 *(this->cmdDd) * *(this->ff2Gain); 821 822 // Apply output limits. 823 if(*(this->maxOutput) != 0.0){ 824 if(output > *(this->maxOutput)){ 825 output = *(this->maxOutput); 826 this->limitState = 1; 827 }else if(output < -*(this->maxOutput)){ 828 output = -*(this->maxOutput); 829 this->limitState = -1; 830 }else{ 831 this->limitState = 0; 832 } 833 } 834 835 // Write final output value to output pin. 836 *this->pOutput = output; 837 }