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/>.
10 #include "libs/Module.h"
11 #include "libs/Kernel.h"
13 #include "modules/robot/Conveyor.h"
14 #include "modules/robot/Block.h"
15 #include "StepperMotor.h"
16 #include "SlowTicker.h"
18 #include "StepTicker.h"
20 #include "StepperMotor.h"
22 #include "checksumm.h"
23 #include "ConfigValue.h"
25 #include "libs/StreamOutput.h"
26 #include "PublicDataRequest.h"
30 // OLD config names for backwards compatibility, NOTE new configs will not be added here
31 #define extruder_module_enable_checksum CHECKSUM("extruder_module_enable")
32 #define extruder_steps_per_mm_checksum CHECKSUM("extruder_steps_per_mm")
33 #define extruder_filament_diameter_checksum CHECKSUM("extruder_filament_diameter")
34 #define extruder_acceleration_checksum CHECKSUM("extruder_acceleration")
35 #define extruder_step_pin_checksum CHECKSUM("extruder_step_pin")
36 #define extruder_dir_pin_checksum CHECKSUM("extruder_dir_pin")
37 #define extruder_en_pin_checksum CHECKSUM("extruder_en_pin")
38 #define extruder_max_speed_checksum CHECKSUM("extruder_max_speed")
41 #define extruder_checksum CHECKSUM("extruder")
43 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
44 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
45 #define filament_diameter_checksum CHECKSUM("filament_diameter")
46 #define acceleration_checksum CHECKSUM("acceleration")
47 #define step_pin_checksum CHECKSUM("step_pin")
48 #define dir_pin_checksum CHECKSUM("dir_pin")
49 #define en_pin_checksum CHECKSUM("en_pin")
50 #define max_speed_checksum CHECKSUM("max_speed")
51 #define x_offset_checksum CHECKSUM("x_offset")
52 #define y_offset_checksum CHECKSUM("y_offset")
53 #define z_offset_checksum CHECKSUM("z_offset")
55 #define retract_length_checksum CHECKSUM("retract_length")
56 #define retract_feedrate_checksum CHECKSUM("retract_feedrate")
57 #define retract_recover_length_checksum CHECKSUM("retract_recover_length")
58 #define retract_recover_feedrate_checksum CHECKSUM("retract_recover_feedrate")
59 #define retract_zlift_length_checksum CHECKSUM("retract_zlift_length")
60 #define retract_zlift_feedrate_checksum CHECKSUM("retract_zlift_feedrate")
70 #define PI 3.14159265358979F
72 #define max(a,b) (((a) > (b)) ? (a) : (b))
74 /* The extruder module controls a filament extruder for 3D printing: http://en.wikipedia.org/wiki/Fused_deposition_modeling
75 * It can work in two modes : either the head does not move, and the extruder moves the filament at a specified speed ( SOLO mode here )
76 * or the head moves, and the extruder moves plastic at a speed proportional to the movement of the head ( FOLLOW mode here ).
79 Extruder::Extruder( uint16_t config_identifier
, bool single
)
81 this->absolute_mode
= true;
82 this->enabled
= false;
84 this->single_config
= single
;
85 this->identifier
= config_identifier
;
86 this->retracted
= false;
87 this->volumetric_multiplier
= 1.0F
;
88 this->extruder_multiplier
= 1.0F
;
90 memset(this->offset
, 0, sizeof(this->offset
));
93 void Extruder::on_halt(void *arg
)
98 // disable if multi extruder
99 if(!this->single_config
)
100 this->enabled
= false;
101 // stop moving if we are moving as the block queue flush won't do it for us
102 if(this->stepper_motor
->is_moving()) this->stepper_motor
->move(0, 0);
106 void Extruder::on_module_loaded()
110 this->on_config_reload(this);
112 // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
113 this->register_for_event(ON_BLOCK_BEGIN
);
114 this->register_for_event(ON_BLOCK_END
);
115 this->register_for_event(ON_GCODE_RECEIVED
);
116 this->register_for_event(ON_GCODE_EXECUTE
);
117 this->register_for_event(ON_PLAY
);
118 this->register_for_event(ON_PAUSE
);
119 this->register_for_event(ON_HALT
);
120 this->register_for_event(ON_SPEED_CHANGE
);
121 this->register_for_event(ON_GET_PUBLIC_DATA
);
124 this->target_position
= 0;
125 this->current_position
= 0;
126 this->unstepped_distance
= 0;
127 this->current_block
= NULL
;
130 // Update speed every *acceleration_ticks_per_second*
131 // TODO: Make this an independent setting
132 THEKERNEL
->slow_ticker
->attach( THEKERNEL
->stepper
->get_acceleration_ticks_per_second() , this, &Extruder::acceleration_tick
);
134 // Stepper motor object for the extruder
135 this->stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(step_pin
, dir_pin
, en_pin
) );
136 this->stepper_motor
->attach(this, &Extruder::stepper_motor_finished_move
);
140 void Extruder::on_config_reload(void *argument
)
142 if( this->single_config
) {
143 // If this module uses the old "single extruder" configuration style
145 this->steps_per_millimeter
= THEKERNEL
->config
->value(extruder_steps_per_mm_checksum
)->by_default(1)->as_number();
146 this->filament_diameter
= THEKERNEL
->config
->value(extruder_filament_diameter_checksum
)->by_default(0)->as_number();
147 this->acceleration
= THEKERNEL
->config
->value(extruder_acceleration_checksum
)->by_default(1000)->as_number();
148 this->max_speed
= THEKERNEL
->config
->value(extruder_max_speed_checksum
)->by_default(1000)->as_number();
149 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default(1000)->as_number();
151 this->step_pin
.from_string( THEKERNEL
->config
->value(extruder_step_pin_checksum
)->by_default("nc" )->as_string())->as_output();
152 this->dir_pin
.from_string( THEKERNEL
->config
->value(extruder_dir_pin_checksum
)->by_default("nc" )->as_string())->as_output();
153 this->en_pin
.from_string( THEKERNEL
->config
->value(extruder_en_pin_checksum
)->by_default("nc" )->as_string())->as_output();
155 for(int i
= 0; i
< 3; i
++) {
159 this->enabled
= true;
162 // If this module was created with the new multi extruder configuration style
164 this->steps_per_millimeter
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, steps_per_mm_checksum
)->by_default(1)->as_number();
165 this->filament_diameter
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, filament_diameter_checksum
)->by_default(0)->as_number();
166 this->acceleration
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, acceleration_checksum
)->by_default(1000)->as_number();
167 this->max_speed
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, max_speed_checksum
)->by_default(1000)->as_number();
168 this->feed_rate
= THEKERNEL
->config
->value( default_feed_rate_checksum
)->by_default(1000)->as_number();
170 this->step_pin
.from_string( THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, step_pin_checksum
)->by_default("nc" )->as_string())->as_output();
171 this->dir_pin
.from_string( THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, dir_pin_checksum
)->by_default("nc" )->as_string())->as_output();
172 this->en_pin
.from_string( THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, en_pin_checksum
)->by_default("nc" )->as_string())->as_output();
174 this->offset
[X_AXIS
] = THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, x_offset_checksum
)->by_default(0)->as_number();
175 this->offset
[Y_AXIS
] = THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, y_offset_checksum
)->by_default(0)->as_number();
176 this->offset
[Z_AXIS
] = THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, z_offset_checksum
)->by_default(0)->as_number();
180 // these are only supported in the new syntax, no need to be backward compatible as they did not exist before the change
181 this->retract_length
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, retract_length_checksum
)->by_default(3)->as_number();
182 this->retract_feedrate
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, retract_feedrate_checksum
)->by_default(45)->as_number();
183 this->retract_recover_length
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, retract_recover_length_checksum
)->by_default(0)->as_number();
184 this->retract_recover_feedrate
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, retract_recover_feedrate_checksum
)->by_default(8)->as_number();
185 this->retract_zlift_length
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, retract_zlift_length_checksum
)->by_default(0)->as_number();
186 this->retract_zlift_feedrate
= THEKERNEL
->config
->value(extruder_checksum
, this->identifier
, retract_zlift_feedrate_checksum
)->by_default(100*60)->as_number(); // mm/min
188 if(filament_diameter
> 0.01) {
189 this->volumetric_multiplier
= 1.0F
/ (powf(this->filament_diameter
/ 2, 2) * PI
);
193 void Extruder::on_get_public_data(void* argument
){
194 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
196 if(!pdr
->starts_with(extruder_checksum
)) return;
199 // Note this is allowing both step/mm and filament diameter to be exposed via public data
200 pdr
->set_data_ptr(&this->steps_per_millimeter
);
205 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
206 void Extruder::on_pause(void *argument
)
209 this->stepper_motor
->pause();
212 // When the play/pause button is set to play, or a module calls the ON_PLAY event
213 void Extruder::on_play(void *argument
)
215 this->paused
= false;
216 this->stepper_motor
->unpause();
219 void Extruder::on_gcode_received(void *argument
)
221 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
223 // M codes most execute immediately, most only execute if enabled
225 if (gcode
->m
== 114 && this->enabled
) {
227 int n
= snprintf(buf
, sizeof(buf
), " E:%1.3f ", this->current_position
);
228 gcode
->txt_after_ok
.append(buf
, n
);
229 gcode
->mark_as_taken();
231 } else if (gcode
->m
== 92 && ( (this->enabled
&& !gcode
->has_letter('P')) || (gcode
->has_letter('P') && gcode
->get_value('P') == this->identifier
) ) ) {
232 float spm
= this->steps_per_millimeter
;
233 if (gcode
->has_letter('E')) {
234 spm
= gcode
->get_value('E');
235 this->steps_per_millimeter
= spm
;
238 gcode
->stream
->printf("E:%g ", spm
);
239 gcode
->add_nl
= true;
240 gcode
->mark_as_taken();
242 } else if (gcode
->m
== 200 && ( (this->enabled
&& !gcode
->has_letter('P')) || (gcode
->has_letter('P') && gcode
->get_value('P') == this->identifier
)) ) {
243 if (gcode
->has_letter('D')) {
244 THEKERNEL
->conveyor
->wait_for_empty_queue(); // only apply after the queue has emptied
245 this->filament_diameter
= gcode
->get_value('D');
246 if(filament_diameter
> 0.01) {
247 this->volumetric_multiplier
= 1.0F
/ (powf(this->filament_diameter
/ 2, 2) * PI
);
249 this->volumetric_multiplier
= 1.0F
;
252 gcode
->mark_as_taken();
254 } else if (gcode
->m
== 204 && gcode
->has_letter('E') &&
255 ( (this->enabled
&& !gcode
->has_letter('P')) || (gcode
->has_letter('P') && gcode
->get_value('P') == this->identifier
)) ) {
256 // extruder acceleration M204 Ennn mm/sec^2 (Pnnn sets the specific extruder for M500)
257 this->acceleration
= gcode
->get_value('E');
258 gcode
->mark_as_taken();
260 } else if (gcode
->m
== 207 && ( (this->enabled
&& !gcode
->has_letter('P')) || (gcode
->has_letter('P') && gcode
->get_value('P') == this->identifier
)) ) {
261 // M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop] Q[zlift feedrate mm/min]
262 if(gcode
->has_letter('S')) retract_length
= gcode
->get_value('S');
263 if(gcode
->has_letter('F')) retract_feedrate
= gcode
->get_value('F')/60.0F
; // specified in mm/min converted to mm/sec
264 if(gcode
->has_letter('Z')) retract_zlift_length
= gcode
->get_value('Z');
265 if(gcode
->has_letter('Q')) retract_zlift_feedrate
= gcode
->get_value('Q');
266 gcode
->mark_as_taken();
268 } else if (gcode
->m
== 208 && ( (this->enabled
&& !gcode
->has_letter('P')) || (gcode
->has_letter('P') && gcode
->get_value('P') == this->identifier
)) ) {
269 // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
270 if(gcode
->has_letter('S')) retract_recover_length
= gcode
->get_value('S');
271 if(gcode
->has_letter('F')) retract_recover_feedrate
= gcode
->get_value('F')/60.0F
; // specified in mm/min converted to mm/sec
272 gcode
->mark_as_taken();
274 } else if (gcode
->m
== 221 && this->enabled
) { // M221 S100 change flow rate by percentage
275 if(gcode
->has_letter('S')) this->extruder_multiplier
= gcode
->get_value('S')/100.0F
;
276 gcode
->mark_as_taken();
278 } else if (gcode
->m
== 500 || gcode
->m
== 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings
279 if( this->single_config
) {
280 gcode
->stream
->printf(";E Steps per mm:\nM92 E%1.4f\n", this->steps_per_millimeter
);
281 gcode
->stream
->printf(";E Filament diameter:\nM200 D%1.4f\n", this->filament_diameter
);
282 gcode
->stream
->printf(";E retract length, feedrate, zlift length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f\n", this->retract_length
, this->retract_feedrate
*60.0F
, this->retract_zlift_length
, this->retract_zlift_feedrate
);
283 gcode
->stream
->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f\n", this->retract_recover_length
, this->retract_recover_feedrate
*60.0F
);
284 gcode
->stream
->printf(";E acceleration mm/sec^2:\nM204 E%1.4f\n", this->acceleration
);
287 gcode
->stream
->printf(";E Steps per mm:\nM92 E%1.4f P%d\n", this->steps_per_millimeter
, this->identifier
);
288 gcode
->stream
->printf(";E Filament diameter:\nM200 D%1.4f P%d\n", this->filament_diameter
, this->identifier
);
289 gcode
->stream
->printf(";E retract length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f P%d\n", this->retract_length
, this->retract_feedrate
*60.0F
, this->retract_zlift_length
, this->retract_zlift_feedrate
, this->identifier
);
290 gcode
->stream
->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f P%d\n", this->retract_recover_length
, this->retract_recover_feedrate
*60.0F
, this->identifier
);
291 gcode
->stream
->printf(";E acceleration mm/sec^2:\nM204 E%1.4f P%d\n", this->acceleration
, this->identifier
);
293 gcode
->mark_as_taken();
294 } else if( gcode
->m
== 17 || gcode
->m
== 18 || gcode
->m
== 82 || gcode
->m
== 83 || gcode
->m
== 84 ) {
295 // Mcodes to pass along to on_gcode_execute
296 THEKERNEL
->conveyor
->append_gcode(gcode
);
297 gcode
->mark_as_taken();
300 }else if(gcode
->has_g
) {
301 // G codes, NOTE some are ignored if not enabled
302 if( (gcode
->g
== 92 && gcode
->has_letter('E')) || (gcode
->g
== 90 || gcode
->g
== 91) ) {
303 // Gcodes to pass along to on_gcode_execute
304 THEKERNEL
->conveyor
->append_gcode(gcode
);
305 gcode
->mark_as_taken();
307 }else if( this->enabled
&& gcode
->g
< 4 && gcode
->has_letter('E') && !gcode
->has_letter('X') && !gcode
->has_letter('Y') && !gcode
->has_letter('Z') ) {
308 // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
309 THEKERNEL
->conveyor
->append_gcode(gcode
);
310 THEKERNEL
->conveyor
->queue_head_block();
311 gcode
->mark_as_taken();
313 }else if( this->enabled
&& (gcode
->g
== 10 || gcode
->g
== 11) ) { // firmware retract command
314 gcode
->mark_as_taken();
315 // check we are in the correct state of retract or unretract
316 if(gcode
->g
== 10 && !retracted
) {
317 this->retracted
= true;
318 this->cancel_zlift_restore
= false;
319 } else if(gcode
->g
== 11 && retracted
){
320 this->retracted
= false;
322 return; // ignore duplicates
324 // now we do a special hack to add zlift if needed, this should go in Robot but if it did the zlift would be executed before retract which is bad
325 // this way zlift will happen after retract, (or before for unretract) NOTE we call the robot->on_gcode_receive directly to avoid recursion
326 if(retract_zlift_length
> 0 && gcode
->g
== 11 && !this->cancel_zlift_restore
) {
327 // reverse zlift happens before unretract
328 // NOTE we do not do this if cancel_zlift_restore is set to true, which happens if there is an absolute Z move inbetween G10 and G11
330 int n
= snprintf(buf
, sizeof(buf
), "G0 Z%1.4f F%1.4f", -retract_zlift_length
, retract_zlift_feedrate
);
332 Gcode
gc(cmd
, &(StreamOutput::NullStream
));
333 bool oldmode
= THEKERNEL
->robot
->absolute_mode
;
334 THEKERNEL
->robot
->absolute_mode
= false; // needs to be relative mode
335 THEKERNEL
->robot
->on_gcode_received(&gc
); // send to robot directly
336 THEKERNEL
->robot
->absolute_mode
= oldmode
; // restore mode
339 // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
340 THEKERNEL
->conveyor
->append_gcode(gcode
);
341 THEKERNEL
->conveyor
->queue_head_block();
343 if(retract_zlift_length
> 0 && gcode
->g
== 10) {
345 int n
= snprintf(buf
, sizeof(buf
), "G0 Z%1.4f F%1.4f", retract_zlift_length
, retract_zlift_feedrate
);
347 Gcode
gc(cmd
, &(StreamOutput::NullStream
));
348 bool oldmode
= THEKERNEL
->robot
->absolute_mode
;
349 THEKERNEL
->robot
->absolute_mode
= false; // needs to be relative mode
350 THEKERNEL
->robot
->on_gcode_received(&gc
); // send to robot directly
351 THEKERNEL
->robot
->absolute_mode
= oldmode
; // restore mode
354 }else if( this->enabled
&& this->retracted
&& (gcode
->g
== 0 || gcode
->g
== 1) && gcode
->has_letter('Z')) {
355 // NOTE we cancel the zlift restore for the following G11 as we have moved to an absolute Z which we need to stay at
356 this->cancel_zlift_restore
= true;
361 // Compute extrusion speed based on parameters and gcode distance of travel
362 void Extruder::on_gcode_execute(void *argument
)
364 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
366 // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
369 // Absolute/relative mode, globably modal affect all extruders whether enabled or not
379 this->absolute_mode
= true;
382 this->absolute_mode
= false;
390 } else if( gcode
->has_g
&& (gcode
->g
== 90 || gcode
->g
== 91) ) {
391 this->absolute_mode
= (gcode
->g
== 90);
396 if( gcode
->has_g
&& this->enabled
) {
397 // G92: Reset extruder position
398 if( gcode
->g
== 92 ) {
399 if( gcode
->has_letter('E') ) {
400 this->current_position
= gcode
->get_value('E');
401 this->target_position
= this->current_position
;
402 this->unstepped_distance
= 0;
403 } else if( gcode
->get_num_args() == 0) {
404 this->current_position
= 0.0;
405 this->target_position
= this->current_position
;
406 this->unstepped_distance
= 0;
409 } else if (gcode
->g
== 10) {
410 // FW retract command
411 feed_rate
= retract_feedrate
; // mm/sec
413 this->travel_distance
= -retract_length
;
414 this->target_position
+= this->travel_distance
;
417 } else if (gcode
->g
== 11) {
418 // un retract command
419 feed_rate
= retract_recover_feedrate
; // mm/sec
421 this->travel_distance
= (retract_length
+ retract_recover_length
);
422 this->target_position
+= this->travel_distance
;
425 } else if (gcode
->g
== 0 || gcode
->g
== 1) {
426 // Extrusion length from 'G' Gcode
427 if( gcode
->has_letter('E' )) {
428 // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position )
429 float extrusion_distance
= gcode
->get_value('E');
430 float relative_extrusion_distance
= extrusion_distance
;
431 if (this->absolute_mode
) {
432 relative_extrusion_distance
-= this->target_position
;
433 this->target_position
= extrusion_distance
;
435 this->target_position
+= relative_extrusion_distance
;
438 // If the robot is moving, we follow it's movement, otherwise, we move alone
439 if( fabs(gcode
->millimeters_of_travel
) < 0.0001F
) { // With floating numbers, we can have 0 != 0 ... beeeh. For more info see : http://upload.wikimedia.org/wikipedia/commons/0/0a/Cain_Henri_Vidal_Tuileries.jpg
441 this->travel_distance
= relative_extrusion_distance
;
443 // We move proportionally to the robot's movement
445 this->travel_ratio
= (relative_extrusion_distance
* this->volumetric_multiplier
* this->extruder_multiplier
) / gcode
->millimeters_of_travel
; // adjust for volumetric extrusion and extruder multiplier
446 // TODO: check resulting flowrate, limit robot speed if it exceeds max_speed
452 if (gcode
->has_letter('F')) {
453 feed_rate
= gcode
->get_value('F') / THEKERNEL
->robot
->get_seconds_per_minute();
454 if (feed_rate
> max_speed
)
455 feed_rate
= max_speed
;
462 // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing )
463 void Extruder::on_block_begin(void *argument
)
465 if(!this->enabled
) return;
466 Block
*block
= static_cast<Block
*>(argument
);
469 if( this->mode
== SOLO
) {
470 // In solo mode we take the block so we can move even if the stepper has nothing to do
472 this->current_position
+= this->travel_distance
;
474 int steps_to_step
= abs(int(floor(this->steps_per_millimeter
* (this->travel_distance
+ this->unstepped_distance
) )));
476 if ( this->travel_distance
> 0 ) {
477 this->unstepped_distance
+= this->travel_distance
- (steps_to_step
/ this->steps_per_millimeter
); //catch any overflow
479 this->unstepped_distance
+= this->travel_distance
+ (steps_to_step
/ this->steps_per_millimeter
); //catch any overflow
482 if( steps_to_step
!= 0 ) {
484 // We take the block, we have to release it or everything gets stuck
486 this->current_block
= block
;
488 this->stepper_motor
->move( ( this->travel_distance
> 0 ), steps_to_step
, 0);
491 this->current_block
= NULL
;
494 } else if( this->mode
== FOLLOW
) {
495 // In non-solo mode, we just follow the stepper module
496 this->travel_distance
= block
->millimeters
* this->travel_ratio
;
498 this->current_position
+= this->travel_distance
;
500 int steps_to_step
= abs(int(floor(this->steps_per_millimeter
* (this->travel_distance
+ this->unstepped_distance
) )));
502 if ( this->travel_distance
> 0 ) {
503 this->unstepped_distance
+= this->travel_distance
- (steps_to_step
/ this->steps_per_millimeter
); //catch any overflow
505 this->unstepped_distance
+= this->travel_distance
+ (steps_to_step
/ this->steps_per_millimeter
); //catch any overflow
508 if( steps_to_step
!= 0 ) {
510 this->current_block
= block
;
512 this->stepper_motor
->move( ( this->travel_distance
> 0 ), steps_to_step
);
513 on_speed_change(0); // set initial speed
515 this->current_block
= NULL
;
518 } else if( this->mode
== OFF
) {
519 // No movement means we must reset our speed
520 this->current_block
= NULL
;
521 //this->stepper_motor->set_speed(0);
527 // When a block ends, pause the stepping interrupt
528 void Extruder::on_block_end(void *argument
)
530 if(!this->enabled
) return;
531 this->current_block
= NULL
;
534 // Called periodically to change the speed to match acceleration or to match the speed of the robot
535 // Only used in SOLO mode
536 uint32_t Extruder::acceleration_tick(uint32_t dummy
)
538 if(!this->enabled
) return 0;
540 // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
541 if( this->current_block
== NULL
|| this->paused
|| this->mode
!= SOLO
) {
545 if(!this->stepper_motor
->is_moving()) return 0;
547 uint32_t current_rate
= this->stepper_motor
->get_steps_per_second();
548 uint32_t target_rate
= int(floor(this->feed_rate
* this->steps_per_millimeter
));
550 if( current_rate
< target_rate
) {
551 uint32_t rate_increase
= int(floor((this->acceleration
/ THEKERNEL
->stepper
->get_acceleration_ticks_per_second()) * this->steps_per_millimeter
));
552 current_rate
= min( target_rate
, current_rate
+ rate_increase
);
554 if( current_rate
> target_rate
) {
555 current_rate
= target_rate
;
559 this->stepper_motor
->set_speed(current_rate
);
564 // Speed has been updated for the robot's stepper, we must update accordingly
565 void Extruder::on_speed_change( void *argument
)
567 if(!this->enabled
) return;
569 // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
570 if( this->current_block
== NULL
|| this->paused
|| this->mode
!= FOLLOW
|| !this->stepper_motor
->is_moving()) {
574 // if we are flushing the queue we need to stop the motor when it has decelerated to zero
575 // this is what steppermotor does
576 if(THEKERNEL
->conveyor
->is_flushing() && THEKERNEL
->stepper
->get_trapezoid_adjusted_rate() == this->current_block
->rate_delta
* 0.5F
) {
577 this->stepper_motor
->move(0, 0);
578 this->current_block
->release();
579 this->current_block
= NULL
;
584 * nominal block duration = current block's steps / ( current block's nominal rate )
585 * nominal extruder rate = extruder steps / nominal block duration
586 * actual extruder rate = nominal extruder rate * ( ( stepper's steps per second ) / ( current block's nominal rate ) )
587 * or actual extruder rate = ( ( extruder steps * ( current block's nominal_rate ) ) / current block's steps ) * ( ( stepper's steps per second ) / ( current block's nominal rate ) )
588 * or simplified : extruder steps * ( stepper's steps per second ) ) / current block's steps
589 * or even : ( stepper steps per second ) * ( extruder steps / current block's steps )
592 this->stepper_motor
->set_speed(THEKERNEL
->stepper
->get_trapezoid_adjusted_rate() * (float)this->stepper_motor
->get_steps_to_move() / (float)this->current_block
->steps_event_count
);
596 // When the stepper has finished it's move
597 uint32_t Extruder::stepper_motor_finished_move(uint32_t dummy
)
599 if(!this->enabled
) return 0;
601 //printf("extruder releasing\r\n");
603 if (this->current_block
) { // this should always be true, but sometimes it isn't. TODO: find out why
604 Block
*block
= this->current_block
;
605 this->current_block
= NULL
;