src/modules/tools/temperaturecontrol/TemperatureControl.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   3       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.
   4       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.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 // TODO : THIS FILE IS LAME, MUST BE MADE MUCH BETTER
   9
  10 #include "libs/Module.h"
  11 #include "libs/Kernel.h"
  12 #include <math.h>
  13 #include "TemperatureControl.h"
  14 #include "TemperatureControlPool.h"
  15 #include "libs/Pin.h"
  16 #include "modules/robot/Conveyor.h"
  17 #include "PublicDataRequest.h"
  18
  19 #include "PublicData.h"
  20 #include "ToolManagerPublicAccess.h"
  21 #include "StreamOutputPool.h"
  22 #include "Config.h"
  23 #include "checksumm.h"
  24 #include "Gcode.h"
  25 #include "SlowTicker.h"
  26 #include "Pauser.h"
  27 #include "ConfigValue.h"
  28 #include "PID_Autotuner.h"
  29 #include "SerialMessage.h"
  30 #include "utils.h"
  31
  32 // Temp sensor implementations:
  33 #include "Thermistor.h"
  34 #include "max31855.h"
  35
  36 #include "MRI_Hooks.h"
  37
  38 #define UNDEFINED -1
  39
  40 #define sensor_checksum                    CHECKSUM("sensor")
  41
  42 #define readings_per_second_checksum       CHECKSUM("readings_per_second")
  43 #define max_pwm_checksum                   CHECKSUM("max_pwm")
  44 #define pwm_frequency_checksum             CHECKSUM("pwm_frequency")
  45 #define bang_bang_checksum                 CHECKSUM("bang_bang")
  46 #define hysteresis_checksum                CHECKSUM("hysteresis")
  47 #define heater_pin_checksum                CHECKSUM("heater_pin")
  48 #define max_temp_checksum                  CHECKSUM("max_temp")
  49 #define min_temp_checksum                  CHECKSUM("min_temp")
  50
  51 #define get_m_code_checksum                CHECKSUM("get_m_code")
  52 #define set_m_code_checksum                CHECKSUM("set_m_code")
  53 #define set_and_wait_m_code_checksum       CHECKSUM("set_and_wait_m_code")
  54
  55 #define designator_checksum                CHECKSUM("designator")
  56
  57 #define p_factor_checksum                  CHECKSUM("p_factor")
  58 #define i_factor_checksum                  CHECKSUM("i_factor")
  59 #define d_factor_checksum                  CHECKSUM("d_factor")
  60
  61 #define i_max_checksum                     CHECKSUM("i_max")
  62 #define windup_checksum                    CHECKSUM("windup")
  63
  64 #define preset1_checksum                   CHECKSUM("preset1")
  65 #define preset2_checksum                   CHECKSUM("preset2")
  66
  67 TemperatureControl::TemperatureControl(uint16_t name, int index)
  68 {
  69     name_checksum= name;
  70     pool_index= index;
  71     waiting= false;
  72     temp_violated= false;
  73     sensor= nullptr;
  74     readonly= false;
  75 }
  76
  77 TemperatureControl::~TemperatureControl()
  78 {
  79     delete sensor;
  80 }
  81
  82 void TemperatureControl::on_module_loaded()
  83 {
  84
  85     // We start not desiring any temp
  86     this->target_temperature = UNDEFINED;
  87     this->sensor_settings= false; // set to true if sensor settings have been overriden
  88
  89     // Settings
  90     this->load_config();
  91
  92     // Register for events
  93     this->register_for_event(ON_GCODE_RECEIVED);
  94     this->register_for_event(ON_GET_PUBLIC_DATA);
  95
  96     if(!this->readonly) {
  97         this->register_for_event(ON_SECOND_TICK);
  98         this->register_for_event(ON_MAIN_LOOP);
  99         this->register_for_event(ON_SET_PUBLIC_DATA);
 100         this->register_for_event(ON_HALT);
 101     }
 102 }
 103
 104 void TemperatureControl::on_halt(void *arg)
 105 {
 106     if(arg == nullptr) {
 107         // turn off heater
 108         this->o = 0;
 109         this->heater_pin.set(0);
 110         this->target_temperature = UNDEFINED;
 111     }
 112 }
 113
 114 void TemperatureControl::on_main_loop(void *argument)
 115 {
 116     if (this->temp_violated) {
 117         this->temp_violated = false;
 118         THEKERNEL->streams->printf("Error: MINTEMP or MAXTEMP triggered. Check your temperature sensors!\n");
 119         THEKERNEL->streams->printf("HALT asserted - reset or M999 required\n");
 120         THEKERNEL->call_event(ON_HALT, nullptr);
 121     }
 122 }
 123
 124 // Get configuration from the config file
 125 void TemperatureControl::load_config()
 126 {
 127
 128     // General config
 129     this->set_m_code          = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, set_m_code_checksum)->by_default(104)->as_number();
 130     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();
 131     this->get_m_code          = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, get_m_code_checksum)->by_default(105)->as_number();
 132     this->readings_per_second = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, readings_per_second_checksum)->by_default(20)->as_number();
 133
 134     this->designator          = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, designator_checksum)->by_default(string("T"))->as_string();
 135
 136     // Max and min temperatures we are not allowed to get over (Safety)
 137     this->max_temp = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, max_temp_checksum)->by_default(300)->as_number();
 138     this->min_temp = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, min_temp_checksum)->by_default(0)->as_number();
 139
 140     // Heater pin
 141     this->heater_pin.from_string( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, heater_pin_checksum)->by_default("nc")->as_string());
 142     if(this->heater_pin.connected()){
 143         this->readonly= false;
 144         this->heater_pin.as_output();
 145
 146     } else {
 147         this->readonly= true;
 148     }
 149
 150     // For backward compatibility, default to a thermistor sensor.
 151     std::string sensor_type = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, sensor_checksum)->by_default("thermistor")->as_string();
 152
 153     // Instantiate correct sensor (TBD: TempSensor factory?)
 154     delete sensor;
 155     sensor = nullptr; // In case we fail to create a new sensor.
 156     if(sensor_type.compare("thermistor") == 0) {
 157         sensor = new Thermistor();
 158     } else if(sensor_type.compare("max31855") == 0) {
 159         sensor = new Max31855();
 160     } else {
 161         sensor = new TempSensor(); // A dummy implementation
 162     }
 163     sensor->UpdateConfig(temperature_control_checksum, this->name_checksum);
 164
 165     this->preset1 = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, preset1_checksum)->by_default(0)->as_number();
 166     this->preset2 = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, preset2_checksum)->by_default(0)->as_number();
 167
 168
 169     // sigma-delta output modulation
 170     this->o = 0;
 171
 172     if(!this->readonly) {
 173         // used to enable bang bang control of heater
 174         this->use_bangbang = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, bang_bang_checksum)->by_default(false)->as_bool();
 175         this->hysteresis = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, hysteresis_checksum)->by_default(2)->as_number();
 176         this->windup = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, windup_checksum)->by_default(false)->as_bool();
 177         this->heater_pin.max_pwm( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, max_pwm_checksum)->by_default(255)->as_number() );
 178         this->heater_pin.set(0);
 179         set_low_on_debug(heater_pin.port_number, heater_pin.pin);
 180         // activate SD-DAC timer
 181         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);
 182     }
 183
 184
 185     // reading tick
 186     THEKERNEL->slow_ticker->attach( this->readings_per_second, this, &TemperatureControl::thermistor_read_tick );
 187     this->PIDdt = 1.0 / this->readings_per_second;
 188
 189     // PID
 190     setPIDp( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, p_factor_checksum)->by_default(10 )->as_number() );
 191     setPIDi( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, i_factor_checksum)->by_default(0.3f)->as_number() );
 192     setPIDd( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, d_factor_checksum)->by_default(200)->as_number() );
 193
 194     if(!this->readonly) {
 195         // set to the same as max_pwm by default
 196         this->i_max = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, i_max_checksum   )->by_default(this->heater_pin.max_pwm())->as_number();
 197     }
 198
 199     this->iTerm = 0.0;
 200     this->lastInput = -1.0;
 201     this->last_reading = 0.0;
 202 }
 203
 204 void TemperatureControl::on_gcode_received(void *argument)
 205 {
 206     Gcode *gcode = static_cast<Gcode *>(argument);
 207     if (gcode->has_m) {
 208
 209         if( gcode->m == this->get_m_code ) {
 210             char buf[32]; // should be big enough for any status
 211             int n = snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), this->o);
 212             gcode->txt_after_ok.append(buf, n);
 213             return;
 214         }
 215
 216         if (gcode->m == 305) { // set or get sensor settings
 217             if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) {
 218                 TempSensor::sensor_options_t args= gcode->get_args();
 219                 args.erase('S'); // don't include the S
 220                 if(args.size() > 0) {
 221                     // set the new options
 222                     if(sensor->set_optional(args)) {
 223                         this->sensor_settings= true;
 224                     }else{
 225                         gcode->stream->printf("Unable to properly set sensor settings, make sure you specify all required values\n");
 226                     }
 227                 }else{
 228                     // don't override
 229                     this->sensor_settings= false;
 230                 }
 231
 232             }else if(!gcode->has_letter('S')) {
 233                 gcode->stream->printf("%s(S%d): using %s\n", this->designator.c_str(), this->pool_index, this->readonly?"Readonly" : this->use_bangbang?"Bangbang":"PID");
 234                 sensor->get_raw();
 235                 TempSensor::sensor_options_t options;
 236                 if(sensor->get_optional(options)) {
 237                     for(auto &i : options) {
 238                         // foreach optional value
 239                         gcode->stream->printf("%s(S%d): %c %1.18f\n", this->designator.c_str(), this->pool_index, i.first, i.second);
 240                     }
 241                 }
 242             }
 243
 244             return;
 245         }
 246
 247         // readonly sensors don't handle the rest
 248         if(this->readonly) return;
 249
 250         if (gcode->m == 143) {
 251             if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) {
 252                 if(gcode->has_letter('P')) {
 253                     max_temp= gcode->get_value('P');
 254
 255                 } else {
 256                     gcode->stream->printf("Nothing set NOTE Usage is M143 S0 P300 where <S> is the hotend index and <P> is the maximum temp to set\n");
 257                 }
 258
 259             }else if(gcode->get_num_args() == 0) {
 260                 gcode->stream->printf("Maximum temperature for %s(%d) is %f°C\n", this->designator.c_str(), this->pool_index, max_temp);
 261             }
 262
 263         } else if (gcode->m == 301) {
 264             if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) {
 265                 if (gcode->has_letter('P'))
 266                     setPIDp( gcode->get_value('P') );
 267                 if (gcode->has_letter('I'))
 268                     setPIDi( gcode->get_value('I') );
 269                 if (gcode->has_letter('D'))
 270                     setPIDd( gcode->get_value('D') );
 271                 if (gcode->has_letter('X'))
 272                     this->i_max = gcode->get_value('X');
 273                 if (gcode->has_letter('Y'))
 274                     this->heater_pin.max_pwm(gcode->get_value('Y'));
 275
 276             }else if(!gcode->has_letter('S')) {
 277                 gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g max pwm: %d 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->heater_pin.max_pwm(), o);
 278             }
 279
 280         } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings
 281             gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f X%1.4f Y%d\n", this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, this->heater_pin.max_pwm());
 282
 283             gcode->stream->printf(";Max temperature setting:\nM143 S%d P%1.4f\n", this->pool_index, this->max_temp);
 284
 285             if(this->sensor_settings) {
 286                 // get or save any sensor specific optional values
 287                 TempSensor::sensor_options_t options;
 288                 if(sensor->get_optional(options) && !options.empty()) {
 289                     gcode->stream->printf(";Optional temp sensor specific settings:\nM305 S%d", this->pool_index);
 290                     for(auto &i : options) {
 291                         gcode->stream->printf(" %c%1.18f", i.first, i.second);
 292                     }
 293                     gcode->stream->printf("\n");
 294                 }
 295             }
 296
 297         } else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S')) {
 298             // this only gets handled if it is not controlled by the tool manager or is active in the toolmanager
 299             this->active = true;
 300
 301             // this is safe as old configs as well as single extruder configs the toolmanager will not be running so will return false
 302             // this will also ignore anything that the tool manager is not controlling and return false, otherwise it returns the active tool
 303             void *returned_data;
 304             bool ok = PublicData::get_value( tool_manager_checksum, is_active_tool_checksum, this->name_checksum, &returned_data );
 305             if (ok) {
 306                 uint16_t active_tool_name =  *static_cast<uint16_t *>(returned_data);
 307                 this->active = (active_tool_name == this->name_checksum);
 308             }
 309
 310             if(this->active) {
 311                 // required so temp change happens in order
 312                 THEKERNEL->conveyor->wait_for_empty_queue();
 313
 314                 float v = gcode->get_value('S');
 315
 316                 if (v == 0.0) {
 317                     this->target_temperature = UNDEFINED;
 318                     this->heater_pin.set((this->o = 0));
 319                 } else {
 320                     this->set_desired_temperature(v);
 321                     // wait for temp to be reached, no more gcodes will be fetched until this is complete
 322                     if( gcode->m == this->set_and_wait_m_code) {
 323                         if(isinf(get_temperature()) && isinf(sensor->get_temperature())) {
 324                             THEKERNEL->streams->printf("Temperature reading is unreliable HALT asserted - reset or M999 required\n");
 325                             THEKERNEL->call_event(ON_HALT, nullptr);
 326                             return;
 327                         }
 328
 329                         this->waiting = true; // on_second_tick will announce temps
 330                         while ( get_temperature() < target_temperature ) {
 331                             THEKERNEL->call_event(ON_IDLE, this);
 332                         }
 333                         this->waiting = false;
 334                     }
 335                 }
 336             }
 337         }
 338     }
 339 }
 340
 341 void TemperatureControl::on_get_public_data(void *argument)
 342 {
 343     PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
 344
 345     if(!pdr->starts_with(temperature_control_checksum)) return;
 346
 347     if(pdr->second_element_is(pool_index_checksum)) {
 348         // asking for our instance pointer if we have this pool_index
 349         if(pdr->third_element_is(this->pool_index)) {
 350             static void *return_data;
 351             return_data = this;
 352             pdr->set_data_ptr(&return_data);
 353             pdr->set_taken();
 354         }
 355         return;
 356
 357     }else if(!pdr->second_element_is(this->name_checksum)) return;
 358
 359     // ok this is targeted at us, so send back the requested data
 360     if(pdr->third_element_is(current_temperature_checksum)) {
 361         this->public_data_return.current_temperature = this->get_temperature();
 362         this->public_data_return.target_temperature = (target_temperature <= 0) ? 0 : this->target_temperature;
 363         this->public_data_return.pwm = this->o;
 364         this->public_data_return.designator= this->designator;
 365         pdr->set_data_ptr(&this->public_data_return);
 366         pdr->set_taken();
 367     }
 368
 369 }
 370
 371 void TemperatureControl::on_set_public_data(void *argument)
 372 {
 373     PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
 374
 375     if(!pdr->starts_with(temperature_control_checksum)) return;
 376
 377     if(!pdr->second_element_is(this->name_checksum)) return;
 378
 379     // ok this is targeted at us, so set the temp
 380     // NOTE unlike the M code this will set the temp now not when the queue is empty
 381     float t = *static_cast<float *>(pdr->get_data_ptr());
 382     this->set_desired_temperature(t);
 383     pdr->set_taken();
 384 }
 385
 386 void TemperatureControl::set_desired_temperature(float desired_temperature)
 387 {
 388     // Never go over the configured max temperature
 389     if( desired_temperature > this->max_temp ){
 390         desired_temperature = this->max_temp;
 391     }
 392
 393     if (desired_temperature == 1.0F)
 394         desired_temperature = preset1;
 395     else if (desired_temperature == 2.0F)
 396         desired_temperature = preset2;
 397
 398     float last_target_temperature= target_temperature;
 399     target_temperature = desired_temperature;
 400     if (desired_temperature <= 0.0F){
 401         // turning it off
 402         heater_pin.set((this->o = 0));
 403
 404     }else if(last_target_temperature <= 0.0F) {
 405         // if it was off and we are now turning it on we need to initialize
 406         this->lastInput= last_reading;
 407         // set to whatever the output currently is See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
 408         this->iTerm= this->o;
 409         if (this->iTerm > this->i_max) this->iTerm = this->i_max;
 410         else if (this->iTerm < 0.0) this->iTerm = 0.0;
 411     }
 412 }
 413
 414 float TemperatureControl::get_temperature()
 415 {
 416     return last_reading;
 417 }
 418
 419 uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy)
 420 {
 421     float temperature = sensor->get_temperature();
 422     if(!this->readonly && target_temperature > 2) {
 423         if (isinf(temperature) || temperature < min_temp || temperature > max_temp) {
 424             this->temp_violated = true;
 425             target_temperature = UNDEFINED;
 426             heater_pin.set((this->o = 0));
 427         } else {
 428             pid_process(temperature);
 429         }
 430     }
 431
 432     last_reading = temperature;
 433     return 0;
 434 }
 435
 436 /**
 437  * Based on https://github.com/br3ttb/Arduino-PID-Library
 438  */
 439 void TemperatureControl::pid_process(float temperature)
 440 {
 441     if(use_bangbang) {
 442         // bang bang is very simple, if temp is < target - hysteresis turn on full else if  temp is > target + hysteresis turn heater off
 443         // good for relays
 444         if(temperature > (target_temperature + hysteresis) && this->o > 0) {
 445             heater_pin.set(false);
 446             this->o = 0; // for display purposes only
 447
 448         } else if(temperature < (target_temperature - hysteresis) && this->o <= 0) {
 449             if(heater_pin.max_pwm() >= 255) {
 450                 // turn on full
 451                 this->heater_pin.set(true);
 452                 this->o = 255; // for display purposes only
 453             } else {
 454                 // only to whatever max pwm is configured
 455                 this->heater_pin.pwm(heater_pin.max_pwm());
 456                 this->o = heater_pin.max_pwm(); // for display purposes only
 457             }
 458         }
 459         return;
 460     }
 461
 462     // regular PID control
 463     float error = target_temperature - temperature;
 464
 465     float new_I = this->iTerm + (error * this->i_factor);
 466     if (new_I > this->i_max) new_I = this->i_max;
 467     else if (new_I < 0.0) new_I = 0.0;
 468     if(!this->windup) this->iTerm= new_I;
 469
 470     float d = (temperature - this->lastInput);
 471
 472     // calculate the PID output
 473     // TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that
 474     this->o = (this->p_factor * error) + new_I - (this->d_factor * d);
 475
 476     if (this->o >= heater_pin.max_pwm())
 477         this->o = heater_pin.max_pwm();
 478     else if (this->o < 0)
 479         this->o = 0;
 480     else if(this->windup)
 481         this->iTerm = new_I; // Only update I term when output is not saturated.
 482
 483     this->heater_pin.pwm(this->o);
 484     this->lastInput = temperature;
 485 }
 486
 487 void TemperatureControl::on_second_tick(void *argument)
 488 {
 489     if (waiting)
 490         THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), o);
 491 }
 492
 493 void TemperatureControl::setPIDp(float p)
 494 {
 495     this->p_factor = p;
 496 }
 497
 498 void TemperatureControl::setPIDi(float i)
 499 {
 500     this->i_factor = i * this->PIDdt;
 501 }
 502
 503 void TemperatureControl::setPIDd(float d)
 504 {
 505     this->d_factor = d / this->PIDdt;
 506 }