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/>.
8 // TODO : THIS FILE IS LAME, MUST BE MADE MUCH BETTER
10 #include "libs/Module.h"
11 #include "libs/Kernel.h"
13 #include "TemperatureControl.h"
14 #include "TemperatureControlPool.h"
16 #include "modules/robot/Conveyor.h"
17 #include "PublicDataRequest.h"
19 #include "PublicData.h"
20 #include "ToolManagerPublicAccess.h"
21 #include "StreamOutputPool.h"
23 #include "checksumm.h"
25 #include "SlowTicker.h"
27 #include "ConfigValue.h"
28 #include "PID_Autotuner.h"
30 // Temp sensor implementations:
31 #include "Thermistor.h"
34 #include "MRI_Hooks.h"
38 #define sensor_checksum CHECKSUM("sensor")
40 #define readings_per_second_checksum CHECKSUM("readings_per_second")
41 #define max_pwm_checksum CHECKSUM("max_pwm")
42 #define pwm_frequency_checksum CHECKSUM("pwm_frequency")
43 #define bang_bang_checksum CHECKSUM("bang_bang")
44 #define hysteresis_checksum CHECKSUM("hysteresis")
45 #define heater_pin_checksum CHECKSUM("heater_pin")
46 #define max_temp_checksum CHECKSUM("max_temp")
48 #define get_m_code_checksum CHECKSUM("get_m_code")
49 #define set_m_code_checksum CHECKSUM("set_m_code")
50 #define set_and_wait_m_code_checksum CHECKSUM("set_and_wait_m_code")
52 #define designator_checksum CHECKSUM("designator")
54 #define p_factor_checksum CHECKSUM("p_factor")
55 #define i_factor_checksum CHECKSUM("i_factor")
56 #define d_factor_checksum CHECKSUM("d_factor")
58 #define i_max_checksum CHECKSUM("i_max")
60 #define preset1_checksum CHECKSUM("preset1")
61 #define preset2_checksum CHECKSUM("preset2")
63 TemperatureControl::TemperatureControl(uint16_t name
, int index
)
68 min_temp_violated
= false;
73 TemperatureControl::~TemperatureControl()
78 void TemperatureControl::on_module_loaded()
81 // We start not desiring any temp
82 this->target_temperature
= UNDEFINED
;
87 // Register for events
88 this->register_for_event(ON_GCODE_RECEIVED
);
89 this->register_for_event(ON_GET_PUBLIC_DATA
);
92 this->register_for_event(ON_GCODE_EXECUTE
);
93 this->register_for_event(ON_SECOND_TICK
);
94 this->register_for_event(ON_MAIN_LOOP
);
95 this->register_for_event(ON_SET_PUBLIC_DATA
);
96 this->register_for_event(ON_HALT
);
100 void TemperatureControl::on_halt(void *arg
)
104 this->heater_pin
.set(0);
105 this->target_temperature
= UNDEFINED
;
108 void TemperatureControl::on_main_loop(void *argument
)
110 if (this->min_temp_violated
) {
111 THEKERNEL
->streams
->printf("Error: MINTEMP triggered. Check your temperature sensors!\n");
112 this->min_temp_violated
= false;
116 // Get configuration from the config file
117 void TemperatureControl::load_config()
121 this->set_m_code
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, set_m_code_checksum
)->by_default(104)->as_number();
122 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();
123 this->get_m_code
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, get_m_code_checksum
)->by_default(105)->as_number();
124 this->readings_per_second
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, readings_per_second_checksum
)->by_default(20)->as_number();
126 this->designator
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, designator_checksum
)->by_default(string("T"))->as_string();
128 // Max temperature we are not allowed to get over
129 this->max_temp
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, max_temp_checksum
)->by_default(1000)->as_number();
132 this->heater_pin
.from_string( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, heater_pin_checksum
)->by_default("nc")->as_string());
133 if(this->heater_pin
.connected()){
134 this->readonly
= false;
135 this->heater_pin
.as_output();
138 this->readonly
= true;
141 // For backward compatibility, default to a thermistor sensor.
142 std::string sensor_type
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, sensor_checksum
)->by_default("thermistor")->as_string();
144 // Instantiate correct sensor (TBD: TempSensor factory?)
146 sensor
= nullptr; // In case we fail to create a new sensor.
147 if(sensor_type
.compare("thermistor") == 0) {
148 sensor
= new Thermistor();
149 } else if(sensor_type
.compare("max31855") == 0) {
150 sensor
= new Max31855();
152 sensor
= new TempSensor(); // A dummy implementation
154 sensor
->UpdateConfig(temperature_control_checksum
, this->name_checksum
);
156 this->preset1
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, preset1_checksum
)->by_default(0)->as_number();
157 this->preset2
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, preset2_checksum
)->by_default(0)->as_number();
160 // sigma-delta output modulation
163 if(!this->readonly
) {
164 // used to enable bang bang control of heater
165 this->use_bangbang
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, bang_bang_checksum
)->by_default(false)->as_bool();
166 this->hysteresis
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, hysteresis_checksum
)->by_default(2)->as_number();
167 this->heater_pin
.max_pwm( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, max_pwm_checksum
)->by_default(255)->as_number() );
168 this->heater_pin
.set(0);
169 set_low_on_debug(heater_pin
.port_number
, heater_pin
.pin
);
170 // activate SD-DAC timer
171 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
);
176 THEKERNEL
->slow_ticker
->attach( this->readings_per_second
, this, &TemperatureControl::thermistor_read_tick
);
177 this->PIDdt
= 1.0 / this->readings_per_second
;
180 setPIDp( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, p_factor_checksum
)->by_default(10 )->as_number() );
181 setPIDi( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, i_factor_checksum
)->by_default(0.3f
)->as_number() );
182 setPIDd( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, d_factor_checksum
)->by_default(200)->as_number() );
184 if(!this->readonly
) {
185 // set to the same as max_pwm by default
186 this->i_max
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, i_max_checksum
)->by_default(this->heater_pin
.max_pwm())->as_number();
190 this->lastInput
= -1.0;
191 this->last_reading
= 0.0;
194 void TemperatureControl::on_gcode_received(void *argument
)
196 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
199 if( gcode
->m
== this->get_m_code
) {
200 char buf
[32]; // should be big enough for any status
201 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
);
202 gcode
->txt_after_ok
.append(buf
, n
);
203 gcode
->mark_as_taken();
207 // readonly sensors don't handle the rest
208 if(this->readonly
) return;
210 if (gcode
->m
== 301) {
211 gcode
->mark_as_taken();
212 if (gcode
->has_letter('S') && (gcode
->get_value('S') == this->pool_index
)) {
213 if (gcode
->has_letter('P'))
214 setPIDp( gcode
->get_value('P') );
215 if (gcode
->has_letter('I'))
216 setPIDi( gcode
->get_value('I') );
217 if (gcode
->has_letter('D'))
218 setPIDd( gcode
->get_value('D') );
219 if (gcode
->has_letter('X'))
220 this->i_max
= gcode
->get_value('X');
222 //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);
223 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
);
225 } else if (gcode
->m
== 500 || gcode
->m
== 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings
226 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
);
227 gcode
->mark_as_taken();
229 } else if( ( gcode
->m
== this->set_m_code
|| gcode
->m
== this->set_and_wait_m_code
) && gcode
->has_letter('S')) {
230 // this only gets handled if it is not controlle dby the tool manager or is active in the toolmanager
233 // this is safe as old configs as well as single extruder configs the toolmanager will not be running so will return false
234 // this will also ignore anything that the tool manager is not controlling and return false, otherwise it returns the active tool
236 bool ok
= PublicData::get_value( tool_manager_checksum
, is_active_tool_checksum
, this->name_checksum
, &returned_data
);
238 uint16_t active_tool_name
= *static_cast<uint16_t *>(returned_data
);
239 this->active
= (active_tool_name
== this->name_checksum
);
243 // Attach gcodes to the last block for on_gcode_execute
244 THEKERNEL
->conveyor
->append_gcode(gcode
);
246 // 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
247 if (gcode
->m
== this->set_and_wait_m_code
) {
248 // ensure that no subsequent gcodes get executed with our M109 or similar
249 THEKERNEL
->conveyor
->queue_head_block();
256 void TemperatureControl::on_gcode_execute(void *argument
)
258 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
260 if (((gcode
->m
== this->set_m_code
) || (gcode
->m
== this->set_and_wait_m_code
))
261 && gcode
->has_letter('S') && this->active
) {
262 float v
= gcode
->get_value('S');
265 this->target_temperature
= UNDEFINED
;
266 this->heater_pin
.set((this->o
= 0));
268 this->set_desired_temperature(v
);
270 if( gcode
->m
== this->set_and_wait_m_code
&& !this->waiting
) {
271 THEKERNEL
->pauser
->take();
272 this->waiting
= true;
279 void TemperatureControl::on_get_public_data(void *argument
)
281 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
283 if(!pdr
->starts_with(temperature_control_checksum
)) return;
285 if(pdr
->second_element_is(pool_index_checksum
)) {
286 // asking for our instance pointer if we have this pool_index
287 if(pdr
->third_element_is(this->pool_index
)) {
288 static void *return_data
;
290 pdr
->set_data_ptr(&return_data
);
295 }else if(!pdr
->second_element_is(this->name_checksum
)) return;
297 // ok this is targeted at us, so send back the requested data
298 if(pdr
->third_element_is(current_temperature_checksum
)) {
299 this->public_data_return
.current_temperature
= this->get_temperature();
300 this->public_data_return
.target_temperature
= (target_temperature
== UNDEFINED
) ? 0 : this->target_temperature
;
301 this->public_data_return
.pwm
= this->o
;
302 this->public_data_return
.designator
= this->designator
;
303 pdr
->set_data_ptr(&this->public_data_return
);
309 void TemperatureControl::on_set_public_data(void *argument
)
311 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
313 if(!pdr
->starts_with(temperature_control_checksum
)) return;
315 if(!pdr
->second_element_is(this->name_checksum
)) return;
317 // ok this is targeted at us, so set the temp
318 float t
= *static_cast<float *>(pdr
->get_data_ptr());
319 this->set_desired_temperature(t
);
323 void TemperatureControl::set_desired_temperature(float desired_temperature
)
325 // Never go over the configured max temperature
326 if( desired_temperature
> this->max_temp
){
327 desired_temperature
= this->max_temp
;
330 if (desired_temperature
== 1.0)
331 desired_temperature
= preset1
;
332 else if (desired_temperature
== 2.0)
333 desired_temperature
= preset2
;
335 target_temperature
= desired_temperature
;
336 if (desired_temperature
== 0.0)
337 heater_pin
.set((this->o
= 0));
340 float TemperatureControl::get_temperature()
345 uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy
)
347 float temperature
= sensor
->get_temperature();
349 last_reading
= temperature
;
353 if (target_temperature
> 0) {
354 if (isinf(temperature
)) {
355 this->min_temp_violated
= true;
356 target_temperature
= UNDEFINED
;
357 heater_pin
.set((this->o
= 0));
359 pid_process(temperature
);
360 if ((temperature
> target_temperature
) && waiting
) {
361 THEKERNEL
->pauser
->release();
366 heater_pin
.set((this->o
= 0));
368 last_reading
= temperature
;
373 * Based on https://github.com/br3ttb/Arduino-PID-Library
375 void TemperatureControl::pid_process(float temperature
)
378 // bang bang is very simple, if temp is < target - hysteresis turn on full else if temp is > target + hysteresis turn heater off
380 if(temperature
> (target_temperature
+ hysteresis
) && this->o
> 0) {
381 heater_pin
.set(false);
382 this->o
= 0; // for display purposes only
384 } else if(temperature
< (target_temperature
- hysteresis
) && this->o
<= 0) {
385 if(heater_pin
.max_pwm() >= 255) {
387 this->heater_pin
.set(true);
388 this->o
= 255; // for display purposes only
390 // only to whatever max pwm is configured
391 this->heater_pin
.pwm(heater_pin
.max_pwm());
392 this->o
= heater_pin
.max_pwm(); // for display purposes only
398 // regular PID control
399 float error
= target_temperature
- temperature
;
400 this->iTerm
+= (error
* this->i_factor
);
401 if (this->iTerm
> this->i_max
) this->iTerm
= this->i_max
;
402 else if (this->iTerm
< 0.0) this->iTerm
= 0.0;
404 if(this->lastInput
< 0.0) this->lastInput
= temperature
; // set first time
405 float d
= (temperature
- this->lastInput
);
407 // calculate the PID output
408 // TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that
409 this->o
= (this->p_factor
* error
) + this->iTerm
- (this->d_factor
* d
);
411 if (this->o
>= heater_pin
.max_pwm())
412 this->o
= heater_pin
.max_pwm();
413 else if (this->o
< 0)
416 this->heater_pin
.pwm(this->o
);
417 this->lastInput
= temperature
;
420 void TemperatureControl::on_second_tick(void *argument
)
423 THEKERNEL
->streams
->printf("%s:%3.1f /%3.1f @%d\n", designator
.c_str(), get_temperature(), ((target_temperature
== UNDEFINED
) ? 0.0 : target_temperature
), o
);
426 void TemperatureControl::setPIDp(float p
)
431 void TemperatureControl::setPIDi(float i
)
433 this->i_factor
= i
* this->PIDdt
;
436 void TemperatureControl::setPIDd(float d
)
438 this->d_factor
= d
/ this->PIDdt
;