Block: don't update trapezoids, and return correct exit speed if block is currently...

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index 56bb343..f13f005 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -89,6 +89,9 @@ void Block::debug()
 */
 void Block::calculate_trapezoid( float entryspeed, float exitspeed )
 {
+    // if block is currently executing, don't touch anything!
+    if (times_taken)
+        return;
 
     // The planner passes us factors, we need to transform them in rates
     this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed);   // (step/min)
@@ -114,6 +117,7 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed )
     this->accelerate_until = accelerate_steps;
     this->decelerate_after = accelerate_steps + plateau_steps;
 
+    this->exit_speed = exitspeed;
 }
 
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
@@ -207,6 +211,11 @@ float Block::forward_pass(float prev_max_exit_speed)
 
 float Block::max_exit_speed()
 {
+    // if block is currently executing, return cached exit speed from calculate_trapezoid
+    // this ensures that a block following a currently executing block will have correct entry speed
+    if (times_taken)
+        return exit_speed;
+
     // if nominal_length_flag is asserted
     // we are guaranteed to reach nominal speed regardless of entry speed
     // thus, max exit will always be nominal

diff --git a/src/modules/robot/Block.h b/src/modules/robot/Block.h
index fb86b85..ce7b894 100644 (file)
--- a/src/modules/robot/Block.h
+++ b/src/modules/robot/Block.h
@@ -55,7 +55,8 @@ class Block {
         unsigned int   nominal_rate;       // Nominal rate in steps per minute
         float          nominal_speed;      // Nominal speed in mm per minute
         float          millimeters;        // Distance for this move
-        float         entry_speed;
+        float          entry_speed;
+        float          exit_speed;
         float          rate_delta;         // Nomber of steps to add to the speed for each acceleration tick
         unsigned int   initial_rate;       // Initial speed in steps per minute
         unsigned int   final_rate;         // Final speed in steps per minute