various bug fixes

[clinton/Smoothieware.git] / src / modules / tools / extruder / Extruder.cpp

diff --git a/src/modules/tools/extruder/Extruder.cpp b/src/modules/tools/extruder/Extruder.cpp
index 57d4b3e..5e0c37e 100644 (file)
--- a/src/modules/tools/extruder/Extruder.cpp
+++ b/src/modules/tools/extruder/Extruder.cpp
@@ -18,11 +18,13 @@ extern "C" void TIMER1_IRQHandler (void){
     }
 }
 
-Extruder::Extruder(PinName stppin) : step_pin(stppin){}
+Extruder::Extruder(PinName stppin) : step_pin(stppin){
+    this->absolute_mode = true;
+}
 
 void Extruder::on_module_loaded() {
 
-    if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() ){ return; } 
+    if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; } 
 
     extruder_for_irq = this;
 
@@ -32,6 +34,7 @@ void Extruder::on_module_loaded() {
     // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one
     this->register_for_event(ON_BLOCK_BEGIN);
     this->register_for_event(ON_BLOCK_END);
+    this->register_for_event(ON_GCODE_EXECUTE);
 
     // Configuration
     this->acceleration_ticker.attach_us(this, &Extruder::acceleration_tick, 1000000/this->kernel->stepper->acceleration_ticks_per_second);
@@ -50,14 +53,47 @@ void Extruder::on_module_loaded() {
 }
 
 void Extruder::on_config_reload(void* argument){
-    this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number();
+    this->microseconds_per_step_pulse = 5; //this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number();
+}
+
+// Computer extrusion speed based on parameters and gcode distance of travel
+void Extruder::on_gcode_execute(void* argument){
+    Gcode* gcode = static_cast<Gcode*>(argument);
+    
+    // Absolute/relative mode
+    if( gcode->has_letter('M')){
+        int code = gcode->get_value('M');
+        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 );
+        }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;
+    }
 }
 
+
+
 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 * 159 ); //TODO : Get from config ( extruder_steps_per_mm )
+    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();
 }
 
 void Extruder::on_block_end(void* argument){
@@ -70,14 +106,19 @@ void Extruder::acceleration_tick(){
     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 next_position_ratio = ( this->kernel->stepper->step_events_completed + steps_by_acceleration_tick ) / this->kernel->stepper->current_block->steps_event_count;
+        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) * 60L; //TODO : Replace with the actual current_position
+        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)*60)/int(floor(desired_speed)); 
+        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 ){