Subversion Repositories HomeAutomation

Rev

Blame | Last modification | View Log | SVN | RSS feed

  1. #include "PID_AutoTune.h"
  2. #include <avr/pgmspace.h>
  3. #include <math.h>
  4. #include <drivers/timer/timer.h>
  5. #include <stdbool.h>
  6. // source of Tyreus-Luyben and Ciancone-Marlin rules:
  7. // "Autotuning of PID Controllers: A Relay Feedback Approach",
  8. //  by Cheng-Ching Yu, 2nd Edition, p.18
  9. // Tyreus-Luyben is more conservative than Ziegler-Nichols
  10. // and is preferred for lag dominated processes
  11. // Ciancone-Marlin is preferred for delay dominated processes
  12. // Ziegler-Nichols is intended for best disturbance rejection
  13. // can lack robustness especially for lag dominated processes
  14.  
  15. // source for Pessen Integral, Some Overshoot, and No Overshoot rules:
  16. // "Rule-Based Autotuning Based on Frequency Domain Identification"
  17. // by Anthony S. McCormack and Keith R. Godfrey
  18. // IEEE Transactions on Control Systems Technology, vol 6 no 1, January 1998.
  19. // as reported on http://www.mstarlabs.com/control/znrule.html
  20.  
  21. // irrational constants
  22.   //static const float CONST_PI          = 3.14159265358979323846;
  23.   //static const float CONST_SQRT2_DIV_2 = 0.70710678118654752440;
  24.   #define CONST_SQRT2_DIV_2     M_SQRT1_2
  25.   #define CONST_PI              M_PI
  26.  
  27.  float processValueOffset(float,     // * returns an estimate of the process value offset
  28.       float);                          //   as a proportion of the amplitude                                        
  29.  
  30.   float *input;
  31.   float *output;
  32.   float setpoint;
  33.  
  34.   float oStep;
  35.   float noiseBand;
  36.   uint8_t nLookBack;
  37.   uint8_t controlType;                     // * selects autotune algorithm
  38.  
  39.   enum AutoTunerState state;            // * state of autotuner finite state machine
  40.   unsigned long lastTime;
  41.   unsigned long sampleTime;
  42.   enum Peak peakType;
  43.   unsigned long lastPeakTime[5];        // * peak time, most recent in array element 0
  44.   float lastPeaks[5];                  // * peak value, most recent in array element 0
  45.   uint8_t peakCount;
  46.   float lastInputs[101];               // * process values, most recent in array element 0
  47.   uint8_t inputCount;
  48.   float outputStart;
  49.   float workingNoiseBand;
  50.   float workingOstep;
  51.   float inducedAmplitude;
  52.   float Kp, Ti, Td;
  53.  
  54.   // used by AMIGOf tuning rule
  55.   float calculatePhaseLag(float);     // * calculate phase lag from noiseBand and inducedAmplitude
  56.   float fastArcTan(float);
  57.   float newWorkingNoiseBand;
  58.   float K_process;
  59.  
  60. #if defined AUTOTUNE_RELAY_BIAS  
  61.   float relayBias;
  62.   unsigned long lastStepTime[5];        // * step time, most recent in array element 0
  63.   float sumInputSinceLastStep[5];      // * integrated process values, most recent in array element 0
  64.   uint8_t stepCount;
  65. #endif  
  66.  
  67.  
  68. // order must be match enumerated type for auto tune methods
  69. struct Tuning tuningRule[NO_OVERSHOOT_PID + 1]  PROGMEM =
  70. {  
  71.   { {  44, 24,   0 } },  // ZIEGLER_NICHOLS_PI
  72.   { {  34, 40, 160 } },  // ZIEGLER_NICHOLS_PID
  73.   { {  64,  9,   0 } },  // TYREUS_LUYBEN_PI
  74.   { {  44,  9, 126 } },  // TYREUS_LUYBEN_PID
  75.   { {  66, 80,   0 } },  // CIANCONE_MARLIN_PI
  76.   { {  66, 88, 162 } },  // CIANCONE_MARLIN_PID
  77.   { {  28, 50, 133 } },  // PESSEN_INTEGRAL_PID
  78.   { {  60, 40,  60 } },  // SOME_OVERSHOOT_PID
  79.   { { 100, 40,  60 } }   // NO_OVERSHOOT_PID
  80. };
  81.  
  82. static bool PID_ATune_PI_controller(void)
  83. {
  84. return pgm_read_byte_near(&tuningRule[2]) == 0;
  85. }
  86.  
  87. static float PID_ATune_divisor( uint8_t index)  
  88. {
  89. return (float)pgm_read_byte_near(&tuningRule[index]) * 0.05;
  90. }
  91.  
  92. void PID_ATune_Init(float* Input, float* Output)
  93. {
  94.   input = Input;
  95.   output = Output;
  96.  
  97.   // constructor defaults
  98.   controlType = ZIEGLER_NICHOLS_PI;
  99.   noiseBand = 0.5;
  100.   state = AUTOTUNER_OFF;
  101.   oStep = 10.0;
  102.   PID_ATune_SetLookbackSec(10);
  103. }
  104.  
  105. void PID_ATune_Cancel(void)
  106. {
  107.   state = AUTOTUNER_OFF;
  108. }
  109.  
  110. float inline PID_ATune_fastArcTan(float x)
  111. {
  112.   // source: “Efficient approximations for the arctangent function”, Rajan, S. Sichun Wang Inkol, R. Joyal, A., May 2006
  113.   //return CONST_PI / 4.0 * x - x * (abs(x) - 1.0) * (0.2447 + 0.0663 * abs(x));
  114.  
  115.   // source: "Understanding Digital Signal Processing", 2nd Ed, Richard G. Lyons, eq. 13-107
  116.   return x / (1.0 + 0.28125 * pow(x, 2));
  117. }
  118.  
  119. float PID_ATune_calculatePhaseLag(float inducedAmplitude)
  120. {
  121.   // calculate phase lag
  122.   // NB hysteresis = 2 * noiseBand;
  123.   float ratio = 2.0 * workingNoiseBand / inducedAmplitude;
  124.   if (ratio > 1.0)
  125.   {
  126.     return CONST_PI / 2.0;
  127.   }
  128.   else
  129.   {
  130.     //return CONST_PI - asin(ratio);
  131.     return CONST_PI - PID_ATune_fastArcTan(ratio / sqrt( 1.0 - pow(ratio, 2)));
  132.   }
  133. }
  134.  
  135. bool PID_ATune_Runtime(void)
  136. {
  137.   // check ready for new input
  138.   unsigned long now = Timer_GetTicks();
  139.   //unsigned long now = millis();
  140.  
  141.   if (state == AUTOTUNER_OFF)
  142.   {
  143.     // initialize working variables the first time around
  144.     peakType = NOT_A_PEAK;
  145.     inputCount = 0;
  146.     peakCount = 0;
  147.     setpoint = *input;
  148.     outputStart = *output;
  149.     lastPeakTime[0] = now;
  150.     workingNoiseBand = noiseBand;
  151.     newWorkingNoiseBand = noiseBand;  
  152.     workingOstep = oStep;
  153.    
  154. #if defined (AUTOTUNE_RELAY_BIAS)
  155.     relayBias = 0.0;
  156.     stepCount = 0;  
  157.     lastStepTime[0] = now;
  158.     sumInputSinceLastStep[0] = 0.0;
  159. #endif    
  160.    
  161.     // move to new state
  162.     if (controlType == AMIGOF_PI)
  163.     {
  164.       state = STEADY_STATE_AT_BASELINE;
  165.     }
  166.     else
  167.     {
  168.       state = RELAY_STEP_UP;
  169.     }
  170.   }
  171.  
  172.   // otherwise check ready for new input
  173.   else if ((now - lastTime) < sampleTime)
  174.   {
  175.     return false;
  176.   }
  177.  
  178.   // get new input
  179.   lastTime = now;
  180.   float refVal = *input;
  181.  
  182. #if defined (AUTOTUNE_RELAY_BIAS)
  183.   // used to calculate relay bias
  184.   sumInputSinceLastStep[0] += refVal;
  185. #endif  
  186.  
  187.   // local flag variable
  188.   bool justChanged = false;
  189.  
  190.   // check input and change relay state if necessary
  191.   if ((state == RELAY_STEP_UP) && (refVal > setpoint + workingNoiseBand))
  192.   {
  193.     state = RELAY_STEP_DOWN;
  194.     justChanged = true;
  195.   }
  196.   else if ((state == RELAY_STEP_DOWN) && (refVal < setpoint - workingNoiseBand))
  197.   {
  198.     state = RELAY_STEP_UP;
  199.     justChanged = true;
  200.   }
  201.   if (justChanged)
  202.   {
  203.     workingNoiseBand = newWorkingNoiseBand;
  204.    
  205. #if defined (AUTOTUNE_RELAY_BIAS)
  206.     // check symmetry of oscillation
  207.     // and introduce relay bias if necessary
  208.     if (stepCount > 4)
  209.     {
  210.       float avgStep1 = 0.5 * (float) ((lastStepTime[0] - lastStepTime[1]) + (lastStepTime[2] - lastStepTime[3]));
  211.       float avgStep2 = 0.5 * (float) ((lastStepTime[1] - lastStepTime[2]) + (lastStepTime[3] - lastStepTime[4]));
  212.       if ((avgStep1 > 1e-10) && (avgStep2 > 1e-10))
  213.       {
  214.         float asymmetry = (avgStep1 > avgStep2) ?
  215.                            (avgStep1 - avgStep2) / avgStep1 : (avgStep2 - avgStep1) / avgStep2;
  216.                            
  217. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  218.         Serial.print(F("asymmetry "));
  219.         Serial.println(asymmetry);
  220. #endif
  221.  
  222.         if (asymmetry > AUTOTUNE_STEP_ASYMMETRY_TOLERANCE)
  223.         {
  224.           // relay steps are asymmetric
  225.           // calculate relay bias using
  226.           // "Autotuning of PID Controllers: A Relay Feedback Approach",
  227.           //  by Cheng-Ching Yu, 2nd Edition, equation 7.39, p. 148
  228.  
  229.           // calculate change in relay bias
  230.           float deltaRelayBias = - PID_ATune_processValueOffset(avgStep1, avgStep2) * workingOstep;
  231.           if (state == RELAY_STEP_DOWN)
  232.           {
  233.             deltaRelayBias = -deltaRelayBias;
  234.           }
  235.          
  236.           if (abs(deltaRelayBias) > workingOstep * AUTOTUNE_STEP_ASYMMETRY_TOLERANCE)
  237.           {
  238.             // change is large enough to bother with
  239.             relayBias += deltaRelayBias;
  240.  
  241.             /*
  242.             // adjust step height with respect to output limits
  243.             // commented out because the auto tuner does not
  244.             // necessarily know what the output limits are
  245.             float relayHigh = outputStart + workingOstep + relayBias;
  246.             float relayLow  = outputStart - workingOstep + relayBias;
  247.             if (relayHigh > outMax)
  248.             {
  249.               relayHigh = outMax;
  250.             }
  251.             if (relayLow  < outMin)
  252.             {
  253.               relayHigh = outMin;
  254.             }
  255.             workingOstep = 0.5 * (relayHigh - relayLow);
  256.             relayBias = relayHigh - outputStart - workingOstep;
  257.             */
  258.  
  259. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  260.             Serial.print(F("deltaRelayBias "));
  261.             Serial.println(deltaRelayBias);
  262.             Serial.print(F("relayBias "));
  263.             Serial.println(relayBias);
  264. #endif
  265.  
  266.             // reset relay step counter
  267.             // to give the process value oscillation
  268.             // time to settle with the new relay bias value
  269.             stepCount = 0;
  270.           }
  271.         }
  272.       }
  273.     }
  274.  
  275.     // shift step time and integrated process value arrays
  276.     for (uint8_t i = (stepCount > 4 ? 4 : stepCount); i > 0; i--)
  277.     {
  278.       lastStepTime[i] = lastStepTime[i - 1];
  279.       sumInputSinceLastStep[i] = sumInputSinceLastStep[i - 1];
  280.     }
  281.     stepCount++;
  282.     lastStepTime[0] = now;
  283.     sumInputSinceLastStep[0] = 0.0;
  284.    
  285. #if defined (AUTOTUNE_DEBUG
  286.     for (uint8_t i = 1; i < (stepCount > 4 ? 5 : stepCount); i++)
  287.     {
  288.       Serial.print(F("step time "));
  289.       Serial.println(lastStepTime[i]);
  290.       Serial.print(F("step sum "));
  291.       Serial.println(sumInputSinceLastStep[i]);
  292.     }
  293. #endif
  294.  
  295. #endif // if defined AUTOTUNE_RELAY_BIAS
  296.  
  297.   } // if justChanged
  298.  
  299.   // set output
  300.   // FIXME need to respect output limits
  301.   // not knowing output limits is one reason
  302.   // to pass entire PID object to autotune method(s)
  303.   if (((uint8_t) state & (STEADY_STATE_AFTER_STEP_UP | RELAY_STEP_UP)) > 0)
  304.   {
  305.    
  306. #if defined (AUTOTUNE_RELAY_BIAS)    
  307.     *output = outputStart + workingOstep + relayBias;
  308. #else    
  309.     *output = outputStart + workingOstep;
  310. #endif    
  311.  
  312.   }
  313.   else if (state == RELAY_STEP_DOWN)
  314.   {
  315.    
  316. #if defined (AUTOTUNE_RELAY_BIAS)    
  317.     *output = outputStart - workingOstep + relayBias;
  318. #else
  319.     *output = outputStart - workingOstep;
  320. #endif
  321.  
  322.   }
  323.  
  324. #if defined (AUTOTUNE_DEBUG)
  325.   Serial.print(F("refVal "));
  326.   Serial.println(refVal);
  327.   Serial.print(F("setpoint "));
  328.   Serial.println(setpoint);
  329.   Serial.print(F("output "));
  330.   Serial.println(*output);
  331.   Serial.print(F("state "));
  332.   Serial.println(state);
  333. #endif
  334.  
  335.   // store initial inputs
  336.   // we don't want to trust the maxes or mins
  337.   // until the input array is full
  338.   inputCount++;
  339.   if (inputCount <= nLookBack)
  340.   {
  341.     lastInputs[nLookBack - inputCount] = refVal;
  342.     return false;
  343.   }
  344.  
  345.   // shift array of process values and identify peaks
  346.   inputCount = nLookBack;
  347.   bool isMax = true;
  348.   bool isMin = true;
  349.   for (int i = inputCount - 1; i >= 0; i--)
  350.   {
  351.     float val = lastInputs[i];
  352.     if (isMax)
  353.     {
  354.       isMax = (refVal >= val);
  355.     }
  356.     if (isMin)
  357.     {
  358.       isMin = (refVal <= val);
  359.     }
  360.     lastInputs[i + 1] = val;
  361.   }
  362.   lastInputs[0] = refVal;
  363.  
  364.   // for AMIGOf tuning rule, perform an initial
  365.   // step change to calculate process gain K_process
  366.   // this may be very slow for lag-dominated processes
  367.   // and may never terminate for integrating processes
  368.   if (((uint8_t) state & (STEADY_STATE_AT_BASELINE | STEADY_STATE_AFTER_STEP_UP)) > 0)
  369.   {
  370.     // check that all the recent inputs are
  371.     // equal give or take expected noise
  372.     float iMax = lastInputs[0];
  373.     float iMin = lastInputs[0];
  374.     float avgInput = 0.0;
  375.     for (uint8_t i = 0; i <= inputCount; i++)
  376.     {
  377.       float val = lastInputs[i];
  378.       if (iMax < val)
  379.       {
  380.         iMax = val;
  381.       }
  382.         if (iMin > val)
  383.       {
  384.         iMin = val;
  385.       }
  386.       avgInput += val;
  387.     }
  388.     avgInput /= (float)(inputCount + 1);
  389.    
  390. #if defined (AUTOTUNE_DEBUG)
  391.   Serial.print(F("iMax "));
  392.   Serial.println(iMax);
  393.   Serial.print(F("iMin "));
  394.   Serial.println(iMin);
  395.   Serial.print(F("avgInput "));
  396.   Serial.println(avgInput);
  397.   Serial.print(F("stable "));
  398.   Serial.println((iMax - iMin) <= 2.0 * workingNoiseBand);
  399. #endif
  400.  
  401.     // if recent inputs are stable
  402.     if ((iMax - iMin) <= 2.0 * workingNoiseBand)
  403.     {
  404.      
  405. #if defined (AUTOTUNE_RELAY_BIAS)      
  406.       lastStepTime[0] = now;
  407. #endif
  408.  
  409.       if (state == STEADY_STATE_AT_BASELINE)
  410.       {
  411.         state = STEADY_STATE_AFTER_STEP_UP;
  412.         lastPeaks[0] = avgInput;  
  413.         inputCount = 0;
  414.         return false;
  415.       }
  416.       // else state == STEADY_STATE_AFTER_STEP_UP
  417.       // calculate process gain
  418.       K_process = (avgInput - lastPeaks[0]) / workingOstep;
  419.  
  420. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  421.       Serial.print(F("Process gain "));
  422.       Serial.println(K_process);
  423. #endif
  424.  
  425.       // bad estimate of process gain
  426.       if (K_process < 1e-10) // zero
  427.       {
  428.         state = AUTOTUNER_OFF;
  429.         return false;
  430.       }
  431.       state = RELAY_STEP_DOWN;
  432.  
  433. #if defined (AUTOTUNE_RELAY_BIAS)      
  434.       sumInputSinceLastStep[0] = 0.0;
  435. #endif
  436.  
  437.       return false;
  438.     }
  439.     else
  440.     {
  441.       return false;
  442.     }
  443.   }
  444.  
  445.   // increment peak count
  446.   // and record peak time
  447.   // for both maxima and minima
  448.   justChanged = false;
  449.   if (isMax)
  450.   {
  451.     if (peakType == MINIMUM)
  452.     {
  453.       justChanged = true;
  454.     }
  455.     peakType = MAXIMUM;
  456.   }
  457.   else if (isMin)
  458.   {
  459.     if (peakType == MAXIMUM)
  460.     {
  461.       justChanged = true;
  462.     }
  463.     peakType = MINIMUM;
  464.   }
  465.  
  466.   // update peak times and values
  467.   if (justChanged)
  468.   {
  469.     peakCount++;
  470.  
  471. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  472.     Serial.println(F("peakCount "));
  473.     Serial.println(peakCount);
  474.     Serial.println(F("peaks"));
  475.     for (uint8_t i = 0; i < (peakCount > 4 ? 5 : peakCount); i++)
  476.     {
  477.       Serial.println(lastPeaks[i]);
  478.     }
  479. #endif
  480.  
  481.     // shift peak time and peak value arrays
  482.     for (uint8_t i = (peakCount > 4 ? 4 : peakCount); i > 0; i--)
  483.     {
  484.       lastPeakTime[i] = lastPeakTime[i - 1];
  485.       lastPeaks[i] = lastPeaks[i - 1];
  486.     }
  487.   }
  488.   if (isMax || isMin)
  489.   {
  490.     lastPeakTime[0] = now;
  491.     lastPeaks[0] = refVal;
  492.  
  493. #if defined (AUTOTUNE_DEBUG)
  494.     Serial.println();
  495.     Serial.println(F("peakCount "));
  496.     Serial.println(peakCount);
  497.     Serial.println(F("refVal "));
  498.     Serial.println(refVal);
  499.     Serial.print(F("peak type "));
  500.     Serial.println(peakType);
  501.     Serial.print(F("isMin "));
  502.     Serial.println(isMin);
  503.     Serial.print(F("isMax "));
  504.     Serial.println(isMax);
  505.     Serial.println();
  506.     Serial.println(F("lastInputs:"));
  507.     for (uint8_t i = 0; i <= inputCount; i++)
  508.     {
  509.       Serial.println(lastInputs[i]);
  510.     }
  511.     Serial.println();
  512. #endif
  513.  
  514.   }
  515.  
  516.   // check for convergence of induced oscillation
  517.   // convergence of amplitude assessed on last 4 peaks (1.5 cycles)
  518.   float inducedAmplitude = 0.0;
  519.   float phaseLag;
  520.   if (
  521.  
  522. #if defined (AUTOTUNE_RELAY_BIAS)  
  523.     (stepCount > 4) &&
  524. #endif
  525.  
  526.     justChanged &&
  527.     (peakCount > 4)
  528.   )
  529.   {
  530.     float absMax = lastPeaks[1];
  531.     float absMin = lastPeaks[1];
  532.     for (uint8_t i = 2; i <= 4; i++)
  533.     {
  534.       float val = lastPeaks[i];
  535.       inducedAmplitude += fabs( val - lastPeaks[i - 1]);
  536.       if (absMax < val)
  537.       {
  538.          absMax = val;
  539.       }
  540.       if (absMin > val)
  541.       {
  542.          absMin = val;
  543.       }
  544.     }
  545.     inducedAmplitude /= 6.0;
  546.  
  547. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  548.     Serial.print(F("amplitude "));
  549.     Serial.println(inducedAmplitude);
  550.     Serial.print(F("absMin "));
  551.     Serial.println(absMin);
  552.     Serial.print(F("absMax "));
  553.     Serial.println(absMax);
  554.     Serial.print(F("convergence criterion "));
  555.     Serial.println((0.5 * (absMax - absMin) - inducedAmplitude) / inducedAmplitude);
  556. #endif
  557.  
  558.     // source for AMIGOf PI auto tuning method:
  559.     // "Revisiting the Ziegler-Nichols tuning rules for PI control —
  560.     //  Part II. The frequency response method."
  561.     // T. Hägglund and K. J. Åström
  562.     // Asian Journal of Control, Vol. 6, No. 4, pp. 469-482, December 2004
  563.     // http://www.ajc.org.tw/pages/paper/6.4PD/AC0604-P469-FR0371.pdf
  564.     if (controlType == AMIGOF_PI)
  565.     {
  566.       phaseLag = PID_ATune_calculatePhaseLag(inducedAmplitude);
  567.  
  568. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  569.       Serial.print(F("phase lag "));
  570.       Serial.println(phaseLag / CONST_PI * 180.0);
  571. #endif
  572.  
  573.       // check that phase lag is within acceptable bounds, ideally between 120° and 140°
  574.       // but 115° to 145° will just about do, and might converge quicker
  575.       if (fabs(phaseLag - CONST_PI * 130.0 / 180.0) > (CONST_PI * 15.0 / 180.0))
  576.       {
  577.         // phase lag outside the desired range
  578.         // set noiseBand to new estimate
  579.         // aiming for 135° = 0.75 * pi (radians)
  580.         // sin(135°) = sqrt(2)/2
  581.         // NB noiseBand = 0.5 * hysteresis
  582.         newWorkingNoiseBand = 0.5 * inducedAmplitude * CONST_SQRT2_DIV_2;
  583.  
  584. #if defined (AUTOTUNE_RELAY_BIAS)
  585.         // we could reset relay step counter because we can't rely
  586.         // on constant phase lag for calculating
  587.         // relay bias having changed noiseBand
  588.         // but this would essentially preclude using relay bias
  589.         // with AMIGOf tuning, which is already a compile option
  590.         /*
  591.         stepCount = 0;
  592.         */
  593. #endif        
  594.  
  595. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  596.         Serial.print(F("newWorkingNoiseBand "));
  597.         Serial.println(newWorkingNoiseBand);  
  598. #endif
  599.  
  600.         return false;
  601.       }
  602.     }
  603.  
  604.     // check convergence criterion for amplitude of induced oscillation
  605.     if (((0.5 * (absMax - absMin) - inducedAmplitude) / inducedAmplitude) < AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE)
  606.     {
  607.       state = CONVERGED;
  608.     }
  609.   }
  610.    
  611.   // if the autotune has not already converged
  612.   // terminate after 10 cycles
  613.   // or if too long between peaks
  614.   // or if too long between relay steps
  615.   if (
  616.  
  617. #if defined (AUTOTUNE_RELAY_BIAS)  
  618.     ((now - lastStepTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) ||
  619. #endif
  620.  
  621.     ((now - lastPeakTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) ||
  622.     (peakCount >= 20)
  623.   )
  624.   {
  625.     state = FAILED;
  626.   }
  627.  
  628.   if (((uint8_t) state & (CONVERGED | FAILED)) == 0)
  629.   {
  630.     return false;
  631.   }
  632.  
  633.   // autotune algorithm has terminated
  634.   // reset autotuner variables
  635.   *output = outputStart;
  636.  
  637.   if (state == FAILED)
  638.   {
  639.     // do not calculate gain parameters
  640.    
  641. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  642.     Serial.println("failed");
  643. #endif
  644.  
  645.     return true;
  646.   }
  647.  
  648.   // finish up by calculating tuning parameters
  649.  
  650.   // calculate ultimate gain
  651.   float Ku = 4.0 * workingOstep / (inducedAmplitude * CONST_PI);
  652.  
  653. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  654.   Serial.print(F("ultimate gain "));
  655.   Serial.println(1.0 / Ku);
  656.   Serial.println(Ku);
  657. #endif
  658.  
  659.   // calculate ultimate period in seconds
  660.   float Pu = (float) 0.5 * ((lastPeakTime[1] - lastPeakTime[3]) + (lastPeakTime[2] - lastPeakTime[4])) / 1000.0;  
  661.  
  662. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  663.   Serial.print(F("ultimate period "));
  664.   Serial.println(Pu);
  665. #endif
  666.  
  667.   // calculate gain parameters using tuning rules
  668.   // NB PID generally outperforms PI for lag-dominated processes
  669.    
  670.   // AMIGOf is slow to tune, especially for lag-dominated processes, because it
  671.   // requires an estimate of the process gain which is implemented in this
  672.   // routine by steady state change in process variable after step change in set point
  673.   // It is intended to give robust tunings for both lag- and delay- dominated processes
  674.   if (controlType == AMIGOF_PI)
  675.   {
  676.     // calculate gain ratio
  677.     float kappa_phi = (1.0 / Ku) / K_process;
  678.  
  679. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  680.   Serial.print(F("gain ratio kappa "));
  681.   Serial.println(kappa_phi);
  682. #endif
  683.  
  684.     // calculate phase lag
  685.     phaseLag = PID_ATune_calculatePhaseLag(inducedAmplitude);
  686.  
  687. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  688.   Serial.print(F("phase lag "));
  689.   Serial.println(phaseLag / CONST_PI * 180.0);
  690. #endif
  691.  
  692.     // calculate tunings
  693.     Kp = (( 2.50 - 0.92 * phaseLag) / (1.0 + (10.75 - 4.01 * phaseLag) * kappa_phi)) * Ku;
  694.     Ti = ((-3.05 + 1.72 * phaseLag) / pow(1.0 + (-6.10 + 3.44 * phaseLag) * kappa_phi, 2)) * Pu;
  695.     Td = 0.0;
  696.    
  697.     // converged
  698.     return true;
  699.   }
  700.  
  701.   Kp = Ku / (float) PID_ATune_divisor(KP_DIVISOR);
  702.   Ti = Pu / (float) PID_ATune_divisor(TI_DIVISOR);
  703.  
  704.   Td = PID_ATune_PI_controller() ?
  705.        0.0 : Pu / (float) PID_ATune_divisor(TD_DIVISOR);;
  706. /*
  707.   Kp = Ku / (float) tuningRule[controlType].divisor(KP_DIVISOR);
  708.   Ti = Pu / (float) tuningRule[controlType].divisor(TI_DIVISOR);
  709.   Td = tuningRule[controlType].PI_controller() ?
  710.        0.0 : Pu / (float) tuningRule[controlType].divisor(TD_DIVISOR);
  711. */  
  712.   // converged
  713.   return true;
  714. }
  715.  
  716. #if defined (AUTOTUNE_RELAY_BIAS)
  717. float PID_ATune_processValueOffset(float avgStep1, float avgStep2)
  718. {
  719.   // calculate offset of oscillation in process value
  720.   // as a proportion of the amplitude
  721.   // approximation assumes a trapezoidal oscillation
  722.   // that is stationary over the last 2 relay cycles
  723.   // needs constant phase lag, so recent changes to noiseBand are bad
  724.      
  725.   if (avgStep1 < 1e-10)
  726.   {
  727.     return 1.0;
  728.   }
  729.   if (avgStep2 < 1e-10)
  730.   {
  731.     return -1.0;
  732.   }
  733.   // ratio of step durations
  734.   float r1 = avgStep1 / avgStep2;
  735.  
  736. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  737.   Serial.print(F("r1 "));
  738.   Serial.println(r1);
  739. #endif
  740.  
  741.   float s1 = (sumInputSinceLastStep[1] + sumInputSinceLastStep[3]);
  742.   float s2 = (sumInputSinceLastStep[2] + sumInputSinceLastStep[4]);
  743.   if (s1 < 1e-10)
  744.   {
  745.     return 1.0;
  746.   }
  747.   if (s2 < 1e-10)
  748.   {
  749.     return -1.0;
  750.   }
  751.   // ratio of integrated process values
  752.   float r2 = s1 / s2;
  753.  
  754. #if defined (AUTOTUNE_DEBUG) || defined (USE_SIMULATION)
  755.   Serial.print(F("r2 "));
  756.   Serial.println(r2);
  757. #endif
  758.  
  759.   // estimate process value offset assuming a trapezoidal response curve
  760.   //
  761.   // assume trapezoidal wave with amplitude a, cycle period t, time at minimum/maximum m * t (0 <= m <= 1)
  762.   //
  763.   // with no offset:
  764.   // area under half wave of process value given by
  765.   //   a * m * t/2 + a/2 * (1 - m) * t/2 = a * (1 + m) * t / 4
  766.   //
  767.   // now with offset d * a (-1 <= d <= 1):
  768.   // step time of relay half-cycle given by
  769.   //   m * t/2 + (1 - d) * (1 - m) * t/2 = (1 - d + d * m) * t/2
  770.   //
  771.   // => ratio of step times in cycle given by:
  772.   // (1) r1 = (1 - d + d * m) / (1 + d - d * m)
  773.   //
  774.   // area under offset half wave = a * (1 - d) * m * t/2 + a/2 * (1 - d) * (1 - d) * (1 - m) * t/2
  775.   //                             = a * (1 - d) * (1 - d + m * (1 + d)) * t/4
  776.   //
  777.   // => ratio of area under offset half waves given by:
  778.   // (2) r2 = (1 - d) * (1 - d + m * (1 + d)) / ((1 + d) * (1 + d + m * (1 - d)))
  779.   //
  780.   // want to calculate d as a function of r1, r2; not interested in m
  781.   //
  782.   // rearranging (1) gives:
  783.   // (3) m = 1 - (1 / d) * (1 - r1) / (1 + r1)
  784.   //
  785.   // substitute (3) into (2):
  786.   // r2 = ((1 - d) * (1 - d + 1 + d - (1 + d) / d * (1 - r1) / (1 + r1)) / ((1 + d) * (1 + d + 1 - d - (1 - d) / d * (1 - r1) / (1 + r1)))  
  787.   //
  788.   // after much algebra, we arrive at:
  789.   // (4) (r1 * r2 + 3 * r1 + 3 * r2 + 1) * d^2 - 2 * (1 + r1)(1 - r2) * d + (1 - r1) * (1 - r2) = 0
  790.   //
  791.   // quadratic solution to (4):
  792.   // (5) d = ((1 + r1) * (1 - r2) +/- 2 * sqrt((1 - r2) * (r1^2 - r2))) / (r1 * r2 + 3 * r1 + 3 * r2 + 1)
  793.  
  794.   // estimate offset as proportion of amplitude
  795.   float discriminant = (1.0 - r2) * (pow(r1, 2) - r2);
  796.   if (discriminant < 1e-10)
  797.   {
  798.     // catch negative values
  799.     discriminant = 0.0;
  800.   }
  801.  
  802.   // return estimated process value offset
  803.   return ((1.0 + r1) * (1.0 - r2) + ((r1 > 1.0) ? 1.0 : -1.0) * sqrt(discriminant)) /
  804.          (r1 * r2 + 3.0 * r1 + 3.0 * r2 + 1.0);
  805. }
  806. #endif // if defined AUTOTUNE_RELAY_BIAS
  807.  
  808. float PID_ATune_GetKp(void)
  809. {
  810.   return Kp;
  811. }
  812.  
  813. float PID_ATune_GetKi(void)
  814. {
  815.   return Kp / Ti;
  816. }
  817.  
  818. float PID_ATune_GetKd(void)
  819. {
  820.   return Kp * Td;
  821. }
  822.  
  823. void PID_ATune_SetOutputStep(float Step)
  824. {
  825.   oStep = Step;
  826. }
  827.  
  828. float PID_ATune_GetOutputStep(void)
  829. {
  830.   return oStep;
  831. }
  832.  
  833. void PID_ATune_SetControlType(uint8_t type)
  834. {
  835.   controlType = type;
  836. }
  837.  
  838. uint8_t PID_ATune_GetControlType(void)
  839. {
  840.   return controlType;
  841. }
  842.  
  843. void PID_ATune_SetNoiseBand(float band)
  844. {
  845.   noiseBand = band;
  846. }
  847.  
  848. float PID_ATune_GetNoiseBand(void)
  849. {
  850.   return noiseBand;
  851. }
  852.  
  853. void PID_ATune_SetLookbackSec(uint16_t value)
  854. {
  855.   if (value < 1)
  856.   {
  857.     value = 1;
  858.   }
  859.   if (value < 25)
  860.   {
  861.     nLookBack = value * 4;
  862.     sampleTime = 250;
  863.   }
  864.   else
  865.   {
  866.     nLookBack = 100;
  867.     sampleTime = value * 10;
  868.   }
  869. }
  870.  
  871. int PID_ATune_GetLookbackSec(void)
  872. {
  873.   return nLookBack * sampleTime / 1000.0;
  874. }
  875.