Extruder.cpp

00001 /*
00002       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 
00008 
00009 #include "libs/Module.h"
00010 #include "libs/Kernel.h"
00011 #include "modules/robot/Player.h"
00012 #include "modules/robot/Block.h"
00013 #include "modules/tools/extruder/Extruder.h"
00014 
00015 #define extruder_step_pin_checksum    40763
00016 #define extruder_dir_pin_checksum     57277
00017 #define extruder_en_pin_checksum      8017
00018 
00019 Extruder::Extruder() {
00020     this->absolute_mode = true;
00021     this->direction     = 1;
00022     this->acceleration_lock = false;
00023     this->step_counter = 0;
00024     this->counter_increment = 0;
00025     this->paused = false;
00026 }
00027 
00028 void Extruder::on_module_loaded() {
00029 
00030     // Do not do anything if not enabledd
00031     if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; }
00032 
00033     // Settings
00034     this->on_config_reload(this);
00035 
00036     // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
00037     this->register_for_event(ON_BLOCK_BEGIN);
00038     this->register_for_event(ON_BLOCK_END);
00039     this->register_for_event(ON_GCODE_EXECUTE);
00040     this->register_for_event(ON_PLAY);
00041     this->register_for_event(ON_PAUSE);
00042 
00043     // Start values
00044     this->start_position = 0;
00045     this->target_position = 0;
00046     this->current_position = 0;
00047     this->current_block = NULL;
00048     this->mode = OFF;
00049 
00050     // Update speed every *acceleration_ticks_per_second*
00051     // TODO: Make this an independent setting
00052     this->kernel->slow_ticker->attach( this->kernel->stepper->acceleration_ticks_per_second , this, &Extruder::acceleration_tick );
00053 
00054     // Initiate main_interrupt timer and step reset timer
00055     this->kernel->step_ticker->attach( this, &Extruder::stepping_tick );
00056     this->kernel->step_ticker->reset_attach( this, &Extruder::reset_step_pin );
00057 
00058 }
00059 
00060 // Get config
00061 void Extruder::on_config_reload(void* argument){
00062     this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_checksum)->by_default(5)->as_number();
00063     this->steps_per_millimeter        = this->kernel->config->value(extruder_steps_per_mm_checksum       )->by_default(1)->as_number();
00064     this->feed_rate                   = this->kernel->config->value(default_feed_rate_checksum          )->by_default(1)->as_number();
00065     this->acceleration                = this->kernel->config->value(acceleration_checksum               )->by_default(1)->as_number();
00066 
00067     this->step_pin                    = this->kernel->config->value(extruder_step_pin_checksum          )->by_default("1.22" )->as_pin()->as_output();
00068     this->dir_pin                     = this->kernel->config->value(extruder_dir_pin_checksum           )->by_default("1.19" )->as_pin()->as_output();
00069     this->en_pin                      = this->kernel->config->value(extruder_en_pin_checksum            )->by_default("0.19" )->as_pin()->as_output();
00070 }
00071 
00072 
00073 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
00074 void Extruder::on_pause(void* argument){
00075     this->paused = true;
00076 }
00077 
00078 // When the play/pause button is set to play, or a module calls the ON_PLAY event
00079 void Extruder::on_play(void* argument){
00080     this->paused = false;
00081 }
00082 
00083 
00084 
00085 // Compute extrusion speed based on parameters and gcode distance of travel
00086 void Extruder::on_gcode_execute(void* argument){
00087     Gcode* gcode = static_cast<Gcode*>(argument);
00088 
00089     // Absolute/relative mode
00090     if( gcode->has_letter('M')){
00091         int code = (int) gcode->get_value('M');
00092         if( code == 82 ){ this->absolute_mode = true; }
00093         if( code == 83 ){ this->absolute_mode = false; }
00094         if( code == 84 ){ this->en_pin->set(0); }
00095     }
00096 
00097     // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
00098     this->mode = OFF;
00099 
00100     if( gcode->has_letter('G') ){
00101         // G92: Reset extruder position
00102         if( gcode->get_value('G') == 92 ){
00103             if( gcode->has_letter('E') ){
00104                 this->current_position = gcode->get_value('E');
00105                 this->target_position  = this->current_position;
00106                 this->start_position   = this->current_position;
00107             }
00108         }else{
00109             // Extrusion length from 'G' Gcode
00110             if( gcode->has_letter('E' )){
00111                 // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position )
00112                 double relative_extrusion_distance = gcode->get_value('E');
00113                 if( this->absolute_mode == true ){ relative_extrusion_distance = relative_extrusion_distance - this->target_position; }
00114 
00115                 // If the robot is moving, we follow it's movement, otherwise, we move alone
00116                 if( fabs(gcode->millimeters_of_travel) < 0.0001 ){  // With floating numbers, we can have 0 != 0 ... beeeh
00117                     this->mode = SOLO;
00118                     this->travel_distance = relative_extrusion_distance;
00119                     if( gcode->has_letter('F') ){ this->feed_rate = gcode->get_value('F'); }
00120                 }else{
00121                     this->mode = FOLLOW;
00122                     // We move proportionally to the robot's movement
00123                     this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel;
00124                 }
00125 
00126                 this->en_pin->set(1);
00127             }
00128         }
00129     }
00130 
00131 }
00132 
00133 // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing )
00134 void Extruder::on_block_begin(void* argument){
00135     Block* block = static_cast<Block*>(argument);
00136     if( this->mode == SOLO ){
00137         // In solo mode we take the block so we can move even if the stepper has nothing to do
00138         block->take();
00139         this->current_block = block;
00140         this->start_position = this->target_position;
00141         this->target_position = this->start_position + this->travel_distance ;
00142         this->travel_ratio = 0.2;   // TODO : Make a real acceleration thing
00143         if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
00144         this->set_speed(int(floor((this->feed_rate/60)*this->steps_per_millimeter)));//Speed in steps per second
00145     }else if( this->mode == FOLLOW ){
00146         // In non-solo mode, we just follow the stepper module
00147         this->current_block = block;
00148         this->start_position = this->target_position;
00149         this->target_position =  this->start_position + ( this->current_block->millimeters * this->travel_ratio );
00150         if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
00151         this->acceleration_tick(0);
00152     }
00153 
00154 }
00155 
00156 // When a block ends, pause the stepping interrupt
00157 void Extruder::on_block_end(void* argument){
00158     Block* block = static_cast<Block*>(argument);
00159     this->current_block = NULL;
00160 }
00161 
00162 // Called periodically to change the speed to match acceleration or to match the speed of the robot
00163 uint32_t Extruder::acceleration_tick(uint32_t dummy){
00164 
00165     // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
00166     if( this->current_block == NULL || this->acceleration_lock || this->paused ){ return 0; }
00167     this->acceleration_lock = true;
00168 
00169     // In solo mode, we mode independently from the robot
00170     if( this->mode == SOLO ){
00171         // TODO : Do real acceleration here
00172         this->travel_ratio += 0.03;
00173         if( this->travel_ratio > 1 ){ this->travel_ratio = 1; }
00174         this->set_speed( int(floor(((this->feed_rate/60)*this->steps_per_millimeter)*this->travel_ratio)) );  // Speed in steps per second
00175 
00176         // In follow mode we match the speed of the robot, + eventually advance
00177     }else if( this->mode == FOLLOW ){
00178         Stepper* stepper = this->kernel->stepper; // Just for convenience
00179 
00180         // Strategy :
00181         //   * Find where in the block will the stepper be at the next tick ( if the block will have ended then, don't change speed )
00182         //   * Find what position this is for us
00183         //   * Find what speed we must go at to be at that position for the next acceleration tick
00184         // TODO : This works, but PLEASE PLEASE PLEASE if you know a better way to do it, do it better, I don't find this elegant at all, it's just the best I could think of
00185         // UPDATE: Yes, this sucks, I have ideas on how to do it better. If this is really bugging you, open a ticket and I'll make it a priority
00186 
00187         int ticks_forward = 3;
00188         // We need to take those values here, and then use those instead of the live values, because using the live values inside the loop can break things ( infinite loops etc ... )
00189         double next_stepper_rate = stepper->trapezoid_adjusted_rate;
00190         double step_events_completed =   (double(double(stepper->step_events_completed)/double(1<<16)));
00191         double position = ( this->current_position - this->start_position ) * this->direction ;
00192         double length = fabs( this->start_position - this->target_position );
00193         double last_ratio = -1;
00194 
00195         // Do the startegy above, but if it does not work, look a bit further and try again, and again ...
00196         while(1){
00197 
00198             // Find the position where we should be at the next tick
00199             double next_ratio = double( step_events_completed + ( next_stepper_rate / 60 / ((double(stepper->acceleration_ticks_per_second)/ticks_forward)) ) ) / double( this->current_block->steps_event_count );
00200             double next_relative_position = ( length * next_ratio );
00201 
00202             // Advance
00203             // TODO: Proper advance configuration
00204             double advance = double(next_stepper_rate) * ( 0.00001 * 0.15 ) * 0.4 ;
00205             //double advance = 0;
00206             next_relative_position += ( advance );
00207 
00208             // TODO : all of those "if->return" is very hacky, we should do the math in a way where most of those don't happen, but that requires doing tons of drawing ...
00209             if( last_ratio == next_ratio ){ this->acceleration_lock = false; return 0; }else{ last_ratio = next_ratio; }
00210             if( next_ratio == 0 || next_ratio > 1 ){ this->acceleration_lock = false; return 0; }
00211             if( ticks_forward > 1000 ){ this->acceleration_lock = false; return 0; } // This is very ugly
00212 
00213             // Hack : We have not looked far enough, we compute how far ahead we must look to get a relevant value
00214             if( position > next_relative_position ){
00215                 double far_back = position - next_relative_position;
00216                 double far_back_ratio = far_back / length;
00217                 double move_duration = double( this->current_block->steps_event_count ) / ( double(next_stepper_rate) / 60 ) ;
00218                 double ticks_in_a_move = floor( stepper->acceleration_ticks_per_second * move_duration +0.5);
00219                 double ratio_per_tick = 1 / ticks_in_a_move;
00220                 double ticks_to_equilibrium = ceil(far_back_ratio / ratio_per_tick) + 1;
00221                 ticks_forward += ticks_to_equilibrium;
00222                 // Because this is a loop, and we can be interrupted by the stepping interrupt, if that interrupt changes block, the new block may not be solo, and we may get trapped into an infinite loop
00223                 if( this->mode != FOLLOW ){ this->acceleration_lock = false; return 0; }
00224                 continue;
00225             }
00226 
00227             // Finally, compute the speed to get to that next position
00228             double next_absolute_position = this->start_position + ( this->direction * next_relative_position );
00229             double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter;
00230             double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) );
00231 
00232             // Change stepping speed
00233             this->set_speed( speed_to_next_tick );
00234 
00235             this->acceleration_lock = false;
00236             return 0;
00237         }
00238     }
00239 
00240     this->acceleration_lock = false;
00241     return 0;
00242 }
00243 
00244 // Convenience function to set stepping speed
00245 void Extruder::set_speed( int steps_per_second ){
00246 
00247     if( steps_per_second < 10 ){ steps_per_second = 10; }
00248 
00249     // TODO : Proper limit config value
00250     if( steps_per_second > (this->feed_rate*double(this->steps_per_millimeter))/60 ){
00251         steps_per_second = (this->feed_rate*double(this->steps_per_millimeter))/60;
00252     }
00253 
00254     this->counter_increment = int(floor(double(1<<16)/double(this->kernel->stepper->base_stepping_frequency / steps_per_second)));
00255 
00256 }
00257 
00258 inline uint32_t Extruder::stepping_tick(uint32_t dummy){
00259     if( this->paused ){ return 0; }
00260 
00261     this->step_counter += this->counter_increment;
00262     if( this->step_counter > 1<<16 ){
00263         this->step_counter -= 1<<16;
00264 
00265         // If we still have steps to do
00266         // TODO: Step using the same timer as the robot, and count steps instead of absolute float position
00267         if( ( this->current_position < this->target_position && this->direction == 1 ) || ( this->current_position > this->target_position && this->direction == -1 ) ){
00268             this->current_position += (double(double(1)/double(this->steps_per_millimeter)))*double(this->direction);
00269             this->dir_pin->set((this->direction > 0) ? 1 : 0);
00270             this->step_pin->set(1);
00271         }else{
00272             // Move finished
00273             if( this->mode == SOLO && this->current_block != NULL ){
00274                 // In follow mode, the robot takes and releases the block, in solo mode we do
00275                 this->current_block->release();
00276             }
00277         }
00278     }
00279     return 0;
00280 }
00281 
00282 uint32_t Extruder::reset_step_pin(uint32_t dummy){
00283     this->step_pin->set(0);
00284     return 0;
00285 }