src/modules/tools/extruder/Extruder.cpp

   1 #include "mbed.h"
   2 #include "libs/Module.h"
   3 #include "libs/Kernel.h"
   4 #include "modules/robot/Block.h"
   5 #include "modules/tools/extruder/Extruder.h"
   6
   7
   8
   9 Extruder* extruder_for_irq; // We need this global because ISRs can't be attached to an object
  10 extern "C" void TIMER1_IRQHandler (void){
  11     if((LPC_TIM1->IR >> 1) & 1){
  12         LPC_TIM1->IR |= 1 << 1;
  13         extruder_for_irq->step_pin = 0;
  14     }
  15     if((LPC_TIM1->IR >> 0) & 1){
  16         LPC_TIM1->IR |= 1 << 0;
  17         extruder_for_irq->stepping_tick();
  18     }
  19 }
  20
  21 Extruder::Extruder(PinName stppin) : step_pin(stppin){
  22     this->absolute_mode = true;
  23 }
  24
  25 void Extruder::on_module_loaded() {
  26
  27     if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; }
  28
  29     extruder_for_irq = this;
  30
  31     // Settings
  32     this->on_config_reload(this);
  33
  34     // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
  35     this->register_for_event(ON_BLOCK_BEGIN);
  36     this->register_for_event(ON_BLOCK_END);
  37     this->register_for_event(ON_GCODE_EXECUTE);
  38
  39     // Configuration
  40     this->acceleration_ticker.attach_us(this, &Extruder::acceleration_tick, 1000000/this->kernel->stepper->acceleration_ticks_per_second);
  41
  42     this->start_position = 0;
  43     this->target_position = 0;
  44     this->current_position = 0;
  45     this->current_block = NULL;
  46
  47     // Start Timer1
  48     LPC_TIM1->MR0 = 10000;
  49     LPC_TIM1->MR1 = 500;
  50     LPC_TIM1->MCR = 11;            // For MR0 and MR1, with no reset at MR1
  51     NVIC_EnableIRQ(TIMER1_IRQn);
  52     LPC_TIM1->TCR = 1;
  53 }
  54
  55 // Get config
  56 void Extruder::on_config_reload(void* argument){
  57     this->microseconds_per_step_pulse = 5; //this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number();
  58 }
  59
  60 // Computer extrusion speed based on parameters and gcode distance of travel
  61 void Extruder::on_gcode_execute(void* argument){
  62     Gcode* gcode = static_cast<Gcode*>(argument);
  63
  64     this->kernel->serial->printf("exec: %s \r\n", gcode->command.c_str() );
  65
  66     // Absolute/relative mode
  67     if( gcode->has_letter('M')){
  68         int code = gcode->get_value('M');
  69         if( code == 82 ){ this->absolute_mode == true; }
  70         if( code == 83 ){ this->absolute_mode == false; }
  71     }
  72
  73     if( gcode->has_letter('G') ){
  74
  75         if( gcode->get_value('G') == 92 ){
  76
  77             if( gcode->has_letter('E') ){
  78                 this->current_position = gcode->get_value('E');
  79                 this->target_position  = this->current_position;
  80                 this->start_position   = this->current_position;
  81             }
  82
  83         }else{
  84
  85             // Extrusion length
  86             if( gcode->has_letter('E' )){
  87                 double extrusion_distance = gcode->get_value('E');
  88                 if( fabs(gcode->millimeters_of_travel) < 0.0001 ){
  89                     this->solo_mode = true;
  90                     this->travel_distance = extrusion_distance;
  91                     this->travel_ratio = 0.0;
  92                 }else{
  93                     this->solo_mode = false;
  94                     this->travel_ratio = extrusion_distance / gcode->millimeters_of_travel;
  95                     this->travel_distance = 0.0;
  96                 }
  97             // Else do not extrude
  98             }else{
  99                 this->travel_ratio = 0.0;
 100                 this->travel_distance = 0.0;
 101             }
 102
 103         }
 104
 105     }
 106
 107 }
 108
 109
 110
 111 void Extruder::on_block_begin(void* argument){
 112     Block* block = static_cast<Block*>(argument);
 113
 114     if( fabs(this->travel_distance) > 0.001 ){
 115         block->take(); // In solo mode we take the block so we can move even if the stepper has nothing to do
 116         this->current_block = block;
 117         this->start_position = this->target_position;
 118         this->target_position = ( !this->absolute_mode ? this->start_position : 0 ) + this->travel_distance ;
 119         //this->acceleration_tick();
 120     }
 121     if( fabs(this->travel_ratio) > 0.001 ){
 122         // In non-solo mode, we just follow the stepper module
 123         this->current_block = block;
 124         this->start_position = this->target_position;
 125         this->target_position =  this->start_position + ( this->current_block->millimeters * this->travel_ratio );
 126         //this->acceleration_tick();
 127     }
 128
 129
 130 }
 131
 132 void Extruder::on_block_end(void* argument){
 133     Block* block = static_cast<Block*>(argument);
 134     this->current_block = NULL;
 135 }
 136
 137 void Extruder::acceleration_tick(){
 138
 139     // If we are currently taking care of a block
 140     if( this->current_block ){
 141
 142         this->kernel->serial->printf("tick start_position:%f current_position:%f target_position:%f travel_distance:%f travel_ratio:%f mm:%f \r\n", this->start_position, this->current_position, this->target_position, this->travel_distance, this->travel_ratio, this->current_block->millimeters);
 143
 144         if( fabs(this->travel_ratio) > 0.001 ){
 145             double steps_by_acceleration_tick = this->kernel->stepper->trapezoid_adjusted_rate / 60 / this->kernel->stepper->acceleration_ticks_per_second;
 146             // Get position along the block at next tick
 147             if( this->kernel->stepper->current_block != NULL ){
 148                 this->kernel->stepper->current_block->debug( this->kernel );
 149             }else{
 150                 this->kernel->serial->printf("NULL\r\n");
 151                 return;
 152             }
 153             double current_position_ratio = double( (this->kernel->stepper->step_events_completed>>16)                              ) / double(this->kernel->stepper->current_block->steps_event_count);
 154             double next_position_ratio =    double( (this->kernel->stepper->step_events_completed>>16) + steps_by_acceleration_tick ) / double(this->kernel->stepper->current_block->steps_event_count);
 155             // Ratio differenc
 156             double ratio_difference = next_position_ratio - current_position_ratio;
 157             // Get total move distance
 158             double distance = this->target_position - this->start_position;
 159             // Get distance to run until next acceleration tick
 160             double desired_distance = distance / ratio_difference;
 161             // Get speed
 162             double desired_speed = desired_distance * double(this->kernel->stepper->acceleration_ticks_per_second); //TODO : Replace with the actual current_position
 163
 164             this->kernel->serial->printf("cur_pos:%f tar_pos:%f start_pos:%f steps_by_acc_tick:%f cur_pos_ratio:%f next_pos_ratio:%f ratio_diff:%f dist:%f desired_dist:%f desired_spd:%f \r\n", this->current_position, this->target_position, this->start_position,  steps_by_acceleration_tick, current_position_ratio, next_position_ratio, ratio_difference, ratio_difference, distance, desired_distance, desired_speed );
 165
 166
 167             if( desired_speed <= 0 ){ return; }
 168
 169             // Set timer
 170             LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(desired_speed*201));
 171
 172             // In case we are trying to set the timer to a limit it has already past by
 173             if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
 174                 LPC_TIM1->TCR = 3;
 175                 LPC_TIM1->TCR = 1;
 176             }
 177
 178             // Update Timer1
 179             LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
 180
 181         }
 182
 183         if( fabs(this->travel_distance) > 0.001 ){
 184
 185             // Set timer
 186             LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(200));
 187
 188             // In case we are trying to set the timer to a limit it has already past by
 189             if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
 190                 LPC_TIM1->TCR = 3;
 191                 LPC_TIM1->TCR = 1;
 192             }
 193
 194             // Update Timer1
 195             LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
 196
 197         }
 198
 199     }
 200 }
 201
 202 inline void Extruder::stepping_tick(){
 203     if( this->current_position < this->target_position ){
 204         this->current_position += double(double(1)/double(201));
 205         this->step_pin = 1;
 206     }else{
 207         // Move finished
 208         if( fabs(this->travel_distance) > 0.001 && this->current_block != NULL ){
 209             this->current_block->release();
 210         }
 211     }
 212 }
 213
 214
 215