halfway thru the Extruder functionality

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index 83ca42b..d105dc3 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -185,9 +185,14 @@ void Block::release(){
     if( this->times_taken < 1 ){
         this->player->kernel->call_event(ON_BLOCK_END, this);
         this->pop_and_execute_gcode(this->player->kernel);
-        Player* player = this->player; 
-        player->queue.delete_first();
-        player->pop_and_process_new_block(); 
+        Player* player = this->player;
+        if( player->queue.size() > 0 ){ 
+            //player->kernel->serial->printf("before: %d\r\n", player->queue.size() );
+            player->queue.delete_first();
+        } 
+        //player->kernel->serial->printf("after: %d\r\n", player->queue.size() );
+        player->current_block = NULL; 
+        player->pop_and_process_new_block();
     }
 }
 

diff --git a/src/modules/robot/Stepper.cpp b/src/modules/robot/Stepper.cpp
index b620ad7..95ff009 100644 (file)
--- a/src/modules/robot/Stepper.cpp
+++ b/src/modules/robot/Stepper.cpp
@@ -99,26 +99,26 @@ void Stepper::on_block_begin(void* argument){
 
     // The stepper does not care about 0-blocks
     if( block->millimeters == 0.0 ){ return; }
-
+    
     this->current_block = block;
 
     // Mark the new block as of interrest to us
-    this->current_block->take();
+    block->take();
 
     // Setup
-    this->trapezoid_generator_reset();
-    this->update_offsets();
     for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; } 
     this->step_events_completed = 0; 
 
+    this->current_block = block;
+
+    this->update_offsets();
+    this->trapezoid_generator_reset();
 }
 
 // Current block is discarded
 void Stepper::on_block_end(void* argument){
     Block* block  = static_cast<Block*>(argument);
-    if( this->current_block != NULL ){
-        this->current_block == NULL; //stfu !
-    }
+    this->current_block = NULL; //stfu !
 }
 
 // Timer0 ISR
