src/modules/tools/temperaturecontrol/PID_Autotuner.cpp

   1 #include "PID_Autotuner.h"
   2 #include "Kernel.h"
   3 #include <cmath>        // std::abs
   4 #include "SlowTicker.h"
   5 #include "Gcode.h"
   6
   7 #define DEBUG_PRINTF s->printf
   8
   9 PID_Autotuner::PID_Autotuner()
  10 {
  11     t = NULL;
  12     s = NULL;
  13     lastInputs = NULL;
  14     peaks = NULL;
  15     tick = false;
  16     tickCnt= 0;
  17 }
  18
  19 void PID_Autotuner::on_module_loaded()
  20 {
  21     tick = false;
  22     THEKERNEL->slow_ticker->attach(20, this, &PID_Autotuner::on_tick );
  23     register_for_event(ON_IDLE);
  24     register_for_event(ON_GCODE_RECEIVED);
  25 }
  26
  27 void PID_Autotuner::begin(TemperatureControl *temp, float target, StreamOutput *stream, int ncycles)
  28 {
  29     noiseBand = 0.5;
  30     oStep = temp->heater_pin.max_pwm(); // use max pwm to cycle temp
  31     nLookBack = 5 * 20; // 5 seconds of lookback
  32     lookBackCnt= 0;
  33     tickCnt= 0;
  34
  35     if (lastInputs != NULL) delete[] lastInputs;
  36     lastInputs = new float[nLookBack+1];
  37     t = temp;
  38     s = stream;
  39
  40     t->heater_pin.set(0);
  41     t->target_temperature = 0.0;
  42
  43     target_temperature = target;
  44     requested_cycles = ncycles;
  45
  46     if (peaks != NULL) delete[] peaks;
  47     peaks = new float[ncycles];
  48
  49     for (int i = 0; i < ncycles; i++) {
  50         peaks[i] = 0.0;
  51     }
  52
  53     peakType = 0;
  54     peakCount = 0;
  55     justchanged = false;
  56
  57     float refVal = t->get_temperature();
  58     absMax = refVal;
  59     absMin = refVal;
  60     output= oStep;
  61     t->heater_pin.pwm(oStep); // turn on to start heating
  62
  63     s->printf("%s: Starting PID Autotune, %d max cycles, M304 aborts\n", t->designator.c_str(), ncycles);
  64 }
  65
  66 void PID_Autotuner::abort()
  67 {
  68     if (!t)
  69         return;
  70
  71     t->target_temperature = 0;
  72     t->heater_pin.set(0);
  73     t = NULL;
  74
  75     if (s)
  76         s->printf("PID Autotune Aborted\n");
  77     s = NULL;
  78
  79     if (peaks != NULL)
  80         delete[] peaks;
  81     peaks = NULL;
  82     if (lastInputs != NULL)
  83         delete[] lastInputs;
  84     lastInputs = NULL;
  85 }
  86
  87 void PID_Autotuner::on_gcode_received(void *argument)
  88 {
  89     Gcode *gcode = static_cast<Gcode *>(argument);
  90
  91     if ((gcode->has_m) && (gcode->m == 304))
  92         abort();
  93 }
  94
  95 uint32_t PID_Autotuner::on_tick(uint32_t dummy)
  96 {
  97     if (t)
  98         tick = true;
  99     tickCnt += 1000/20; // millisecond tick count
 100     return 0;
 101 }
 102
 103 /**
 104  * this autopid is based on https://github.com/br3ttb/Arduino-PID-AutoTune-Library/blob/master/PID_AutoTune_v0/PID_AutoTune_v0.cpp
 105  */
 106 void PID_Autotuner::on_idle(void *)
 107 {
 108     if (!tick)
 109         return;
 110
 111     tick = false;
 112
 113     if (t == NULL)
 114         return;
 115
 116     if(peakCount >= requested_cycles) {
 117         finishUp();
 118         return;
 119     }
 120
 121     float refVal = t->get_temperature();
 122
 123     if (refVal > absMax) absMax = refVal;
 124     if (refVal < absMin) absMin = refVal;
 125
 126     // oscillate the output base on the input's relation to the setpoint
 127     if (refVal > target_temperature + noiseBand){
 128         output= 0;
 129         //t->heater_pin.pwm(output);
 130         t->heater_pin.set(0);
 131     } else if (refVal < target_temperature - noiseBand) {
 132         output= oStep;
 133         t->heater_pin.pwm(output);
 134     }
 135
 136     bool isMax = true, isMin = true;
 137
 138     // id peaks
 139     for (int i = nLookBack - 1; i >= 0; i--) {
 140         float val = lastInputs[i];
 141         if (isMax) isMax = refVal > val;
 142         if (isMin) isMin = refVal < val;
 143         lastInputs[i + 1] = lastInputs[i];
 144     }
 145
 146     lastInputs[0] = refVal;
 147
 148     if (lookBackCnt < nLookBack) {
 149         lookBackCnt++; // count number of times we have filled lastInputs
 150         //we don't want to trust the maxes or mins until the inputs array has been filled
 151         return;
 152     }
 153
 154     if (isMax) {
 155         if (peakType == 0) peakType = 1;
 156         if (peakType == -1) {
 157             peakType = 1;
 158             justchanged = true;
 159             peak2 = peak1;
 160         }
 161         peak1 = tickCnt;
 162         peaks[peakCount] = refVal;
 163
 164     } else if (isMin) {
 165         if (peakType == 0) peakType = -1;
 166         if (peakType == 1) {
 167             peakType = -1;
 168             peakCount++;
 169             justchanged = true;
 170         }
 171
 172         if (peakCount < requested_cycles) peaks[peakCount] = refVal;
 173     }
 174
 175     // we need to ignore the first cycle warming up from room temp
 176
 177     if (justchanged && peakCount > 2) {
 178         if(peakCount == 3) { // reset min to new min
 179             absMin= refVal;
 180         }
 181         //we've transitioned. check if we can autotune based on the last peaks
 182         float avgSeparation = (std::abs(peaks[peakCount - 1] - peaks[peakCount - 2]) + std::abs(peaks[peakCount - 2] - peaks[peakCount - 3])) / 2;
 183         s->printf("Cycle %d: max: %g, min: %g, avg separation: %g\n", peakCount, absMax, absMin, avgSeparation);
 184         if (peakCount > 3 && avgSeparation < 0.05 * (absMax - absMin)) {
 185             DEBUG_PRINTF("Stabilized\n");
 186             finishUp();
 187             return;
 188         }
 189     }
 190
 191     justchanged = false;
 192
 193     if ((tickCnt % 1000) == 0) {
 194         s->printf("%s: %5.1f/%5.1f @%d %d/%d\n", t->designator.c_str(), t->get_temperature(), target_temperature, output, peakCount, requested_cycles);
 195         DEBUG_PRINTF("lookBackCnt= %d, peakCount= %d, absmax= %g, absmin= %g, peak1= %lu, peak2= %lu\n", lookBackCnt, peakCount, absMax, absMin, peak1, peak2);
 196     }
 197 }
 198
 199
 200 void PID_Autotuner::finishUp()
 201 {
 202     //we can generate tuning parameters!
 203     float Ku = 4*(2*oStep)/((absMax-absMin)*3.14159);
 204     float Pu = (float)(peak1-peak2) / 1000;
 205     s->printf("\tKu: %g, Pu: %g\n", Ku, Pu);
 206
 207     float kp = 0.6 * Ku;
 208     float ki = 1.2 * Ku / Pu;
 209     float kd = Ku * Pu * 0.075;
 210
 211     s->printf("\tTrying:\n\tKp: %5.1f\n\tKi: %5.3f\n\tKd: %5.0f\n", kp, ki, kd);
 212
 213     t->setPIDp(kp);
 214     t->setPIDi(ki);
 215     t->setPIDd(kd);
 216
 217     s->printf("PID Autotune Complete! The settings above have been loaded into memory, but not written to your config file.\n");
 218
 219
 220     // and clean up
 221     t->target_temperature = 0;
 222     t->heater_pin.set(0);
 223     t = NULL;
 224     s = NULL;
 225
 226     if (peaks != NULL)
 227         delete[] peaks;
 228     peaks = NULL;
 229
 230     if (lastInputs != NULL)
 231         delete[] lastInputs;
 232     lastInputs = NULL;
 233 }