PID_Autotuner.cpp

00001 #include "PID_Autotuner.h"
00002 #include "Kernel.h"
00003 #include <cmath>        // std::abs
00004 
00005 #define DEBUG_PRINTF s->printf
00006 
00007 PID_Autotuner::PID_Autotuner()
00008 {
00009     t = NULL;
00010     s = NULL;
00011     lastInputs = NULL;
00012     peaks = NULL;
00013     tick = false;
00014     tickCnt= 0;
00015 }
00016 
00017 void PID_Autotuner::on_module_loaded()
00018 {
00019     tick = false;
00020     THEKERNEL->slow_ticker->attach(20, this, &PID_Autotuner::on_tick );
00021     register_for_event(ON_IDLE);
00022     register_for_event(ON_GCODE_RECEIVED);
00023 }
00024 
00025 void PID_Autotuner::begin(TemperatureControl *temp, float target, StreamOutput *stream, int ncycles)
00026 {
00027     noiseBand = 0.5;
00028     oStep = temp->heater_pin.max_pwm(); // use max pwm to cycle temp
00029     nLookBack = 5 * 20; // 5 seconds of lookback
00030     lookBackCnt= 0;
00031     tickCnt= 0;
00032 
00033     if (lastInputs != NULL) delete[] lastInputs;
00034     lastInputs = new float[nLookBack+1];
00035     t = temp;
00036     s = stream;
00037 
00038     t->heater_pin.set(0);
00039     t->target_temperature = 0.0;
00040 
00041     target_temperature = target;
00042     requested_cycles = ncycles;
00043 
00044     if (peaks != NULL) delete[] peaks;
00045     peaks = new float[ncycles];
00046 
00047     for (int i = 0; i < ncycles; i++) {
00048         peaks[i] = 0.0;
00049     }
00050 
00051     peakType = 0;
00052     peakCount = 0;
00053     justchanged = false;
00054 
00055     float refVal = t->get_temperature();
00056     absMax = refVal;
00057     absMin = refVal;
00058     output= oStep;
00059     t->heater_pin.pwm(oStep); // turn on to start heating
00060 
00061     s->printf("%s: Starting PID Autotune, %d max cycles, M304 aborts\n", t->designator.c_str(), ncycles);
00062 }
00063 
00064 void PID_Autotuner::abort()
00065 {
00066     if (!t)
00067         return;
00068 
00069     t->target_temperature = 0;
00070     t->heater_pin.set(0);
00071     t = NULL;
00072 
00073     if (s)
00074         s->printf("PID Autotune Aborted\n");
00075     s = NULL;
00076 
00077     if (peaks != NULL)
00078         delete[] peaks;
00079     peaks = NULL;
00080     if (lastInputs != NULL)
00081         delete[] lastInputs;
00082     lastInputs = NULL;
00083 }
00084 
00085 void PID_Autotuner::on_gcode_received(void *argument)
00086 {
00087     Gcode *gcode = static_cast<Gcode *>(argument);
00088 
00089     if ((gcode->has_m) && (gcode->m == 304))
00090         abort();
00091 }
00092 
00093 uint32_t PID_Autotuner::on_tick(uint32_t dummy)
00094 {
00095     if (t)
00096         tick = true;
00097     tickCnt += 1000/20; // millisecond tick count
00098     return 0;
00099 }
00100 
00101 /**
00102  * this autopid is based on https://github.com/br3ttb/Arduino-PID-AutoTune-Library/blob/master/PID_AutoTune_v0/PID_AutoTune_v0.cpp
00103  */
00104 void PID_Autotuner::on_idle(void *)
00105 {
00106     if (!tick)
00107         return;
00108 
00109     tick = false;
00110 
00111     if (t == NULL)
00112         return;
00113 
00114     if(peakCount >= requested_cycles) {
00115         finishUp();
00116         return;
00117     }
00118 
00119     float refVal = t->get_temperature();
00120 
00121     if (refVal > absMax) absMax = refVal;
00122     if (refVal < absMin) absMin = refVal;
00123 
00124     // oscillate the output base on the input's relation to the setpoint
00125     if (refVal > target_temperature + noiseBand){
00126         output= 0;
00127         //t->heater_pin.pwm(output);
00128         t->heater_pin.set(0);
00129     } else if (refVal < target_temperature - noiseBand) {
00130         output= oStep;
00131         t->heater_pin.pwm(output);
00132     }
00133 
00134     bool isMax = true, isMin = true;
00135 
00136     // id peaks
00137     for (int i = nLookBack - 1; i >= 0; i--) {
00138         float val = lastInputs[i];
00139         if (isMax) isMax = refVal > val;
00140         if (isMin) isMin = refVal < val;
00141         lastInputs[i + 1] = lastInputs[i];
00142     }
00143 
00144     lastInputs[0] = refVal;
00145 
00146     if (lookBackCnt < nLookBack) {
00147         lookBackCnt++; // count number of times we have filled lastInputs
00148         //we don't want to trust the maxes or mins until the inputs array has been filled
00149         return;
00150     }
00151 
00152     if (isMax) {
00153         if (peakType == 0) peakType = 1;
00154         if (peakType == -1) {
00155             peakType = 1;
00156             justchanged = true;
00157             peak2 = peak1;
00158         }
00159         peak1 = tickCnt;
00160         peaks[peakCount] = refVal;
00161 
00162     } else if (isMin) {
00163         if (peakType == 0) peakType = -1;
00164         if (peakType == 1) {
00165             peakType = -1;
00166             peakCount++;
00167             justchanged = true;
00168         }
00169 
00170         if (peakCount < requested_cycles) peaks[peakCount] = refVal;
00171     }
00172 
00173     // we need to ignore the first cycle warming up from room temp
00174 
00175     if (justchanged && peakCount > 2) {
00176         if(peakCount == 3) { // reset min to new min
00177             absMin= refVal;
00178         }
00179         //we've transitioned. check if we can autotune based on the last peaks
00180         float avgSeparation = (std::abs(peaks[peakCount - 1] - peaks[peakCount - 2]) + std::abs(peaks[peakCount - 2] - peaks[peakCount - 3])) / 2;
00181         s->printf("Cycle %d: max: %g, min: %g, avg separation: %g\n", peakCount, absMax, absMin, avgSeparation);
00182         if (peakCount > 3 && avgSeparation < 0.05 * (absMax - absMin)) {
00183             DEBUG_PRINTF("Stabilized\n");
00184             finishUp();
00185             return;
00186         }
00187     }
00188 
00189     justchanged = false;
00190 
00191     if ((tickCnt % 1000) == 0) {
00192         s->printf("%s: %5.1f/%5.1f @%d %d/%d\n", t->designator.c_str(), t->get_temperature(), target_temperature, output, peakCount, requested_cycles);
00193         DEBUG_PRINTF("lookBackCnt= %d, peakCount= %d, absmax= %g, absmin= %g, peak1= %lu, peak2= %lu\n", lookBackCnt, peakCount, absMax, absMin, peak1, peak2);
00194     }
00195 }
00196 
00197 
00198 void PID_Autotuner::finishUp()
00199 {
00200     //we can generate tuning parameters!
00201     float Ku = 4*(2*oStep)/((absMax-absMin)*3.14159);
00202     float Pu = (float)(peak1-peak2) / 1000;
00203     s->printf("\tKu: %g, Pu: %g\n", Ku, Pu);
00204 
00205     float kp = 0.6 * Ku;
00206     float ki = 1.2 * Ku / Pu;
00207     float kd = Ku * Pu * 0.075;
00208 
00209     s->printf("\tTrying:\n\tKp: %5.1f\n\tKi: %5.3f\n\tKd: %5.0f\n", kp, ki, kd);
00210 
00211     t->setPIDp(kp);
00212     t->setPIDi(ki);
00213     t->setPIDd(kd);
00214 
00215     s->printf("PID Autotune Complete! The settings above have been loaded into memory, but not written to your config file.\n");
00216 
00217 
00218     // and clean up
00219     t->target_temperature = 0;
00220     t->heater_pin.set(0);
00221     t = NULL;
00222     s = NULL;
00223 
00224     if (peaks != NULL)
00225         delete[] peaks;
00226     peaks = NULL;
00227 
00228     if (lastInputs != NULL)
00229         delete[] lastInputs;
00230     lastInputs = NULL;
00231 }