src/modules/tools/extruder/Extruder.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 #include "Extruder.h"
   9
  10 #include "libs/Module.h"
  11 #include "libs/Kernel.h"
  12
  13 #include "modules/robot/Conveyor.h"
  14 #include "modules/robot/Block.h"
  15 #include "StepperMotor.h"
  16 #include "SlowTicker.h"
  17 #include "Stepper.h"
  18 #include "StepTicker.h"
  19 #include "Config.h"
  20 #include "StepperMotor.h"
  21 #include "Robot.h"
  22 #include "checksumm.h"
  23 #include "ConfigValue.h"
  24 #include "Gcode.h"
  25 #include "libs/StreamOutput.h"
  26 #include "PublicDataRequest.h"
  27 #include "StreamOutputPool.h"
  28 #include "ExtruderPublicAccess.h"
  29
  30 #include <mri.h>
  31
  32 // OLD config names for backwards compatibility, NOTE new configs will not be added here
  33 #define extruder_module_enable_checksum      CHECKSUM("extruder_module_enable")
  34 #define extruder_steps_per_mm_checksum       CHECKSUM("extruder_steps_per_mm")
  35 #define extruder_filament_diameter_checksum  CHECKSUM("extruder_filament_diameter")
  36 #define extruder_acceleration_checksum       CHECKSUM("extruder_acceleration")
  37 #define extruder_step_pin_checksum           CHECKSUM("extruder_step_pin")
  38 #define extruder_dir_pin_checksum            CHECKSUM("extruder_dir_pin")
  39 #define extruder_en_pin_checksum             CHECKSUM("extruder_en_pin")
  40 #define extruder_max_speed_checksum          CHECKSUM("extruder_max_speed")
  41 #define extruder_default_feed_rate_checksum  CHECKSUM("extruder_default_feed_rate")
  42
  43 // NEW config names
  44
  45 #define default_feed_rate_checksum           CHECKSUM("default_feed_rate")
  46 #define steps_per_mm_checksum                CHECKSUM("steps_per_mm")
  47 #define filament_diameter_checksum           CHECKSUM("filament_diameter")
  48 #define acceleration_checksum                CHECKSUM("acceleration")
  49 #define step_pin_checksum                    CHECKSUM("step_pin")
  50 #define dir_pin_checksum                     CHECKSUM("dir_pin")
  51 #define en_pin_checksum                      CHECKSUM("en_pin")
  52 #define max_speed_checksum                   CHECKSUM("max_speed")
  53 #define x_offset_checksum                    CHECKSUM("x_offset")
  54 #define y_offset_checksum                    CHECKSUM("y_offset")
  55 #define z_offset_checksum                    CHECKSUM("z_offset")
  56
  57 #define retract_length_checksum              CHECKSUM("retract_length")
  58 #define retract_feedrate_checksum            CHECKSUM("retract_feedrate")
  59 #define retract_recover_length_checksum      CHECKSUM("retract_recover_length")
  60 #define retract_recover_feedrate_checksum    CHECKSUM("retract_recover_feedrate")
  61 #define retract_zlift_length_checksum        CHECKSUM("retract_zlift_length")
  62 #define retract_zlift_feedrate_checksum      CHECKSUM("retract_zlift_feedrate")
  63
  64 #define X_AXIS      0
  65 #define Y_AXIS      1
  66 #define Z_AXIS      2
  67
  68 #define OFF 0
  69 #define SOLO 1
  70 #define FOLLOW 2
  71
  72 #define PI 3.14159265358979F
  73
  74
  75 /* The extruder module controls a filament extruder for 3D printing: http://en.wikipedia.org/wiki/Fused_deposition_modeling
  76 * 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 )
  77 * or the head moves, and the extruder moves plastic at a speed proportional to the movement of the head ( FOLLOW mode here ).
  78 */
  79
  80 Extruder::Extruder( uint16_t config_identifier, bool single )
  81 {
  82     this->absolute_mode = true;
  83     this->milestone_absolute_mode = true;
  84     this->enabled = false;
  85     this->single_config = single;
  86     this->identifier = config_identifier;
  87     this->retracted = false;
  88     this->volumetric_multiplier = 1.0F;
  89     this->extruder_multiplier = 1.0F;
  90     this->stepper_motor = nullptr;
  91     this->milestone_last_position = 0;
  92     this->max_volumetric_rate = 0;
  93
  94     memset(this->offset, 0, sizeof(this->offset));
  95 }
  96
  97 Extruder::~Extruder()
  98 {
  99     delete stepper_motor;
 100 }
 101
 102 void Extruder::on_halt(void *arg)
 103 {
 104     if(arg == nullptr) {
 105         // turn off motor
 106         this->en_pin.set(1);
 107     }
 108 }
 109
 110 void Extruder::on_module_loaded()
 111 {
 112     // Settings
 113     this->on_config_reload(this);
 114
 115     // Start values
 116     this->target_position = 0;
 117     this->current_position = 0;
 118     this->unstepped_distance = 0;
 119     this->current_block = NULL;
 120     this->mode = OFF;
 121
 122     // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
 123     this->register_for_event(ON_BLOCK_BEGIN);
 124     this->register_for_event(ON_BLOCK_END);
 125     this->register_for_event(ON_GCODE_RECEIVED);
 126     this->register_for_event(ON_GCODE_EXECUTE);
 127     this->register_for_event(ON_HALT);
 128     this->register_for_event(ON_SPEED_CHANGE);
 129     this->register_for_event(ON_GET_PUBLIC_DATA);
 130     this->register_for_event(ON_SET_PUBLIC_DATA);
 131
 132     // Update speed every *acceleration_ticks_per_second*
 133     THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {
 134         acceleration_tick();
 135     });
 136 }
 137
 138 // Get config
 139 void Extruder::on_config_reload(void *argument)
 140 {
 141     if( this->single_config ) {
 142         // If this module uses the old "single extruder" configuration style
 143
 144         this->steps_per_millimeter        = THEKERNEL->config->value(extruder_steps_per_mm_checksum      )->by_default(1)->as_number();
 145         this->filament_diameter           = THEKERNEL->config->value(extruder_filament_diameter_checksum )->by_default(0)->as_number();
 146         this->acceleration                = THEKERNEL->config->value(extruder_acceleration_checksum      )->by_default(1000)->as_number();
 147         this->feed_rate                   = THEKERNEL->config->value(extruder_default_feed_rate_checksum )->by_default(1000)->as_number();
 148
 149         this->step_pin.from_string(         THEKERNEL->config->value(extruder_step_pin_checksum          )->by_default("nc" )->as_string())->as_output();
 150         this->dir_pin.from_string(          THEKERNEL->config->value(extruder_dir_pin_checksum           )->by_default("nc" )->as_string())->as_output();
 151         this->en_pin.from_string(           THEKERNEL->config->value(extruder_en_pin_checksum            )->by_default("nc" )->as_string())->as_output();
 152
 153         for(int i = 0; i < 3; i++) {
 154             this->offset[i] = 0;
 155         }
 156
 157         this->enabled = true;
 158
 159     } else {
 160         // If this module was created with the new multi extruder configuration style
 161
 162         this->steps_per_millimeter = THEKERNEL->config->value(extruder_checksum, this->identifier, steps_per_mm_checksum      )->by_default(1)->as_number();
 163         this->filament_diameter    = THEKERNEL->config->value(extruder_checksum, this->identifier, filament_diameter_checksum )->by_default(0)->as_number();
 164         this->acceleration         = THEKERNEL->config->value(extruder_checksum, this->identifier, acceleration_checksum      )->by_default(1000)->as_number();
 165         this->feed_rate            = THEKERNEL->config->value(extruder_checksum, this->identifier, default_feed_rate_checksum )->by_default(1000)->as_number();
 166
 167         this->step_pin.from_string( THEKERNEL->config->value(extruder_checksum, this->identifier, step_pin_checksum          )->by_default("nc" )->as_string())->as_output();
 168         this->dir_pin.from_string(  THEKERNEL->config->value(extruder_checksum, this->identifier, dir_pin_checksum           )->by_default("nc" )->as_string())->as_output();
 169         this->en_pin.from_string(   THEKERNEL->config->value(extruder_checksum, this->identifier, en_pin_checksum            )->by_default("nc" )->as_string())->as_output();
 170
 171         this->offset[X_AXIS] = THEKERNEL->config->value(extruder_checksum, this->identifier, x_offset_checksum          )->by_default(0)->as_number();
 172         this->offset[Y_AXIS] = THEKERNEL->config->value(extruder_checksum, this->identifier, y_offset_checksum          )->by_default(0)->as_number();
 173         this->offset[Z_AXIS] = THEKERNEL->config->value(extruder_checksum, this->identifier, z_offset_checksum          )->by_default(0)->as_number();
 174
 175     }
 176
 177     // these are only supported in the new syntax, no need to be backward compatible as they did not exist before the change
 178     this->retract_length           = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_length_checksum)->by_default(3)->as_number();
 179     this->retract_feedrate         = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_feedrate_checksum)->by_default(45)->as_number();
 180     this->retract_recover_length   = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_recover_length_checksum)->by_default(0)->as_number();
 181     this->retract_recover_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_recover_feedrate_checksum)->by_default(8)->as_number();
 182     this->retract_zlift_length     = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_length_checksum)->by_default(0)->as_number();
 183     this->retract_zlift_feedrate   = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_feedrate_checksum)->by_default(100 * 60)->as_number(); // mm/min
 184
 185     if(filament_diameter > 0.01F) {
 186         this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI);
 187     }
 188
 189     // Stepper motor object for the extruder
 190     this->stepper_motor = new StepperMotor(step_pin, dir_pin, en_pin);
 191     this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move );
 192     if( this->single_config ) {
 193         this->stepper_motor->set_max_rate(THEKERNEL->config->value(extruder_max_speed_checksum)->by_default(1000)->as_number());
 194     } else {
 195         this->stepper_motor->set_max_rate(THEKERNEL->config->value(extruder_checksum, this->identifier, max_speed_checksum)->by_default(1000)->as_number());
 196     }
 197 }
 198
 199 void Extruder::on_get_public_data(void *argument)
 200 {
 201     PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
 202
 203     if(!pdr->starts_with(extruder_checksum)) return;
 204
 205     if(this->enabled) {
 206         // Note this is allowing both step/mm and filament diameter to be exposed via public data
 207         pdr->set_data_ptr(&this->steps_per_millimeter);
 208         pdr->set_taken();
 209     }
 210 }
 211
 212 // check against maximum speeds and return the rate modifier
 213 float Extruder::check_max_speeds(float target, float isecs)
 214 {
 215     float rm = 1.0F; // default no rate modification
 216     float delta;
 217     // get change in E (may be mm or mm³)
 218     if(milestone_absolute_mode) {
 219         delta = fabsf(target - milestone_last_position); // delta move
 220         milestone_last_position = target;
 221
 222     } else {
 223         delta = target;
 224         milestone_last_position += target;
 225     }
 226
 227     if(this->max_volumetric_rate > 0 && this->filament_diameter > 0.01F) {
 228         // volumetric enabled and check for volumetric rate
 229         float v = delta * isecs; // the flow rate in mm³/sec
 230
 231         // return the rate change needed to stay within the max rate
 232         if(v > max_volumetric_rate) {
 233             rm = max_volumetric_rate / v;
 234             isecs *= rm; // this slows the rate down for the next test
 235         }
 236         //THEKERNEL->streams->printf("requested flow rate: %f mm³/sec, corrected flow rate: %f  mm³/sec\n", v, v * rm);
 237     }
 238
 239     // check for max speed as well
 240     float max_speed = this->stepper_motor->get_max_rate();
 241     if(max_speed > 0) {
 242         if(this->filament_diameter > 0.01F) {
 243             // volumetric so need to convert delta which is mm³ to mm
 244             delta *= volumetric_multiplier;
 245         }
 246
 247         float sm = 1.0F;
 248         float v = delta * isecs; // the speed in mm/sec
 249         if(v > max_speed) {
 250             sm *= (max_speed / v);
 251         }
 252         //THEKERNEL->streams->printf("requested speed: %f mm/sec, corrected speed: %f  mm/sec\n", v, v * sm);
 253         rm *= sm;
 254     }
 255     return rm;
 256 }
 257
 258 void Extruder::on_set_public_data(void *argument)
 259 {
 260     PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
 261
 262     if(!pdr->starts_with(extruder_checksum)) return;
 263
 264     // handle extrude rates request from robot
 265     if(pdr->second_element_is(target_checksum)) {
 266         // disabled extruders do not reply NOTE only one enabled extruder supported
 267         if(!this->enabled) return;
 268
 269         float *d = static_cast<float *>(pdr->get_data_ptr());
 270         float target = d[0]; // the E passed in on Gcode is in mm³ (maybe absolute or relative)
 271         float isecs = d[1]; // inverted secs
 272
 273         // check against maximum speeds and return rate modifier
 274         d[1] = check_max_speeds(target, isecs);
 275
 276         pdr->set_taken();
 277         return;
 278     }
 279
 280     // save or restore state
 281     if(pdr->second_element_is(save_state_checksum)) {
 282         this->saved_current_position = this->current_position;
 283         this->saved_absolute_mode = this->absolute_mode;
 284         pdr->set_taken();
 285     } else if(pdr->second_element_is(restore_state_checksum)) {
 286         // NOTE this only gets called when the queue is empty so the milestones will be the same
 287         this->milestone_last_position= this->current_position = this->saved_current_position;
 288         this->milestone_absolute_mode= this->absolute_mode = this->saved_absolute_mode;
 289         pdr->set_taken();
 290     }
 291 }
 292
 293 void Extruder::on_gcode_received(void *argument)
 294 {
 295     Gcode *gcode = static_cast<Gcode *>(argument);
 296
 297     // M codes most execute immediately, most only execute if enabled
 298     if (gcode->has_m) {
 299         if (gcode->m == 114 && this->enabled) {
 300             char buf[16];
 301             int n = snprintf(buf, sizeof(buf), " E:%1.3f ", this->current_position);
 302             gcode->txt_after_ok.append(buf, n);
 303
 304         } else if (gcode->m == 92 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier) ) ) {
 305             float spm = this->steps_per_millimeter;
 306             if (gcode->has_letter('E')) {
 307                 spm = gcode->get_value('E');
 308                 this->steps_per_millimeter = spm;
 309             }
 310
 311             gcode->stream->printf("E:%g ", spm);
 312             gcode->add_nl = true;
 313
 314         } else if (gcode->m == 200 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
 315             if (gcode->has_letter('D')) {
 316                 THEKERNEL->conveyor->wait_for_empty_queue(); // only apply after the queue has emptied
 317                 this->filament_diameter = gcode->get_value('D');
 318                 if(filament_diameter > 0.01F) {
 319                     this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI);
 320                 } else {
 321                     this->volumetric_multiplier = 1.0F;
 322                 }
 323             } else {
 324                 if(filament_diameter > 0.01F) {
 325                     gcode->stream->printf("Filament Diameter: %f\n", this->filament_diameter);
 326                 } else {
 327                     gcode->stream->printf("Volumetric extrusion is disabled\n");
 328                 }
 329             }
 330
 331         } else if (gcode->m == 203 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
 332             // M203 Exxx Vyyy Set maximum feedrates xxx mm/sec and/or yyy mm³/sec
 333             if(gcode->get_num_args() == 0) {
 334                 gcode->stream->printf("E:%g V:%g", this->stepper_motor->get_max_rate(), this->max_volumetric_rate);
 335                 gcode->add_nl = true;
 336
 337             } else {
 338                 if(gcode->has_letter('E')) {
 339                     this->stepper_motor->set_max_rate(gcode->get_value('E'));
 340                 }
 341                 if(gcode->has_letter('V')) {
 342                     this->max_volumetric_rate = gcode->get_value('V');
 343                 }
 344             }
 345
 346         } else if (gcode->m == 204 && gcode->has_letter('E') &&
 347                    ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
 348             // extruder acceleration M204 Ennn mm/sec^2 (Pnnn sets the specific extruder for M500)
 349             this->acceleration = gcode->get_value('E');
 350
 351         } else if (gcode->m == 207 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
 352             // M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop] Q[zlift feedrate mm/min]
 353             if(gcode->has_letter('S')) retract_length = gcode->get_value('S');
 354             if(gcode->has_letter('F')) retract_feedrate = gcode->get_value('F') / 60.0F; // specified in mm/min converted to mm/sec
 355             if(gcode->has_letter('Z')) retract_zlift_length = gcode->get_value('Z');
 356             if(gcode->has_letter('Q')) retract_zlift_feedrate = gcode->get_value('Q');
 357
 358         } else if (gcode->m == 208 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
 359             // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
 360             if(gcode->has_letter('S')) retract_recover_length = gcode->get_value('S');
 361             if(gcode->has_letter('F')) retract_recover_feedrate = gcode->get_value('F') / 60.0F; // specified in mm/min converted to mm/sec
 362
 363         } else if (gcode->m == 221 && this->enabled) { // M221 S100 change flow rate by percentage
 364             if(gcode->has_letter('S')) {
 365                 this->extruder_multiplier = gcode->get_value('S') / 100.0F;
 366             } else {
 367                 gcode->stream->printf("Flow rate at %6.2f %%\n", this->extruder_multiplier * 100.0F);
 368             }
 369
 370         } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings
 371             if( this->single_config ) {
 372                 gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f\n", this->steps_per_millimeter);
 373                 gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f\n", this->filament_diameter);
 374                 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);
 375                 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);
 376                 gcode->stream->printf(";E acceleration mm/sec²:\nM204 E%1.4f\n", this->acceleration);
 377                 gcode->stream->printf(";E max feed rate mm/sec:\nM203 E%1.4f\n", this->stepper_motor->get_max_rate());
 378                 if(this->max_volumetric_rate > 0) {
 379                     gcode->stream->printf(";E max volumetric rate mm³/sec:\nM203 V%1.4f\n", this->max_volumetric_rate);
 380                 }
 381
 382             } else {
 383                 gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f P%d\n", this->steps_per_millimeter, this->identifier);
 384                 gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f P%d\n", this->filament_diameter, this->identifier);
 385                 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);
 386                 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);
 387                 gcode->stream->printf(";E acceleration mm/sec²:\nM204 E%1.4f P%d\n", this->acceleration, this->identifier);
 388                 gcode->stream->printf(";E max feed rate mm/sec:\nM203 E%1.4f P%d\n", this->stepper_motor->get_max_rate(), this->identifier);
 389                 if(this->max_volumetric_rate > 0) {
 390                     gcode->stream->printf(";E max volumetric rate mm³/sec:\nM203 V%1.4f P%d\n", this->max_volumetric_rate, this->identifier);
 391                 }
 392             }
 393
 394         } else if( gcode->m == 17 || gcode->m == 18 || gcode->m == 82 || gcode->m == 83 || gcode->m == 84 ) {
 395             // Mcodes to pass along to on_gcode_execute
 396             THEKERNEL->conveyor->append_gcode(gcode);
 397
 398         }
 399
 400     } else if(gcode->has_g) {
 401         // G codes, NOTE some are ignored if not enabled
 402         if( (gcode->g == 92 && gcode->has_letter('E')) || (gcode->g == 90 || gcode->g == 91) ) {
 403             // Gcodes to pass along to on_gcode_execute
 404             THEKERNEL->conveyor->append_gcode(gcode);
 405
 406         } else if( this->enabled && gcode->g < 4 && gcode->has_letter('E') && fabsf(gcode->millimeters_of_travel) < 0.00001F ) { // With floating numbers, we can have 0 != 0, NOTE needs to be same as in Robot.cpp#745
 407             // NOTE was ... gcode->has_letter('E') && !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ) {
 408             // This is a SOLO move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
 409             THEKERNEL->conveyor->append_gcode(gcode);
 410             THEKERNEL->conveyor->queue_head_block();
 411
 412         } else if( this->enabled && (gcode->g == 10 || gcode->g == 11) ) { // firmware retract command
 413             // check we are in the correct state of retract or unretract
 414             if(gcode->g == 10 && !retracted) {
 415                 this->retracted = true;
 416                 this->cancel_zlift_restore = false;
 417             } else if(gcode->g == 11 && retracted) {
 418                 this->retracted = false;
 419             } else
 420                 return; // ignore duplicates
 421
 422             // 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
 423             // this way zlift will happen after retract, (or before for unretract) NOTE we call the robot->on_gcode_receive directly to avoid recursion
 424             if(retract_zlift_length > 0 && gcode->g == 11 && !this->cancel_zlift_restore) {
 425                 // reverse zlift happens before unretract
 426                 // 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
 427                 char buf[32];
 428                 int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", -retract_zlift_length, retract_zlift_feedrate);
 429                 string cmd(buf, n);
 430                 Gcode gc(cmd, &(StreamOutput::NullStream));
 431                 bool oldmode = THEKERNEL->robot->absolute_mode;
 432                 THEKERNEL->robot->absolute_mode = false; // needs to be relative mode
 433                 THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
 434                 THEKERNEL->robot->absolute_mode = oldmode; // restore mode
 435             }
 436
 437             // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
 438             THEKERNEL->conveyor->append_gcode(gcode);
 439             THEKERNEL->conveyor->queue_head_block();
 440
 441             if(retract_zlift_length > 0 && gcode->g == 10) {
 442                 char buf[32];
 443                 int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", retract_zlift_length, retract_zlift_feedrate);
 444                 string cmd(buf, n);
 445                 Gcode gc(cmd, &(StreamOutput::NullStream));
 446                 bool oldmode = THEKERNEL->robot->absolute_mode;
 447                 THEKERNEL->robot->absolute_mode = false; // needs to be relative mode
 448                 THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
 449                 THEKERNEL->robot->absolute_mode = oldmode; // restore mode
 450             }
 451
 452         } else if( this->enabled && this->retracted && (gcode->g == 0 || gcode->g == 1) && gcode->has_letter('Z')) {
 453             // NOTE we cancel the zlift restore for the following G11 as we have moved to an absolute Z which we need to stay at
 454             this->cancel_zlift_restore = true;
 455         }
 456     }
 457
 458     // handle some codes now for the volumetric rate limiting
 459     // G90 G91 G92 M82 M83
 460     if(gcode->has_m) {
 461         switch(gcode->m) {
 462             case 82: this->milestone_absolute_mode = true; break;
 463             case 83: this->milestone_absolute_mode = false; break;
 464         }
 465
 466     } else if(gcode->has_g) {
 467         switch(gcode->g) {
 468             case 90: this->milestone_absolute_mode = true; break;
 469             case 91: this->milestone_absolute_mode = false; break;
 470             case 92:
 471                 if(this->enabled) {
 472                     if(gcode->has_letter('E')) {
 473                         this->milestone_last_position = gcode->get_value('E');
 474                     } else if(gcode->get_num_args() == 0) {
 475                         this->milestone_last_position = 0;
 476                     }
 477                 }
 478                 break;
 479         }
 480     }
 481 }
 482
 483 // Compute extrusion speed based on parameters and gcode distance of travel
 484 void Extruder::on_gcode_execute(void *argument)
 485 {
 486     Gcode *gcode = static_cast<Gcode *>(argument);
 487
 488     // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
 489     this->mode = OFF;
 490
 491     // Absolute/relative mode, globably modal affect all extruders whether enabled or not
 492     if( gcode->has_m ) {
 493         switch(gcode->m) {
 494             case 17:
 495                 this->en_pin.set(0);
 496                 break;
 497             case 18:
 498                 this->en_pin.set(1);
 499                 break;
 500             case 82:
 501                 this->absolute_mode = true;
 502                 break;
 503             case 83:
 504                 this->absolute_mode = false;
 505                 break;
 506             case 84:
 507                 this->en_pin.set(1);
 508                 break;
 509         }
 510         return;
 511
 512     } else if( gcode->has_g && (gcode->g == 90 || gcode->g == 91) ) {
 513         this->absolute_mode = (gcode->g == 90);
 514         return;
 515     }
 516
 517
 518     if( gcode->has_g && this->enabled ) {
 519         // G92: Reset extruder position
 520         if( gcode->g == 92 ) {
 521             if( gcode->has_letter('E') ) {
 522                 this->current_position = gcode->get_value('E');
 523                 this->target_position  = this->current_position;
 524                 this->unstepped_distance = 0;
 525             } else if( gcode->get_num_args() == 0) {
 526                 this->current_position = 0.0;
 527                 this->target_position = this->current_position;
 528                 this->unstepped_distance = 0;
 529             }
 530
 531         } else if (gcode->g == 10) {
 532             // FW retract command
 533             feed_rate = retract_feedrate; // mm/sec
 534             this->mode = SOLO;
 535             this->travel_distance = -retract_length;
 536             this->target_position += this->travel_distance;
 537             this->en_pin.set(0);
 538
 539         } else if (gcode->g == 11) {
 540             // un retract command
 541             feed_rate = retract_recover_feedrate; // mm/sec
 542             this->mode = SOLO;
 543             this->travel_distance = (retract_length + retract_recover_length);
 544             this->target_position += this->travel_distance;
 545             this->en_pin.set(0);
 546
 547         } else if (gcode->g <= 3) {
 548             // Extrusion length from 'G' Gcode
 549             if( gcode->has_letter('E' )) {
 550                 // Get relative extrusion distance depending on mode ( in absolute mode we must subtract target_position )
 551                 float extrusion_distance = gcode->get_value('E');
 552                 float relative_extrusion_distance = extrusion_distance;
 553                 if (this->absolute_mode) {
 554                     relative_extrusion_distance -= this->target_position;
 555                     this->target_position = extrusion_distance;
 556                 } else {
 557                     this->target_position += relative_extrusion_distance;
 558                 }
 559
 560                 // If the robot is moving, we follow it's movement, otherwise, we move alone
 561                 if( fabsf(gcode->millimeters_of_travel) < 0.00001F ) { // With floating numbers, we can have 0 != 0, NOTE needs to be same as in Robot.cpp#745
 562                     this->mode = SOLO;
 563                     this->travel_distance = relative_extrusion_distance;
 564                 } else {
 565                     // We move proportionally to the robot's movement
 566                     this->mode = FOLLOW;
 567                     this->travel_ratio = (relative_extrusion_distance * this->volumetric_multiplier * this->extruder_multiplier) / gcode->millimeters_of_travel; // adjust for volumetric extrusion and extruder multiplier
 568                 }
 569
 570                 this->en_pin.set(0);
 571             }
 572
 573             // NOTE this is only used in SOLO mode, but any F on a G0/G1 will set the speed for future retracts that are not firmware retracts
 574             if (gcode->has_letter('F')) {
 575                 feed_rate = gcode->get_value('F') / THEKERNEL->robot->get_seconds_per_minute();
 576                 if (stepper_motor->get_max_rate() > 0 && feed_rate > stepper_motor->get_max_rate())
 577                     feed_rate = stepper_motor->get_max_rate();
 578             }
 579         }
 580     }
 581 }
 582
 583 // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing )
 584 void Extruder::on_block_begin(void *argument)
 585 {
 586     if(!this->enabled) return;
 587
 588     if( this->mode == OFF ) {
 589         this->current_block = NULL;
 590         this->stepper_motor->set_moved_last_block(false);
 591         return;
 592     }
 593
 594     Block *block = static_cast<Block *>(argument);
 595     if( this->mode == FOLLOW ) {
 596         // In FOLLOW mode, we just follow the stepper module
 597         this->travel_distance = block->millimeters * this->travel_ratio;
 598     }
 599
 600     // common for both FOLLOW and SOLO
 601     this->current_position += this->travel_distance ;
 602
 603     // round down, we take care of the fractional part next time
 604     int steps_to_step = abs(floorf(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) ));
 605
 606     // accumulate the fractional part
 607     if ( this->travel_distance > 0 ) {
 608         this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter);
 609     } else {
 610         this->unstepped_distance += this->travel_distance + (steps_to_step / this->steps_per_millimeter);
 611     }
 612
 613     if( steps_to_step != 0 ) {
 614         // We take the block, we have to release it or everything gets stuck
 615         block->take();
 616         this->current_block = block;
 617         this->stepper_motor->move( (this->travel_distance > 0), steps_to_step);
 618
 619         if(this->mode == FOLLOW) {
 620             on_speed_change(this); // set initial speed
 621             this->stepper_motor->set_moved_last_block(true);
 622         } else {
 623             // SOLO
 624             uint32_t target_rate = floorf(this->feed_rate * this->steps_per_millimeter);
 625             this->stepper_motor->set_speed(min( target_rate, rate_increase() ));  // start at first acceleration step
 626             this->stepper_motor->set_moved_last_block(false);
 627         }
 628
 629     } else {
 630         // no steps to take this time
 631         this->current_block = NULL;
 632         this->stepper_motor->set_moved_last_block(false);
 633     }
 634
 635 }
 636
 637 // When a block ends, pause the stepping interrupt
 638 void Extruder::on_block_end(void *argument)
 639 {
 640     if(!this->enabled) return;
 641     this->current_block = NULL;
 642 }
 643
 644 uint32_t Extruder::rate_increase() const
 645 {
 646     return floorf((this->acceleration / THEKERNEL->acceleration_ticks_per_second) * this->steps_per_millimeter);
 647 }
 648
 649 // Called periodically to change the speed to match acceleration or to match the speed of the robot
 650 // Only used in SOLO mode
 651 void Extruder::acceleration_tick(void)
 652 {
 653     // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
 654     if(!this->enabled || this->mode != SOLO || this->current_block == NULL || !this->stepper_motor->is_moving() ) {
 655         return;
 656     }
 657
 658     uint32_t current_rate = this->stepper_motor->get_steps_per_second();
 659     uint32_t target_rate = floorf(this->feed_rate * this->steps_per_millimeter);
 660
 661     if( current_rate < target_rate ) {
 662         current_rate = min( target_rate, current_rate + rate_increase() );
 663         // steps per second
 664         this->stepper_motor->set_speed(current_rate);
 665     }
 666
 667     return;
 668 }
 669
 670 // Speed has been updated for the robot's stepper, we must update accordingly
 671 void Extruder::on_speed_change( void *argument )
 672 {
 673     // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
 674     if(!this->enabled || this->current_block == NULL || this->mode != FOLLOW || !this->stepper_motor->is_moving()) {
 675         return;
 676     }
 677
 678     // if we are flushing the queue we need to stop the motor when it has decelerated to zero, we get this call with argumnet == 0 when this happens
 679     // this is what steppermotor does
 680     if(argument == 0) {
 681         this->stepper_motor->move(0, 0);
 682         this->current_block->release();
 683         this->current_block = NULL;
 684         return;
 685     }
 686
 687     /*
 688     * nominal block duration = current block's steps / ( current block's nominal rate )
 689     * nominal extruder rate = extruder steps / nominal block duration
 690     * actual extruder rate = nominal extruder rate * ( ( stepper's steps per second ) / ( current block's nominal rate ) )
 691     * 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 ) )
 692     * or simplified : extruder steps * ( stepper's steps per second ) ) / current block's steps
 693     * or even : ( stepper steps per second ) * ( extruder steps / current block's steps )
 694     */
 695
 696     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);
 697 }
 698
 699 // When the stepper has finished it's move
 700 uint32_t Extruder::stepper_motor_finished_move(uint32_t dummy)
 701 {
 702     if(!this->enabled) return 0;
 703
 704     //printf("extruder releasing\r\n");
 705
 706     if (this->current_block) { // this should always be true, but sometimes it isn't. TODO: find out why
 707         Block *block = this->current_block;
 708         this->current_block = NULL;
 709         block->release();
 710     }
 711     return 0;
 712
 713 }