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                             // check if ON_HALT was called (usually by kill button)
 333                             if(THEKERNEL->is_halted() || this->target_temperature == UNDEFINED) {
 334                                 THEKERNEL->streams->printf("Wait on temperature aborted by kill\n");
 335                                 break;
 336                             }
 337                         }
 338                         this->waiting = false;
 339                     }
 340                 }
 341             }
 342         }
 343     }
 344 }
 345
 346 void TemperatureControl::on_get_public_data(void *argument)
 347 {
 348     PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
 349
 350     if(!pdr->starts_with(temperature_control_checksum)) return;
 351
 352     if(pdr->second_element_is(pool_index_checksum)) {
 353         // asking for our instance pointer if we have this pool_index
 354         if(pdr->third_element_is(this->pool_index)) {
 355             static void *return_data;
 356             return_data = this;
 357             pdr->set_data_ptr(&return_data);
 358             pdr->set_taken();
 359         }
 360
 361     }else if(pdr->second_element_is(poll_controls_checksum)) {
 362         // polling for all temperature controls
 363         // add our data to the list which is passed in via the data_ptr
 364
 365         std::vector<struct pad_temperature> *v= static_cast<std::vector<pad_temperature>*>(pdr->get_data_ptr());
 366
 367         struct pad_temperature t;
 368         // setup data
 369         t.current_temperature = this->get_temperature();
 370         t.target_temperature = (target_temperature <= 0) ? 0 : this->target_temperature;
 371         t.pwm = this->o;
 372         t.designator= this->designator;
 373         t.id= this->name_checksum;
 374         v->push_back(t);
 375         pdr->set_taken();
 376
 377     }else if(pdr->second_element_is(current_temperature_checksum)) {
 378         // if targeted at us
 379         if(pdr->third_element_is(this->name_checksum)) {
 380             // ok this is targeted at us, so set the requ3sted data in the pointer passed into us
 381             struct pad_temperature *t= static_cast<pad_temperature*>(pdr->get_data_ptr());
 382             t->current_temperature = this->get_temperature();
 383             t->target_temperature = (target_temperature <= 0) ? 0 : this->target_temperature;
 384             t->pwm = this->o;
 385             t->designator= this->designator;
 386             t->id= this->name_checksum;
 387             pdr->set_taken();
 388         }
 389     }
 390
 391 }
 392
 393 void TemperatureControl::on_set_public_data(void *argument)
 394 {
 395     PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
 396
 397     if(!pdr->starts_with(temperature_control_checksum)) return;
 398
 399     if(!pdr->second_element_is(this->name_checksum)) return;
 400
 401     // ok this is targeted at us, so set the temp
 402     // NOTE unlike the M code this will set the temp now not when the queue is empty
 403     float t = *static_cast<float *>(pdr->get_data_ptr());
 404     this->set_desired_temperature(t);
 405     pdr->set_taken();
 406 }
 407
 408 void TemperatureControl::set_desired_temperature(float desired_temperature)
 409 {
 410     // Never go over the configured max temperature
 411     if( desired_temperature > this->max_temp ){
 412         desired_temperature = this->max_temp;
 413     }
 414
 415     if (desired_temperature == 1.0F)
 416         desired_temperature = preset1;
 417     else if (desired_temperature == 2.0F)
 418         desired_temperature = preset2;
 419
 420     float last_target_temperature= target_temperature;
 421     target_temperature = desired_temperature;
 422     if (desired_temperature <= 0.0F){
 423         // turning it off
 424         heater_pin.set((this->o = 0));
 425
 426     }else if(last_target_temperature <= 0.0F) {
 427         // if it was off and we are now turning it on we need to initialize
 428         this->lastInput= last_reading;
 429         // set to whatever the output currently is See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
 430         this->iTerm= this->o;
 431         if (this->iTerm > this->i_max) this->iTerm = this->i_max;
 432         else if (this->iTerm < 0.0) this->iTerm = 0.0;
 433     }
 434 }
 435
 436 float TemperatureControl::get_temperature()
 437 {
 438     return last_reading;
 439 }
 440
 441 uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy)
 442 {
 443     float temperature = sensor->get_temperature();
 444     if(!this->readonly && target_temperature > 2) {
 445         if (isinf(temperature) || temperature < min_temp || temperature > max_temp) {
 446             this->temp_violated = true;
 447             target_temperature = UNDEFINED;
 448             heater_pin.set((this->o = 0));
 449         } else {
 450             pid_process(temperature);
 451         }
 452     }
 453
 454     last_reading = temperature;
 455     return 0;
 456 }
 457
 458 /**
 459  * Based on https://github.com/br3ttb/Arduino-PID-Library
 460  */
 461 void TemperatureControl::pid_process(float temperature)
 462 {
 463     if(use_bangbang) {
 464         // bang bang is very simple, if temp is < target - hysteresis turn on full else if  temp is > target + hysteresis turn heater off
 465         // good for relays
 466         if(temperature > (target_temperature + hysteresis) && this->o > 0) {
 467             heater_pin.set(false);
 468             this->o = 0; // for display purposes only
 469
 470         } else if(temperature < (target_temperature - hysteresis) && this->o <= 0) {
 471             if(heater_pin.max_pwm() >= 255) {
 472                 // turn on full
 473                 this->heater_pin.set(true);
 474                 this->o = 255; // for display purposes only
 475             } else {
 476                 // only to whatever max pwm is configured
 477                 this->heater_pin.pwm(heater_pin.max_pwm());
 478                 this->o = heater_pin.max_pwm(); // for display purposes only
 479             }
 480         }
 481         return;
 482     }
 483
 484     // regular PID control
 485     float error = target_temperature - temperature;
 486
 487     float new_I = this->iTerm + (error * this->i_factor);
 488     if (new_I > this->i_max) new_I = this->i_max;
 489     else if (new_I < 0.0) new_I = 0.0;
 490     if(!this->windup) this->iTerm= new_I;
 491
 492     float d = (temperature - this->lastInput);
 493
 494     // calculate the PID output
 495     // TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that
 496     this->o = (this->p_factor * error) + new_I - (this->d_factor * d);
 497
 498     if (this->o >= heater_pin.max_pwm())
 499         this->o = heater_pin.max_pwm();
 500     else if (this->o < 0)
 501         this->o = 0;
 502     else if(this->windup)
 503         this->iTerm = new_I; // Only update I term when output is not saturated.
 504
 505     this->heater_pin.pwm(this->o);
 506     this->lastInput = temperature;
 507 }
 508
 509 void TemperatureControl::on_second_tick(void *argument)
 510 {
 511     if (waiting)
 512         THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), o);
 513 }
 514
 515 void TemperatureControl::setPIDp(float p)
 516 {
 517     this->p_factor = p;
 518 }
 519
 520 void TemperatureControl::setPIDi(float i)
 521 {
 522     this->i_factor = i * this->PIDdt;
 523 }
 524
 525 void TemperatureControl::setPIDd(float d)
 526 {
 527     this->d_factor = d / this->PIDdt;
 528 }