Fix a bug where a move could result in zero steps for all axis

diff --git a/src/modules/robot/Conveyor.cpp b/src/modules/robot/Conveyor.cpp
index db9015c..8af3dd1 100644 (file)
--- a/src/modules/robot/Conveyor.cpp
+++ b/src/modules/robot/Conveyor.cpp
@@ -110,7 +110,7 @@ void Conveyor::on_idle(void*)
         } else {
             // Cleanly delete block
             Block* block = queue.tail_ref();
-            //block->debug();
+            block->debug();
             block->clear();
             queue.consume_tail();
         }

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index bc47572..c4711fa 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -50,24 +50,30 @@ void Planner::config_load()
 
 
 // Append a block to the queue, compute it's speed factors
-void Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration)
+bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration)
 {
     // Create ( recycle ) a new block
     Block* block = THECONVEYOR->queue.head_ref();
 
     // Direction bits
+    bool has_steps= false;
     for (size_t i = 0; i < n_motors; i++) {
         int32_t steps = THEROBOT->actuators[i]->steps_to_target(actuator_pos[i]);
         // Update current position
-        THEROBOT->actuators[i]->update_last_milestones(actuator_pos[i], steps);
+        if(steps != 0) {
+            THEROBOT->actuators[i]->update_last_milestones(actuator_pos[i], steps);
+            has_steps= true;
+        }
 
         // find direction
         block->direction_bits[i] = (steps < 0) ? 1 : 0;
-
         // save actual steps in block
         block->steps[i] = labs(steps);
     }
 
+    // sometimres even though there is a detectable movement it turns out there are no steps to be had from such a small move
+    if(!has_steps) return false;
+
     // use default JD
     float junction_deviation = this->junction_deviation;
 
@@ -177,6 +183,8 @@ void Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
     block->ready();
 
     THECONVEYOR->queue_head_block();
+
+    return true;
 }
 
 void Planner::recalculate()

diff --git a/src/modules/robot/Planner.h b/src/modules/robot/Planner.h
index cfa27d7..ee43057 100644 (file)
--- a/src/modules/robot/Planner.h
+++ b/src/modules/robot/Planner.h
@@ -20,7 +20,7 @@ public:
     friend class Robot; // for acceleration, junction deviation, minimum_planner_speed
 
 private:
-    void append_block(ActuatorCoordinates &target, uint8_t n_motors, float rate_mm_s, float distance, float unit_vec[], float accleration);
+    bool append_block(ActuatorCoordinates &target, uint8_t n_motors, float rate_mm_s, float distance, float unit_vec[], float accleration);
     void recalculate();
     void config_load();
     float previous_unit_vec[3];

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 1f93201..65847cb 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -992,9 +992,6 @@ bool Robot::append_milestone(const float target[], float rate_mm_s, bool disable
     }
 #endif
 
-    // this is the machine position, we update here as we have detected a move
-    memcpy(this->last_machine_position, transformed_target, n_motors*sizeof(float));
-
     // use default acceleration to start with
     float acceleration = default_acceleration;
 
@@ -1026,7 +1023,11 @@ bool Robot::append_milestone(const float target[], float rate_mm_s, bool disable
 
     // Append the block to the planner
     // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
-    THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration );
+    if(THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration )) {
+        // this is the machine position
+        memcpy(this->last_machine_position, transformed_target, n_motors*sizeof(float));
+    }
+
 
     return true;
 }