TemperatureControl.cpp

00001 /*
00002       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
00003       Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
00004       Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 
00008 // TODO : THIS FILE IS LAME, MUST BE MADE MUCH BETTER
00009 
00010 #include "libs/Module.h"
00011 #include "libs/Kernel.h"
00012 #include <math.h>
00013 #include "TemperatureControl.h"
00014 #include "TemperatureControlPool.h"
00015 #include "libs/Pin.h"
00016 #include "libs/Median.h"
00017 #include "modules/robot/Conveyor.h"
00018 #include "PublicDataRequest.h"
00019 #include "TemperatureControlPublicAccess.h"
00020 
00021 #include "MRI_Hooks.h"
00022 
00023 #define UNDEFINED -1
00024 
00025 #define thermistor_checksum                CHECKSUM("thermistor")
00026 #define r0_checksum                        CHECKSUM("r0")
00027 #define readings_per_second_checksum       CHECKSUM("readings_per_second")
00028 #define max_pwm_checksum                   CHECKSUM("max_pwm")
00029 #define pwm_frequency_checksum             CHECKSUM("pwm_frequency")
00030 #define t0_checksum                        CHECKSUM("t0")
00031 #define beta_checksum                      CHECKSUM("beta")
00032 #define vadc_checksum                      CHECKSUM("vadc")
00033 #define vcc_checksum                       CHECKSUM("vcc")
00034 #define r1_checksum                        CHECKSUM("r1")
00035 #define r2_checksum                        CHECKSUM("r2")
00036 #define thermistor_pin_checksum            CHECKSUM("thermistor_pin")
00037 #define heater_pin_checksum                CHECKSUM("heater_pin")
00038 
00039 #define get_m_code_checksum                CHECKSUM("get_m_code")
00040 #define set_m_code_checksum                CHECKSUM("set_m_code")
00041 #define set_and_wait_m_code_checksum       CHECKSUM("set_and_wait_m_code")
00042 
00043 #define designator_checksum                CHECKSUM("designator")
00044 
00045 #define p_factor_checksum                  CHECKSUM("p_factor")
00046 #define i_factor_checksum                  CHECKSUM("i_factor")
00047 #define d_factor_checksum                  CHECKSUM("d_factor")
00048 
00049 #define i_max_checksum                     CHECKSUM("i_max")
00050 
00051 #define preset1_checksum                   CHECKSUM("preset1")
00052 #define preset2_checksum                   CHECKSUM("preset2")
00053 
00054 
00055 TemperatureControl::TemperatureControl(uint16_t name) :
00056   name_checksum(name), waiting(false), min_temp_violated(false) {}
00057 
00058 void TemperatureControl::on_module_loaded(){
00059 
00060     // We start not desiring any temp
00061     this->target_temperature = UNDEFINED;
00062 
00063     // Settings
00064     this->on_config_reload(this);
00065 
00066     this->acceleration_factor = 10;
00067 
00068     // Register for events
00069     register_for_event(ON_CONFIG_RELOAD);
00070     this->register_for_event(ON_GCODE_EXECUTE);
00071     this->register_for_event(ON_GCODE_RECEIVED);
00072     this->register_for_event(ON_MAIN_LOOP);
00073     this->register_for_event(ON_SECOND_TICK);
00074     this->register_for_event(ON_GET_PUBLIC_DATA);
00075     this->register_for_event(ON_SET_PUBLIC_DATA);
00076 }
00077 
00078 void TemperatureControl::on_main_loop(void* argument){
00079     if (this->min_temp_violated) {
00080         THEKERNEL->streams->printf("Error: MINTEMP triggered on P%d.%d! check your thermistors!\n", this->thermistor_pin.port_number, this->thermistor_pin.pin);
00081         this->min_temp_violated = false;
00082     }
00083 }
00084 
00085 // Get configuration from the config file
00086 void TemperatureControl::on_config_reload(void* argument){
00087 
00088     // General config
00089     this->set_m_code          = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, set_m_code_checksum)->by_default(104)->as_number();
00090     this->set_and_wait_m_code = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, set_and_wait_m_code_checksum)->by_default(109)->as_number();
00091     this->get_m_code          = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, get_m_code_checksum)->by_default(105)->as_number();
00092     this->readings_per_second = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, readings_per_second_checksum)->by_default(20)->as_number();
00093 
00094     this->designator          = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, designator_checksum)->by_default(string("T"))->as_string();
00095 
00096     // Values are here : http://reprap.org/wiki/Thermistor
00097     this->r0   = 100000;
00098     this->t0   = 25;
00099     this->beta = 4066;
00100     this->r1   = 0;
00101     this->r2   = 4700;
00102 
00103     // Preset values for various common types of thermistors
00104     ConfigValue* thermistor = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, thermistor_checksum);
00105     if(       thermistor->value.compare("EPCOS100K"    ) == 0 ){ // Default
00106     }else if( thermistor->value.compare("RRRF100K"     ) == 0 ){ this->beta = 3960;
00107     }else if( thermistor->value.compare("RRRF10K"      ) == 0 ){ this->beta = 3964; this->r0 = 10000; this->r1 = 680; this->r2 = 1600;
00108     }else if( thermistor->value.compare("Honeywell100K") == 0 ){ this->beta = 3974;
00109     }else if( thermistor->value.compare("Semitec"      ) == 0 ){ this->beta = 4267;
00110     }else if( thermistor->value.compare("HT100K"       ) == 0 ){ this->beta = 3990; }
00111 
00112     // Preset values are overriden by specified values
00113     this->r0 =                  THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, r0_checksum  )->by_default(this->r0  )->as_number();               // Stated resistance eg. 100K
00114     this->t0 =                  THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, t0_checksum  )->by_default(this->t0  )->as_number();               // Temperature at stated resistance, eg. 25C
00115     this->beta =                THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, beta_checksum)->by_default(this->beta)->as_number();               // Thermistor beta rating. See http://reprap.org/bin/view/Main/MeasuringThermistorBeta
00116     this->r1 =                  THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, r1_checksum  )->by_default(this->r1  )->as_number();
00117     this->r2 =                  THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, r2_checksum  )->by_default(this->r2  )->as_number();
00118 
00119     this->preset1 =             THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, preset1_checksum)->by_default(0)->as_number();
00120     this->preset2 =             THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, preset2_checksum)->by_default(0)->as_number();
00121 
00122 
00123     // Thermistor math
00124     j = (1.0 / beta);
00125     k = (1.0 / (t0 + 273.15));
00126 
00127     // sigma-delta output modulation
00128     o = 0;
00129 
00130     // Thermistor pin for ADC readings
00131     this->thermistor_pin.from_string(THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, thermistor_pin_checksum )->required()->as_string());
00132     THEKERNEL->adc->enable_pin(&thermistor_pin);
00133 
00134     // Heater pin
00135     this->heater_pin.from_string(    THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, heater_pin_checksum)->required()->as_string())->as_output();
00136     this->heater_pin.max_pwm(        THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, max_pwm_checksum)->by_default(255)->as_number() );
00137     this->heater_pin.set(0);
00138 
00139     set_low_on_debug(heater_pin.port_number, heater_pin.pin);
00140 
00141     // activate SD-DAC timer
00142     THEKERNEL->slow_ticker->attach( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, pwm_frequency_checksum)->by_default(2000)->as_number() , &heater_pin, &Pwm::on_tick);
00143 
00144     // reading tick
00145     THEKERNEL->slow_ticker->attach( this->readings_per_second, this, &TemperatureControl::thermistor_read_tick );
00146     this->PIDdt= 1.0 / this->readings_per_second;
00147 
00148     // PID
00149     setPIDp( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, p_factor_checksum)->by_default(10 )->as_number() );
00150     setPIDi( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, i_factor_checksum)->by_default(0.3f)->as_number() );
00151     setPIDd( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, d_factor_checksum)->by_default(200)->as_number() );
00152     // set to the same as max_pwm by default
00153     this->i_max = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, i_max_checksum   )->by_default(this->heater_pin.max_pwm())->as_number();
00154     this->iTerm = 0.0;
00155     this->lastInput= -1.0;
00156     this->last_reading = 0.0;
00157 }
00158 
00159 void TemperatureControl::on_gcode_received(void* argument){
00160     Gcode* gcode = static_cast<Gcode*>(argument);
00161     if (gcode->has_m) {
00162         // Get temperature
00163         if( gcode->m == this->get_m_code ){
00164             char buf[32]; // should be big enough for any status
00165             int n= snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature == UNDEFINED)?0.0:target_temperature), this->o);
00166             gcode->txt_after_ok.append(buf, n);
00167             gcode->mark_as_taken();
00168 
00169         } else if (gcode->m == 301) {
00170             gcode->mark_as_taken();
00171             if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index))
00172             {
00173                 if (gcode->has_letter('P'))
00174                     setPIDp( gcode->get_value('P') );
00175                 if (gcode->has_letter('I'))
00176                     setPIDi( gcode->get_value('I') );
00177                 if (gcode->has_letter('D'))
00178                     setPIDd( gcode->get_value('D') );
00179                 if (gcode->has_letter('X'))
00180                     this->i_max    = gcode->get_value('X');
00181             }
00182             //gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g Pv:%g Iv:%g Dv:%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, this->p, this->i, this->d, o);
00183             gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, o);
00184 
00185         } else if (gcode->m == 303) {
00186             if (gcode->has_letter('E') && (gcode->get_value('E') == this->pool_index)) {
00187                 gcode->mark_as_taken();
00188                 float target = 150.0;
00189                 if (gcode->has_letter('S')) {
00190                     target = gcode->get_value('S');
00191                     gcode->stream->printf("Target: %5.1f\n", target);
00192                 }
00193                 int ncycles= 8;
00194                 if (gcode->has_letter('C')) {
00195                     ncycles= gcode->get_value('C');
00196                 }
00197                 gcode->stream->printf("Start PID tune, command is %s\n", gcode->command.c_str());
00198                 this->pool->PIDtuner->begin(this, target, gcode->stream, ncycles);
00199             }
00200 
00201         } else if (gcode->m == 500 || gcode->m == 503){// M500 saves some volatile settings to config override file, M503 just prints the settings
00202             gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f\n", this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt);
00203             gcode->mark_as_taken();
00204 
00205         } else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S') ) {
00206             // Attach gcodes to the last block for on_gcode_execute
00207             THEKERNEL->conveyor->append_gcode(gcode);
00208 
00209             // push an empty block if we have to wait, so the Planner can get things right, and we can prevent subsequent non-move gcodes from executing
00210             if (gcode->m == this->set_and_wait_m_code)
00211                 // ensure that no subsequent gcodes get executed with our M109 or similar
00212                 THEKERNEL->conveyor->queue_head_block();
00213         }
00214     }
00215 }
00216 
00217 void TemperatureControl::on_gcode_execute(void* argument){
00218     Gcode* gcode = static_cast<Gcode*>(argument);
00219     if( gcode->has_m){
00220         if (((gcode->m == this->set_m_code) || (gcode->m == this->set_and_wait_m_code))
00221             && gcode->has_letter('S'))
00222         {
00223             float v = gcode->get_value('S');
00224 
00225             if (v == 0.0)
00226             {
00227                 this->target_temperature = UNDEFINED;
00228                 this->heater_pin.set(0);
00229             }
00230             else
00231             {
00232                 this->set_desired_temperature(v);
00233 
00234                 if( gcode->m == this->set_and_wait_m_code)
00235                 {
00236                     THEKERNEL->pauser->take();
00237                     this->waiting = true;
00238                 }
00239             }
00240         }
00241     }
00242 }
00243 
00244 void TemperatureControl::on_get_public_data(void* argument){
00245     PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
00246 
00247     if(!pdr->starts_with(temperature_control_checksum)) return;
00248 
00249     if(!pdr->second_element_is(this->name_checksum)) return; // will be bed or hotend
00250 
00251     // ok this is targeted at us, so send back the requested data
00252     if(pdr->third_element_is(current_temperature_checksum)) {
00253         // this must be static as it will be accessed long after we have returned
00254         static struct pad_temperature temp_return;
00255         temp_return.current_temperature= this->get_temperature();
00256         temp_return.target_temperature= (target_temperature == UNDEFINED) ? 0 : this->target_temperature;
00257         temp_return.pwm= this->o;
00258 
00259         pdr->set_data_ptr(&temp_return);
00260         pdr->set_taken();
00261     }
00262 }
00263 
00264 void TemperatureControl::on_set_public_data(void* argument){
00265     PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
00266 
00267     if(!pdr->starts_with(temperature_control_checksum)) return;
00268 
00269     if(!pdr->second_element_is(this->name_checksum)) return; // will be bed or hotend
00270 
00271     // ok this is targeted at us, so set the temp
00272     float t= *static_cast<float*>(pdr->get_data_ptr());
00273     this->set_desired_temperature(t);
00274     pdr->set_taken();
00275 }
00276 
00277 void TemperatureControl::set_desired_temperature(float desired_temperature)
00278 {
00279     if (desired_temperature == 1.0)
00280         desired_temperature = preset1;
00281     else if (desired_temperature == 2.0)
00282         desired_temperature = preset2;
00283 
00284     target_temperature = desired_temperature;
00285     if (desired_temperature == 0.0)
00286         heater_pin.set((o = 0));
00287 }
00288 
00289 float TemperatureControl::get_temperature(){
00290     return last_reading;
00291 }
00292 
00293 float TemperatureControl::adc_value_to_temperature(int adc_value)
00294 {
00295     if ((adc_value == 4095) || (adc_value == 0))
00296         return INFINITY;
00297     float r = r2 / ((4095.0 / adc_value) - 1.0);
00298     if (r1 > 0)
00299         r = (r1 * r) / (r1 - r);
00300     return (1.0 / (k + (j * log(r / r0)))) - 273.15;
00301 }
00302 
00303 uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy){
00304     int r = new_thermistor_reading();
00305 
00306     float temperature = adc_value_to_temperature(r);
00307 
00308     if (target_temperature > 0)
00309     {
00310         if ((r <= 1) || (r >= 4094))
00311         {
00312             this->min_temp_violated = true;
00313             target_temperature = UNDEFINED;
00314             heater_pin.set(0);
00315         }
00316         else
00317         {
00318             pid_process(temperature);
00319             if ((temperature > target_temperature) && waiting)
00320             {
00321                 THEKERNEL->pauser->release();
00322                 waiting = false;
00323             }
00324         }
00325     }
00326     else
00327     {
00328         heater_pin.set((o = 0));
00329     }
00330     last_reading = temperature;
00331     return 0;
00332 }
00333 
00334 /**
00335  * Based on https://github.com/br3ttb/Arduino-PID-Library
00336  */
00337 void TemperatureControl::pid_process(float temperature)
00338 {
00339     float error = target_temperature - temperature;
00340 
00341     this->iTerm += (error * this->i_factor);
00342     if (this->iTerm > this->i_max) this->iTerm = this->i_max;
00343     else if (this->iTerm < 0.0) this->iTerm = 0.0;
00344 
00345     if(this->lastInput < 0.0) this->lastInput= temperature; // set first time
00346     float d= (temperature - this->lastInput);
00347 
00348     // calculate the PID output
00349     // TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that
00350     this->o = (this->p_factor*error) + this->iTerm - (this->d_factor*d);
00351 
00352     if (this->o >= heater_pin.max_pwm())
00353         this->o = heater_pin.max_pwm();
00354     else if (this->o < 0)
00355         this->o = 0;
00356 
00357     this->heater_pin.pwm(this->o);
00358     this->lastInput= temperature;
00359 }
00360 
00361 int TemperatureControl::new_thermistor_reading()
00362 {
00363     int last_raw = THEKERNEL->adc->read(&thermistor_pin);
00364     if (queue.size() >= queue.capacity()) {
00365         uint16_t l;
00366         queue.pop_front(l);
00367     }
00368     uint16_t r = last_raw;
00369     queue.push_back(r);
00370     for (int i=0; i<queue.size(); i++)
00371       median_buffer[i] = *queue.get_ref(i);
00372     uint16_t m = median_buffer[quick_median(median_buffer, queue.size())];
00373     return m;
00374 }
00375 
00376 void TemperatureControl::on_second_tick(void* argument)
00377 {
00378     if (waiting)
00379         THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature == UNDEFINED)?0.0:target_temperature), o);
00380 }
00381 
00382 void TemperatureControl::setPIDp(float p) {
00383     this->p_factor= p;
00384 }
00385 
00386 void TemperatureControl::setPIDi(float i) {
00387     this->i_factor= i*this->PIDdt;
00388 }
00389 
00390 void TemperatureControl::setPIDd(float d) {
00391     this->d_factor= d/this->PIDdt;
00392 }