src/modules/tools/extruder/Extruder.cpp

   1 #include "mbed.h"
   2 #include "libs/Module.h"
   3 #include "libs/Kernel.h"
   4 #include "modules/robot/Player.h"
   5 #include "modules/robot/Block.h"
   6 #include "modules/tools/extruder/Extruder.h"
   7
   8 Extruder::Extruder(PinName stppin, PinName dirpin) : step_pin(stppin), dir_pin(dirpin) {
   9     this->absolute_mode = true;
  10     this->direction     = 1;
  11     this->acceleration_lock = false;
  12 }
  13
  14 void Extruder::on_module_loaded() {
  15
  16     // Do not do anything if not enabledd
  17     if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; }
  18
  19     // Settings
  20     this->on_config_reload(this);
  21
  22     // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
  23     this->register_for_event(ON_BLOCK_BEGIN);
  24     this->register_for_event(ON_BLOCK_END);
  25     this->register_for_event(ON_GCODE_EXECUTE);
  26
  27     // Start values
  28     this->start_position = 0;
  29     this->target_position = 0;
  30     this->current_position = 0;
  31     this->current_block = NULL;
  32     this->mode = OFF;
  33
  34     // Update speed every *acceleration_ticks_per_second*
  35     // TODO: Make this an independent setting
  36     this->kernel->slow_ticker->attach( this, &Extruder::acceleration_tick );
  37
  38     // Initiate main_interrupt timer and step reset timer
  39     this->kernel->step_ticker->attach( this, &Extruder::stepping_tick );
  40     this->kernel->step_ticker->reset_attach( this, &Extruder::reset_step_pin );
  41
  42 }
  43
  44 // Get config
  45 void Extruder::on_config_reload(void* argument){
  46     this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number();
  47     this->steps_per_millimeter        = this->kernel->config->value(steps_per_millimeter_checksum       )->by_default(1)->as_number();
  48     this->feed_rate                   = this->kernel->config->value(default_feed_rate_checksum          )->by_default(1)->as_number();
  49     this->acceleration                = this->kernel->config->value(acceleration_checksum               )->by_default(1)->as_number();
  50 }
  51
  52 // Compute extrusion speed based on parameters and gcode distance of travel
  53 void Extruder::on_gcode_execute(void* argument){
  54     Gcode* gcode = static_cast<Gcode*>(argument);
  55
  56     // Absolute/relative mode
  57     if( gcode->has_letter('M')){
  58         int code = gcode->get_value('M');
  59         if( code == 82 ){ this->absolute_mode == true; }
  60         if( code == 83 ){ this->absolute_mode == false; }
  61     }
  62
  63     // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
  64     this->mode = OFF;
  65
  66     if( gcode->has_letter('G') ){
  67         // G92: Reset extruder position
  68         if( gcode->get_value('G') == 92 ){
  69             if( gcode->has_letter('E') ){
  70                 this->current_position = gcode->get_value('E');
  71                 this->target_position  = this->current_position;
  72                 this->start_position   = this->current_position;
  73             }
  74         }else{
  75             // Extrusion length from 'G' Gcode
  76             if( gcode->has_letter('E' )){
  77                 // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position )
  78                 double relative_extrusion_distance = gcode->get_value('E');
  79                 if( this->absolute_mode == true ){ relative_extrusion_distance = relative_extrusion_distance - this->target_position; }
  80
  81                 // If the robot is moving, we follow it's movement, otherwise, we move alone
  82                 if( fabs(gcode->millimeters_of_travel) < 0.0001 ){  // With floating numbers, we can have 0 != 0 ... beeeh
  83                     this->mode = SOLO;
  84                     this->travel_distance = relative_extrusion_distance;
  85                     if( gcode->has_letter('F') ){ this->feed_rate = gcode->get_value('F'); }
  86                 }else{
  87                     this->mode = FOLLOW;
  88                     // We move proportionally to the robot's movement
  89                     this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel;
  90                 }
  91             }
  92         }
  93     }
  94
  95 }
  96
  97 // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing )
  98 void Extruder::on_block_begin(void* argument){
  99     Block* block = static_cast<Block*>(argument);
 100
 101     if( this->mode == SOLO ){
 102         // In solo mode we take the block so we can move even if the stepper has nothing to do
 103         block->take();
 104         this->current_block = block;
 105         this->start_position = this->target_position;
 106         this->target_position = this->start_position + this->travel_distance ;
 107         this->travel_ratio = 0.2;   // TODO : Make a real acceleration thing
 108         if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
 109         this->set_speed(int(floor((this->feed_rate/60)*this->steps_per_millimeter)));//Speed in steps per second
 110     }else if( this->mode == FOLLOW ){
 111         // In non-solo mode, we just follow the stepper module
 112         this->current_block = block;
 113         this->start_position = this->target_position;
 114         this->target_position =  this->start_position + ( this->current_block->millimeters * this->travel_ratio );
 115         if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
 116         this->acceleration_tick();
 117     }
 118
 119 }
 120
 121 // When a block ends, pause the stepping interrupt
 122 void Extruder::on_block_end(void* argument){
 123     Block* block = static_cast<Block*>(argument);
 124     this->current_block = NULL;
 125 }
 126
 127 // Called periodically to change the speed to match acceleration or to match the speed of the robot
 128 void Extruder::acceleration_tick(){
 129
 130     // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
 131     if( this->current_block == NULL || this->acceleration_lock ){ return; }
 132     this->acceleration_lock = true;
 133
 134     // In solo mode, we mode independently from the robot
 135     if( this->mode == SOLO ){
 136         // TODO : Do real acceleration here
 137         this->travel_ratio += 0.03;
 138         if( this->travel_ratio > 1 ){ this->travel_ratio = 1; }
 139         this->set_speed( int(floor(((this->feed_rate/60)*this->steps_per_millimeter)*this->travel_ratio)) );  // Speed in steps per second
 140
 141         // In follow mode we match the speed of the robot, + eventually advance
 142     }else if( this->mode == FOLLOW ){
 143         Stepper* stepper = this->kernel->stepper; // Just for convenience
 144
 145         // Strategy :
 146         //   * Find where in the block will the stepper be at the next tick ( if the block will have ended then, don't change speed )
 147         //   * Find what position this is for us
 148         //   * Find what speed we must go at to be at that position for the next acceleration tick
 149         // 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
 150         // 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
 151
 152         int ticks_forward = 3;
 153         // 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 ... )
 154         double next_stepper_rate = stepper->trapezoid_adjusted_rate;
 155         double step_events_completed =   (double(double(stepper->step_events_completed)/double(1<<16)));
 156         double position = ( this->current_position - this->start_position ) * this->direction ;
 157         double length = fabs( this->start_position - this->target_position );
 158         double last_ratio = -1;
 159
 160         // Do the startegy above, but if it does not work, look a bit further and try again, and again ...
 161         while(1){
 162
 163             // Find the position where we should be at the next tick
 164             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 );
 165             double next_relative_position = ( length * next_ratio );
 166
 167             // Advance
 168             // TODO: Proper advance configuration
 169             //double advance = double(next_stepper_rate) * 0.00001 * 0.15;
 170             double advance = 0;
 171             next_relative_position += ( advance );
 172
 173             // 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 ...
 174             if( last_ratio == next_ratio ){ this->acceleration_lock = false; return; }else{ last_ratio = next_ratio; }
 175             if( next_ratio == 0 || next_ratio > 1 ){ this->acceleration_lock = false; return; }
 176             if( ticks_forward > 1000 ){ this->acceleration_lock = false; return; } // This is very ugly
 177
 178             // Hack : We have not looked far enough, we compute how far ahead we must look to get a relevant value
 179             if( position > next_relative_position ){
 180                 double far_back = position - next_relative_position;
 181                 double far_back_ratio = far_back / length;
 182                 double move_duration = double( this->current_block->steps_event_count ) / ( double(next_stepper_rate) / 60 ) ;
 183                 double ticks_in_a_move = round( stepper->acceleration_ticks_per_second * move_duration );
 184                 double ratio_per_tick = 1 / ticks_in_a_move;
 185                 double ticks_to_equilibrium = ceil(far_back_ratio / ratio_per_tick) + 1;
 186                 ticks_forward += ticks_to_equilibrium;
 187                 // 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
 188                 if( this->mode != FOLLOW ){ this->acceleration_lock = false; return; }
 189                 continue;
 190             }
 191
 192             // Finally, compute the speed to get to that next position
 193             double next_absolute_position = this->start_position + ( this->direction * next_relative_position );
 194             double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter;
 195             double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) );
 196
 197             // Change stepping speed
 198             this->set_speed( speed_to_next_tick );
 199
 200             this->acceleration_lock = false;
 201             return;
 202         }
 203     }
 204
 205     this->acceleration_lock = false;
 206 }
 207
 208 // Convenience function to set stepping speed
 209 void Extruder::set_speed( int steps_per_second ){
 210
 211     // TODO : Proper limit config value
 212     if( steps_per_second > (this->feed_rate*double(this->steps_per_millimeter))/60 ){
 213         steps_per_second = (this->feed_rate*double(this->steps_per_millimeter))/60;
 214     }
 215
 216     this->counter_increment = int(floor(double(1<<16)/double(this->kernel->stepper->base_stepping_frequency / steps_per_second)));
 217
 218 }
 219
 220 inline void Extruder::stepping_tick(){
 221
 222     this->step_counter += this->counter_increment;
 223     if( this->step_counter > 1<<16 ){
 224         this->step_counter -= 1<<16;
 225
 226         // If we still have steps to do
 227         // TODO: Step using the same timer as the robot, and count steps instead of absolute float position
 228         if( ( this->current_position < this->target_position && this->direction == 1 ) || ( this->current_position > this->target_position && this->direction == -1 ) ){
 229             this->current_position += (double(double(1)/double(this->steps_per_millimeter)))*double(this->direction);
 230             this->dir_pin = ((this->direction > 0) ? 1 : 0);
 231             this->step_pin = 1;
 232         }else{
 233             // Move finished
 234             if( this->mode == SOLO && this->current_block != NULL ){
 235                 // In follow mode, the robot takes and releases the block, in solo mode we do
 236                 this->current_block->release();
 237             }
 238         }
 239     }
 240 }
 241
 242 void Extruder::reset_step_pin(){
 243     this->step_pin = 0;
 244 }