@@ -168,7 +168,9 @@ inline void Stepper::main_interrupt(){
         this->step_events_completed += this->counter_increment;
         if( this->step_events_completed >= this->current_block->steps_event_count<<16 ){ 
             if( this->stepped[ALPHA_STEPPER] == this->current_block->steps[ALPHA_STEPPER] && this->stepped[BETA_STEPPER] == this->current_block->steps[BETA_STEPPER] && this->stepped[GAMMA_STEPPER] == this->current_block->steps[GAMMA_STEPPER] ){ 
-                this->current_block->release(); 
+                if( this->current_block != NULL ){
+                    this->current_block->release(); 
+                }
             }
         }
 

diff --git a/src/modules/tools/extruder/Extruder.cpp b/src/modules/tools/extruder/Extruder.cpp
index a613131..2fef76b 100644 (file)
--- a/src/modules/tools/extruder/Extruder.cpp
+++ b/src/modules/tools/extruder/Extruder.cpp
@@ -67,34 +67,69 @@ void Extruder::on_gcode_execute(void* argument){
         if( code == 82 ){ this->absolute_mode == true; }
         if( code == 83 ){ this->absolute_mode == false; }
     } 
-    
-    // Extrusion length 
-    if( gcode->has_letter('E' )){
-        double extrusion_distance = gcode->get_value('E');
-        //this->kernel->serial->printf("extrusion_distance: %f, millimeters_of_travel: %f\r\n", extrusion_distance, gcode->millimeters_of_travel);
-        if( gcode->millimeters_of_travel == 0.0 ){
-            this->solo_mode = true;
-            this->travel_distance = extrusion_distance;
-            //this->kernel->serial->printf("solo mode distance: %f\r\n", this->travel_distance );
+   
+    if( gcode->has_letter('G') ){
+
+        if( gcode->get_value('G') == 92 ){
+
+            if( gcode->has_letter('E') ){
+                this->current_position = gcode->get_value('E') * 100;
+                this->target_position  = this->current_position;
+                this->start_position   = this->current_position; 
+            }            
+
         }else{
-            this->solo_mode = false;
-            this->travel_ratio = extrusion_distance / gcode->millimeters_of_travel; 
-            //this->kernel->serial->printf("follow mode ratio: %f\r\n", this->travel_ratio);
-        } 
-    }else{
-        this->travel_ratio = 0;
-    }
+
+            // Extrusion length 
+            if( gcode->has_letter('E' )){
+                double extrusion_distance = gcode->get_value('E');
+                //this->kernel->serial->printf("extrusion_distance: %f, millimeters_of_travel: %f\r\n", extrusion_distance, gcode->millimeters_of_travel);
+                if( gcode->millimeters_of_travel == 0.0 ){
+                    this->solo_mode = true;
+                    this->travel_distance = extrusion_distance;
+                    //this->kernel->serial->printf("solo mode distance: %f\r\n", this->travel_distance );
+                }else{
+                    this->solo_mode = false;
+                    this->travel_ratio = extrusion_distance / gcode->millimeters_of_travel; 
+                    //this->kernel->serial->printf("follow mode ratio: %f\r\n", this->travel_ratio);
+                } 
+            // Else do not extrude
+            }else{
+                this->travel_ratio = 0.0;
+                this->travel_distance = 0.0;
+            }
+
+        }
+ 
+    }    
+    
 }
 
 
 
 void Extruder::on_block_begin(void* argument){
     Block* block = static_cast<Block*>(argument);
-    this->current_block = block; 
-    //this->kernel->serial->printf("block: solomode: %d, travel_distance: %f, travel_ratio: %f \r\n", this->solo_mode, this->travel_distance, this->travel_ratio);
-    this->start_position = this->current_position;
-    this->target_position = this->start_position + ( this->current_block->millimeters * this->travel_ratio * 159 ); //TODO : Get from config ( extruder_steps_per_mm )
-    this->acceleration_tick();
+
+    if( fabs(this->travel_distance) > 0.001 ){
+
+        block->take(); // In solo mode we take the block so we can move even if the stepper has nothing to do
+        this->current_block = block; 
+        this->start_position = this->current_position;
+        this->target_position = ( !this->absolute_mode ? this->start_position : 0 ) + ( this->travel_distance * 100 ); //TODO : Get from config ( extruder_steps_per_mm )
+        this->acceleration_tick();
+        //this->kernel->serial->printf("distance %f %f\r\n", this->travel_distance, this->travel_ratio);
+    }   
+    if( fabs(this->travel_ratio) > 0.001 ){
+        
+        // In non-solo mode, we just follow the stepper module
+        this->current_block = block; 
+        this->start_position = this->current_position;
+        this->target_position =  ( !this->absolute_mode ? this->start_position : 0 ) + ( this->current_block->millimeters * this->travel_ratio * 100 ); //TODO : Get from config ( extruder_steps_per_mm )
+        this->acceleration_tick();
+        //this->kernel->serial->printf("ratio %f %f %d\r\n", this->travel_distance, this->travel_ratio, block->times_taken );
+
+    } 
+    
 }
 
 void Extruder::on_block_end(void* argument){
@@ -103,32 +138,52 @@ void Extruder::on_block_end(void* argument){
 }       
 
 void Extruder::acceleration_tick(){
+    
     // If we are currently taking care of a block            
     if( this->current_block ){
-        double steps_by_acceleration_tick = this->kernel->stepper->trapezoid_adjusted_rate / 60 / this->kernel->stepper->acceleration_ticks_per_second;                
-        // Get position along the block at next tick
-        double current_position_ratio = double( (this->kernel->stepper->step_events_completed>>16) ) / double(this->kernel->stepper->current_block->steps_event_count);
-        double next_position_ratio = ( (this->kernel->stepper->step_events_completed>>16) + steps_by_acceleration_tick ) / this->kernel->stepper->current_block->steps_event_count;
-        // Get wanted next position
-        double next_absolute_position = ( this->target_position - this->start_position ) * next_position_ratio;
-        // Get desired  speed in steps per minute to get to the next position by the next acceleration tick
-        double desired_speed = ( ( next_absolute_position + this->start_position ) - this->current_position ) * double(this->kernel->stepper->acceleration_ticks_per_second); //TODO : Replace with the actual current_position
-
-        //this->kernel->serial->printf("stp->sec: %d, stp->cb->secnt: %u, sbat: %.4f, npr: %.4f, ds: %f --> tap: %u, nap: %f, sp: %u, cp: %u, (tp-sp): %d, cpr: %f\r\n", this->kernel->stepper->step_events_completed>>16, this->kernel->stepper->current_block->steps_event_count,  steps_by_acceleration_tick, next_position_ratio, desired_speed,this->target_position,  next_absolute_position, this->start_position, this->current_position, int(this->target_position-this->start_position), current_position_ratio );
-
-        if( desired_speed <= 0 ){ return; }
-
-        // Set timer
-        LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(desired_speed)); 
-
-        // In case we are trying to set the timer to a limit it has already past by
-        if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
-            LPC_TIM1->TCR = 3; 
-            LPC_TIM1->TCR = 1; 
+
+        if( fabs(this->travel_ratio) > 0.001 ){
+            double steps_by_acceleration_tick = this->kernel->stepper->trapezoid_adjusted_rate / 60 / this->kernel->stepper->acceleration_ticks_per_second;                
+            // Get position along the block at next tick
+            double current_position_ratio = double( (this->kernel->stepper->step_events_completed>>16) ) / double(this->kernel->stepper->current_block->steps_event_count);
+            double next_position_ratio = ( (this->kernel->stepper->step_events_completed>>16) + steps_by_acceleration_tick ) / this->kernel->stepper->current_block->steps_event_count;
+            // Get wanted next position
+            double next_absolute_position = ( this->target_position - this->start_position ) * next_position_ratio;
+            // Get desired  speed in steps per minute to get to the next position by the next acceleration tick
+            double desired_speed = ( ( next_absolute_position + this->start_position ) - this->current_position ) * double(this->kernel->stepper->acceleration_ticks_per_second); //TODO : Replace with the actual current_position
+
+            if( desired_speed <= 0 ){ return; }
+
+            // Set timer
+            LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(desired_speed)); 
+
+            // In case we are trying to set the timer to a limit it has already past by
+            if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
+                LPC_TIM1->TCR = 3; 
+                LPC_TIM1->TCR = 1; 
+            }
+
+            // Update Timer1    
+            LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
+
+        }
+
+        if( fabs(this->travel_distance) > 0.001 ){
+
+            // Set timer
+            LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(300)); 
+
+            // In case we are trying to set the timer to a limit it has already past by
+            if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
+                LPC_TIM1->TCR = 3; 
+                LPC_TIM1->TCR = 1; 
+            }
+
+            // Update Timer1    
+            LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
+
         }
 
-        // Update Timer1    
-        LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
     }
 }
 
@@ -137,9 +192,10 @@ inline void Extruder::stepping_tick(){
         this->current_position++;
         this->step_pin = 1;
     }else{
-        if( this->current_block == NULL ){
-            //this->stepping_ticker.detach();
-        }
+        // Move finished
+        if( fabs(this->travel_distance) > 0.001 && this->current_block != NULL ){
+            this->current_block->release();        
+        } 
     }
 }