src/modules/tools/temperaturecontrol/PID_Autotuner.cpp

   1 #include "PID_Autotuner.h"
   2
   3 #include "Kernel.h"
   4
   5 PID_Autotuner::PID_Autotuner()
   6 {
   7     t = NULL;
   8     s = NULL;
   9 }
  10
  11 void PID_Autotuner::on_module_loaded()
  12 {
  13     this->kernel->slow_ticker->attach(20, this, &PID_Autotuner::on_tick );
  14 }
  15
  16 void PID_Autotuner::begin(TemperatureControl *temp, double target, StreamOutput *stream)
  17 {
  18     if (t)
  19         t->heater_pin->set(0);
  20
  21     t = temp;
  22
  23     t->target_temperature = 0.0;
  24
  25     target_temperature = target;
  26
  27     for (cycle = 0; cycle < 8; cycle++)
  28     {
  29         cycles[cycle].ticks_high = 0;
  30         cycles[cycle].ticks_low  = 0;
  31         cycles[cycle].t_max      = 0.0;
  32         cycles[cycle].t_min      = 1000.0;
  33     }
  34     cycle = 0;
  35
  36     s = stream;
  37
  38     s->printf("%s: Starting PID Autotune\n", t->designator.c_str());
  39
  40     bias = d = t->heater_pin->max_pwm() >> 1;
  41
  42     output = true;
  43     last_output = true;
  44 }
  45
  46 uint32_t PID_Autotuner::on_tick(uint32_t dummy)
  47 {
  48     if (cycle >= PID_AUTOTUNER_CYCLES)
  49         return 0;
  50     if (t == NULL)
  51         return 0;
  52
  53     if (t->last_reading > (target_temperature + 0.25))
  54         output = false;
  55     else if (t->last_reading < (target_temperature - 0.25))
  56         output = true;;
  57
  58     if (last_output == false && output)
  59     {
  60         s->printf("Cycle %d:\n\tMax: %5.1f  Min: %5.1f  high time: %3.1fs  low time: %3.1fs\n", cycle, cycles[cycle].t_max, cycles[cycle].t_min, cycles[cycle].ticks_high / 20.0, cycles[cycle].ticks_low / 20.0);
  61
  62         // this code taken from http://github.com/ErikZalm/Marlin/blob/Marlin_v1/Marlin/temperature.cpp
  63         bias += (d * (cycles[cycle].ticks_high - cycles[cycle].ticks_low) * (1000.0 / 20.0)) / ((cycles[cycle].ticks_high + cycles[cycle].ticks_low) * (1000.0 / 20.0));
  64         bias = confine(bias, 20, t->heater_pin->max_pwm() - 20);
  65         if (bias > (t->heater_pin->max_pwm() / 2))
  66             d = t->heater_pin->max_pwm() - 1 - bias;
  67         else
  68             d = bias;
  69         // end code from Marlin firmware
  70
  71         cycle++;
  72         if (cycle == PID_AUTOTUNER_CYCLES)
  73         {
  74             t->heater_pin->set(0);
  75             t->set_desired_temperature(0.0);
  76             // TODO: finish
  77             double tmax_avg   = 0.0,
  78                    tmin_avg   = 0.0,
  79                    t_high_avg = 0.0,
  80                    t_low_avg  = 0.0;
  81             for (cycle = PID_AUTOTUNER_CYCLES - 3; cycle < PID_AUTOTUNER_CYCLES; cycle++)
  82             {
  83                 tmax_avg   += cycles[cycle].t_max;
  84                 tmin_avg   += cycles[cycle].t_min;
  85                 t_high_avg += cycles[cycle].ticks_high;
  86                 t_low_avg  += cycles[cycle].ticks_low;
  87             }
  88             tmax_avg   /= (PID_AUTOTUNER_CYCLES - 1.0);
  89             tmin_avg   /= (PID_AUTOTUNER_CYCLES - 1.0);
  90             t_high_avg /= (PID_AUTOTUNER_CYCLES - 1.0);
  91             t_low_avg  /= (PID_AUTOTUNER_CYCLES - 1.0);
  92
  93             s->printf("Averages over last %d cycles: Max: %5.1fc  Min: %5.1fc  high samples: %3.0f  low samples: %3.0f\n", 3, tmax_avg, tmin_avg, t_high_avg, t_low_avg);
  94
  95             // this code taken from http://github.com/ErikZalm/Marlin/blob/Marlin_v1/Marlin/temperature.cpp
  96             double ku = (4.0 * d) / (3.141592653589 * (tmax_avg - tmin_avg) / 2.0);
  97             double tu = (t_low_avg + t_high_avg) * (1000.0 / 20.0) / 1000.0;
  98
  99             s->printf("\tku: %g\n\ttu: %g\n", ku, tu);
 100
 101             double kp = 0.6 * ku;
 102             double ki = 2 * kp / tu / 20.0;
 103             double kd = kp * tu / 8.0;
 104
 105             s->printf("\tTrying:\n\tKp: %5.1f\n\tKi: %5.3f\n\tKd: %5.0f\n", kp, ki, kd);
 106             // end code from Marlin Firmware
 107
 108             t->p_factor = kp;
 109             t->i_factor = ki;
 110             t->d_factor = kd;
 111
 112             t = NULL;
 113             s = NULL;
 114
 115             return 0;
 116         }
 117         s->printf("Cycle %d:\n\tbias: %4d d: %4d\n", cycle, bias, d);
 118     }
 119
 120     int ticks;
 121
 122     if (output)
 123     {
 124         ticks = ++cycles[cycle].ticks_high;
 125         t->heater_pin->pwm((t->o = ((bias + d) >> 1)));
 126     }
 127     else
 128     {
 129         ticks = ++cycles[cycle].ticks_low;
 130         t->heater_pin->set((t->o = 0));
 131     }
 132
 133     if ((ticks % 16) == 0)
 134     {
 135         s->printf("%s: %5.1f/%5.1f @%d %d %d/8\n", t->designator.c_str(), t->last_reading, target_temperature, t->o, (output?1:0), cycle);
 136     }
 137
 138     if (t->last_reading > cycles[cycle].t_max)
 139         cycles[cycle].t_max = t->last_reading;
 140
 141     if (t->last_reading < cycles[cycle].t_min)
 142         cycles[cycle].t_min = t->last_reading;
 143
 144     last_output = output;
 145
 146     return 0;
 147 }