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 "libs/Median.h"
17 #include "modules/robot/Conveyor.h"
18 #include "PublicDataRequest.h"
19 #include "TemperatureControlPublicAccess.h"
20 #include "StreamOutputPool.h"
22 #include "checksumm.h"
25 #include "SlowTicker.h"
27 #include "ConfigValue.h"
28 #include "TemperatureControl.h"
29 #include "PID_Autotuner.h"
31 #include "MRI_Hooks.h"
35 #define thermistor_checksum CHECKSUM("thermistor")
36 #define r0_checksum CHECKSUM("r0")
37 #define readings_per_second_checksum CHECKSUM("readings_per_second")
38 #define max_pwm_checksum CHECKSUM("max_pwm")
39 #define pwm_frequency_checksum CHECKSUM("pwm_frequency")
40 #define bang_bang_checksum CHECKSUM("bang_bang")
41 #define hysteresis_checksum CHECKSUM("hysteresis")
42 #define t0_checksum CHECKSUM("t0")
43 #define beta_checksum CHECKSUM("beta")
44 #define vadc_checksum CHECKSUM("vadc")
45 #define vcc_checksum CHECKSUM("vcc")
46 #define r1_checksum CHECKSUM("r1")
47 #define r2_checksum CHECKSUM("r2")
48 #define thermistor_pin_checksum CHECKSUM("thermistor_pin")
49 #define heater_pin_checksum CHECKSUM("heater_pin")
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")
55 #define designator_checksum CHECKSUM("designator")
57 #define p_factor_checksum CHECKSUM("p_factor")
58 #define i_factor_checksum CHECKSUM("i_factor")
59 #define d_factor_checksum CHECKSUM("d_factor")
61 #define i_max_checksum CHECKSUM("i_max")
63 #define preset1_checksum CHECKSUM("preset1")
64 #define preset2_checksum CHECKSUM("preset2")
67 TemperatureControl::TemperatureControl(uint16_t name
) :
68 name_checksum(name
), waiting(false), min_temp_violated(false) {}
70 void TemperatureControl::on_module_loaded(){
72 // We start not desiring any temp
73 this->target_temperature
= UNDEFINED
;
76 this->on_config_reload(this);
78 this->acceleration_factor
= 10;
80 // Register for events
81 register_for_event(ON_CONFIG_RELOAD
);
82 this->register_for_event(ON_GCODE_EXECUTE
);
83 this->register_for_event(ON_GCODE_RECEIVED
);
84 this->register_for_event(ON_MAIN_LOOP
);
85 this->register_for_event(ON_SECOND_TICK
);
86 this->register_for_event(ON_GET_PUBLIC_DATA
);
87 this->register_for_event(ON_SET_PUBLIC_DATA
);
90 void TemperatureControl::on_main_loop(void* argument
){
91 if (this->min_temp_violated
) {
92 THEKERNEL
->streams
->printf("Error: MINTEMP triggered on P%d.%d! check your thermistors!\n", this->thermistor_pin
.port_number
, this->thermistor_pin
.pin
);
93 this->min_temp_violated
= false;
97 // Get configuration from the config file
98 void TemperatureControl::on_config_reload(void* argument
){
101 this->set_m_code
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, set_m_code_checksum
)->by_default(104)->as_number();
102 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();
103 this->get_m_code
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, get_m_code_checksum
)->by_default(105)->as_number();
104 this->readings_per_second
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, readings_per_second_checksum
)->by_default(20)->as_number();
106 this->designator
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, designator_checksum
)->by_default(string("T"))->as_string();
108 // Values are here : http://reprap.org/wiki/Thermistor
115 // Preset values for various common types of thermistors
116 ConfigValue
* thermistor
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, thermistor_checksum
);
117 if( thermistor
->value
.compare("EPCOS100K" ) == 0 ){ // Default
118 }else if( thermistor
->value
.compare("RRRF100K" ) == 0 ){ this->beta
= 3960;
119 }else if( thermistor
->value
.compare("RRRF10K" ) == 0 ){ this->beta
= 3964; this->r0
= 10000; this->r1
= 680; this->r2
= 1600;
120 }else if( thermistor
->value
.compare("Honeywell100K") == 0 ){ this->beta
= 3974;
121 }else if( thermistor
->value
.compare("Semitec" ) == 0 ){ this->beta
= 4267;
122 }else if( thermistor
->value
.compare("HT100K" ) == 0 ){ this->beta
= 3990; }
124 // Preset values are overriden by specified values
125 this->r0
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, r0_checksum
)->by_default(this->r0
)->as_number(); // Stated resistance eg. 100K
126 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
127 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
128 this->r1
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, r1_checksum
)->by_default(this->r1
)->as_number();
129 this->r2
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, r2_checksum
)->by_default(this->r2
)->as_number();
131 this->preset1
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, preset1_checksum
)->by_default(0)->as_number();
132 this->preset2
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, preset2_checksum
)->by_default(0)->as_number();
137 k
= (1.0 / (t0
+ 273.15));
139 // sigma-delta output modulation
142 // Thermistor pin for ADC readings
143 this->thermistor_pin
.from_string(THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, thermistor_pin_checksum
)->required()->as_string());
144 THEKERNEL
->adc
->enable_pin(&thermistor_pin
);
147 this->heater_pin
.from_string( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, heater_pin_checksum
)->required()->as_string())->as_output();
148 this->heater_pin
.max_pwm( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, max_pwm_checksum
)->by_default(255)->as_number() );
150 this->heater_pin
.set(0);
151 this->heater_on
= false;
153 // used to enable bang bang control of heater
154 this->use_bangbang
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, bang_bang_checksum
)->by_default(false)->as_bool();
155 this->hysteresis
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, hysteresis_checksum
)->by_default(2)->as_number();
157 set_low_on_debug(heater_pin
.port_number
, heater_pin
.pin
);
159 // activate SD-DAC timer
160 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
);
163 THEKERNEL
->slow_ticker
->attach( this->readings_per_second
, this, &TemperatureControl::thermistor_read_tick
);
164 this->PIDdt
= 1.0 / this->readings_per_second
;
167 setPIDp( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, p_factor_checksum
)->by_default(10 )->as_number() );
168 setPIDi( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, i_factor_checksum
)->by_default(0.3f
)->as_number() );
169 setPIDd( THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, d_factor_checksum
)->by_default(200)->as_number() );
170 // set to the same as max_pwm by default
171 this->i_max
= THEKERNEL
->config
->value(temperature_control_checksum
, this->name_checksum
, i_max_checksum
)->by_default(this->heater_pin
.max_pwm())->as_number();
173 this->lastInput
= -1.0;
174 this->last_reading
= 0.0;
177 void TemperatureControl::on_gcode_received(void* argument
){
178 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
181 if( gcode
->m
== this->get_m_code
){
182 char buf
[32]; // should be big enough for any status
183 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
);
184 gcode
->txt_after_ok
.append(buf
, n
);
185 gcode
->mark_as_taken();
187 } else if (gcode
->m
== 301) {
188 gcode
->mark_as_taken();
189 if (gcode
->has_letter('S') && (gcode
->get_value('S') == this->pool_index
))
191 if (gcode
->has_letter('P'))
192 setPIDp( gcode
->get_value('P') );
193 if (gcode
->has_letter('I'))
194 setPIDi( gcode
->get_value('I') );
195 if (gcode
->has_letter('D'))
196 setPIDd( gcode
->get_value('D') );
197 if (gcode
->has_letter('X'))
198 this->i_max
= gcode
->get_value('X');
200 //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);
201 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
);
203 } else if (gcode
->m
== 303) {
204 if (gcode
->has_letter('E') && (gcode
->get_value('E') == this->pool_index
)) {
205 gcode
->mark_as_taken();
206 float target
= 150.0;
207 if (gcode
->has_letter('S')) {
208 target
= gcode
->get_value('S');
209 gcode
->stream
->printf("Target: %5.1f\n", target
);
212 if (gcode
->has_letter('C')) {
213 ncycles
= gcode
->get_value('C');
215 gcode
->stream
->printf("Start PID tune, command is %s\n", gcode
->command
.c_str());
216 this->pool
->PIDtuner
->begin(this, target
, gcode
->stream
, ncycles
);
219 } else if (gcode
->m
== 500 || gcode
->m
== 503){// M500 saves some volatile settings to config override file, M503 just prints the settings
220 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
);
221 gcode
->mark_as_taken();
223 } else if( ( gcode
->m
== this->set_m_code
|| gcode
->m
== this->set_and_wait_m_code
) && gcode
->has_letter('S') ) {
224 // Attach gcodes to the last block for on_gcode_execute
225 THEKERNEL
->conveyor
->append_gcode(gcode
);
227 // 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
228 if (gcode
->m
== this->set_and_wait_m_code
)
229 // ensure that no subsequent gcodes get executed with our M109 or similar
230 THEKERNEL
->conveyor
->queue_head_block();
235 void TemperatureControl::on_gcode_execute(void* argument
){
236 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
238 if (((gcode
->m
== this->set_m_code
) || (gcode
->m
== this->set_and_wait_m_code
))
239 && gcode
->has_letter('S'))
241 float v
= gcode
->get_value('S');
245 this->target_temperature
= UNDEFINED
;
246 this->heater_pin
.set(0);
250 this->set_desired_temperature(v
);
252 if( gcode
->m
== this->set_and_wait_m_code
)
254 THEKERNEL
->pauser
->take();
255 this->waiting
= true;
262 void TemperatureControl::on_get_public_data(void* argument
){
263 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
265 if(!pdr
->starts_with(temperature_control_checksum
)) return;
267 if(!pdr
->second_element_is(this->name_checksum
)) return; // will be bed or hotend
269 // ok this is targeted at us, so send back the requested data
270 if(pdr
->third_element_is(current_temperature_checksum
)) {
271 // this must be static as it will be accessed long after we have returned
272 static struct pad_temperature temp_return
;
273 temp_return
.current_temperature
= this->get_temperature();
274 temp_return
.target_temperature
= (target_temperature
== UNDEFINED
) ? 0 : this->target_temperature
;
275 temp_return
.pwm
= this->o
;
277 pdr
->set_data_ptr(&temp_return
);
282 void TemperatureControl::on_set_public_data(void* argument
){
283 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
285 if(!pdr
->starts_with(temperature_control_checksum
)) return;
287 if(!pdr
->second_element_is(this->name_checksum
)) return; // will be bed or hotend
289 // ok this is targeted at us, so set the temp
290 float t
= *static_cast<float*>(pdr
->get_data_ptr());
291 this->set_desired_temperature(t
);
295 void TemperatureControl::set_desired_temperature(float desired_temperature
)
297 if (desired_temperature
== 1.0)
298 desired_temperature
= preset1
;
299 else if (desired_temperature
== 2.0)
300 desired_temperature
= preset2
;
302 target_temperature
= desired_temperature
;
303 if (desired_temperature
== 0.0)
304 heater_pin
.set((o
= 0));
307 float TemperatureControl::get_temperature(){
311 float TemperatureControl::adc_value_to_temperature(int adc_value
)
313 if ((adc_value
== 4095) || (adc_value
== 0))
315 float r
= r2
/ ((4095.0 / adc_value
) - 1.0);
317 r
= (r1
* r
) / (r1
- r
);
318 return (1.0 / (k
+ (j
* log(r
/ r0
)))) - 273.15;
321 uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy
){
322 int r
= new_thermistor_reading();
324 float temperature
= adc_value_to_temperature(r
);
326 if (target_temperature
> 0)
328 if ((r
<= 1) || (r
>= 4094))
330 this->min_temp_violated
= true;
331 target_temperature
= UNDEFINED
;
336 pid_process(temperature
);
337 if ((temperature
> target_temperature
) && waiting
)
339 THEKERNEL
->pauser
->release();
346 heater_pin
.set((o
= 0));
348 last_reading
= temperature
;
353 * Based on https://github.com/br3ttb/Arduino-PID-Library
355 void TemperatureControl::pid_process(float temperature
)
358 // bang bang if very simple, if temp is < target - hysteresis turn on full else if temp is > target + hysteresis turn heater off
360 if(temperature
> target_temperature
+hysteresis
&& heater_on
) {
361 heater_pin
.set(false);
363 this->o
= 0; // for display purposes only
365 }else if(temperature
< target_temperature
-hysteresis
&& !heater_on
) {
366 if(heater_pin
.max_pwm() >= 255) {
368 this->heater_pin
.set(true);
369 this->o
= 255; // for display purposes only
371 // only to whatever max pwm is configured
372 this->heater_pin
.pwm(heater_pin
.max_pwm());
373 this->o
= heater_pin
.max_pwm(); // for display purposes only
380 // regular PID control
381 float error
= target_temperature
- temperature
;
382 this->iTerm
+= (error
* this->i_factor
);
383 if (this->iTerm
> this->i_max
) this->iTerm
= this->i_max
;
384 else if (this->iTerm
< 0.0) this->iTerm
= 0.0;
386 if(this->lastInput
< 0.0) this->lastInput
= temperature
; // set first time
387 float d
= (temperature
- this->lastInput
);
389 // calculate the PID output
390 // TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that
391 this->o
= (this->p_factor
*error
) + this->iTerm
- (this->d_factor
*d
);
393 if (this->o
>= heater_pin
.max_pwm())
394 this->o
= heater_pin
.max_pwm();
395 else if (this->o
< 0)
398 this->heater_pin
.pwm(this->o
);
399 this->lastInput
= temperature
;
402 int TemperatureControl::new_thermistor_reading()
404 int last_raw
= THEKERNEL
->adc
->read(&thermistor_pin
);
405 if (queue
.size() >= queue
.capacity()) {
409 uint16_t r
= last_raw
;
411 for (int i
=0; i
<queue
.size(); i
++)
412 median_buffer
[i
] = *queue
.get_ref(i
);
413 uint16_t m
= median_buffer
[quick_median(median_buffer
, queue
.size())];
417 void TemperatureControl::on_second_tick(void* argument
)
420 THEKERNEL
->streams
->printf("%s:%3.1f /%3.1f @%d\n", designator
.c_str(), get_temperature(), ((target_temperature
== UNDEFINED
)?0.0:target_temperature
), o
);
423 void TemperatureControl::setPIDp(float p
) {
427 void TemperatureControl::setPIDi(float i
) {
428 this->i_factor
= i
*this->PIDdt
;
431 void TemperatureControl::setPIDd(float d
) {
432 this->d_factor
= d
/this->PIDdt
;