src/modules/tools/temperaturecontrol/PID_Autotuner.cpp

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