Extruder should ignore M114 subcodes > 0

[clinton/Smoothieware.git] / src / modules / robot / Stepper.cpp

diff --git a/src/modules/robot/Stepper.cpp b/src/modules/robot/Stepper.cpp
index 40c807f..b4760bd 100644 (file)
--- a/src/modules/robot/Stepper.cpp
+++ b/src/modules/robot/Stepper.cpp
@@ -55,9 +55,8 @@ void Stepper::on_module_loaded()
     THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); });
 
     // Attach to the end_of_move stepper event
-    THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
-    THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move );
-    THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
+    for (auto actuator : THEKERNEL->robot->actuators)
+        actuator->attach(this, &Stepper::stepper_motor_finished_move );
 }
 
 // Get configuration from the config file
@@ -104,8 +103,8 @@ void Stepper::on_gcode_execute(void *argument)
 // Enable steppers
 void Stepper::turn_enable_pins_on()
 {
-    for (StepperMotor *m : THEKERNEL->robot->actuators)
-        m->enable(true);
+    for (auto a : THEKERNEL->robot->actuators)
+        a->enable(true);
     this->enable_pins_status = true;
     THEKERNEL->call_event(ON_ENABLE, (void*)1);
 }
@@ -113,8 +112,8 @@ void Stepper::turn_enable_pins_on()
 // Disable steppers
 void Stepper::turn_enable_pins_off()
 {
-    for (StepperMotor *m : THEKERNEL->robot->actuators)
-        m->enable(false);
+    for (auto a : THEKERNEL->robot->actuators)
+        a->enable(false);
     this->enable_pins_status = false;
     THEKERNEL->call_event(ON_ENABLE, nullptr);
 }
@@ -125,9 +124,14 @@ void Stepper::on_block_begin(void *argument)
     Block *block  = static_cast<Block *>(argument);
 
     // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc)
-    if(block->millimeters > 0.0F && (block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0) ) {
+    bool take = false;
+    if (block->millimeters > 0.0F) {
+        for (size_t s = 0; !take && s < THEKERNEL->robot->actuators.size(); s++) {
+            take = block->steps[s] > 0;
+        }
+    }
+    if (take){
         block->take();
-
     } else {
         // none of the steppers move this block so make sure they know that
         for(auto a : THEKERNEL->robot->actuators) {
@@ -143,28 +147,20 @@ void Stepper::on_block_begin(void *argument)
 
     // Setup : instruct stepper motors to move
     // Find the stepper with the more steps, it's the one the speed calculations will want to follow
-    this->main_stepper= nullptr;
-    if( block->steps[ALPHA_STEPPER] > 0 ) {
-        THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER])->set_moved_last_block(true);
-        this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
-    }else{
-        THEKERNEL->robot->alpha_stepper_motor->set_moved_last_block(false);
-    }
-
-    if( block->steps[BETA_STEPPER ] > 0 ) {
-        THEKERNEL->robot->beta_stepper_motor->move(  block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ])->set_moved_last_block(true);
-        if(this->main_stepper == nullptr || THEKERNEL->robot->beta_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move())
-            this->main_stepper = THEKERNEL->robot->beta_stepper_motor;
-    }else{
-        THEKERNEL->robot->beta_stepper_motor->set_moved_last_block(false);
-    }
-
-    if( block->steps[GAMMA_STEPPER] > 0 ) {
-        THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER])->set_moved_last_block(true);
-        if(this->main_stepper == nullptr || THEKERNEL->robot->gamma_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move())
-            this->main_stepper = THEKERNEL->robot->gamma_stepper_motor;
-    }else{
-        THEKERNEL->robot->gamma_stepper_motor->set_moved_last_block(false);
+    this->main_stepper = nullptr;
+    int most_steps_to_move = 0;
+    for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
+        if (block->steps[i] > 0) {
+            THEKERNEL->robot->actuators[i]->move(block->direction_bits[i], block->steps[i])->set_moved_last_block(true);
+            int steps_to_move = THEKERNEL->robot->actuators[i]->get_steps_to_move();
+            if (steps_to_move > most_steps_to_move) {
+                most_steps_to_move = steps_to_move;
+                this->main_stepper = THEKERNEL->robot->actuators[i];
+            }
+        }
+        else {
+            THEKERNEL->robot->actuators[i]->set_moved_last_block(false);
+        }
     }
 
     this->current_block = block;
@@ -194,8 +190,9 @@ void Stepper::on_block_end(void *argument)
 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy)
 {
     // We care only if none is still moving
-    if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ) {
-        return 0;
+    for (auto a : THEKERNEL->robot->actuators) {
+        if(a->moving)    
+            return 0;
     }
 
     // This block is finished, release it
@@ -289,14 +286,10 @@ void Stepper::set_step_events_per_second( float steps_per_second )
     float isps= steps_per_second / this->current_block->steps_event_count;
 
     // Instruct the stepper motors
-    if( THEKERNEL->robot->alpha_stepper_motor->moving ) {
-        THEKERNEL->robot->alpha_stepper_motor->set_speed(isps * this->current_block->steps[ALPHA_STEPPER]);
-    }
-    if( THEKERNEL->robot->beta_stepper_motor->moving  ) {
-        THEKERNEL->robot->beta_stepper_motor->set_speed(isps * this->current_block->steps[BETA_STEPPER]);
-    }
-    if( THEKERNEL->robot->gamma_stepper_motor->moving ) {
-        THEKERNEL->robot->gamma_stepper_motor->set_speed(isps * this->current_block->steps[GAMMA_STEPPER]);
+    for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
+        if (THEKERNEL->robot->actuators[i]->moving) {
+            THEKERNEL->robot->actuators[i]->set_speed(isps * this->current_block->steps[i]);
+        }
     }
 
     // Other modules might want to know the speed changed