Consolidate planner's block queue scan

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index da2fda8..09959ea 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -173,75 +173,54 @@ void Planner::append_block( int target[], double feed_rate, double distance, dou
 // 3. Recalculate trapezoids for all blocks.
 //
 void Planner::recalculate() {
-   this->reverse_pass();
-   this->forward_pass();
-   this->recalculate_trapezoids();
-}
-
-// Planner::recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
-// implements the reverse pass.
-void Planner::reverse_pass(){
-    // For each block
     int block_index = this->kernel->conveyor->queue.head;
-    Block* blocks[3] = {NULL,NULL,NULL};
-
-    while(block_index!=this->kernel->conveyor->queue.tail){
-        block_index = this->kernel->conveyor->queue.prev_block_index( block_index );
-        blocks[2] = blocks[1];
-        blocks[1] = blocks[0];
-        blocks[0] = &this->kernel->conveyor->queue.buffer[block_index];
-        if( blocks[1] == NULL ){ continue; }
-        blocks[1]->reverse_pass(blocks[2], blocks[0]);
-    }
-    
-}
 
-// Planner::recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
-// implements the forward pass.
-void Planner::forward_pass() {
-    // For each block
-    int block_index = this->kernel->conveyor->queue.tail;
-    Block* blocks[3] = {NULL,NULL,NULL};
-
-    while(block_index!=this->kernel->conveyor->queue.head){
-        blocks[0] = blocks[1];
-        blocks[1] = blocks[2];
-        blocks[2] = &this->kernel->conveyor->queue.buffer[block_index];
-        if( blocks[0] == NULL ){ continue; }
-        blocks[1]->forward_pass(blocks[0],blocks[2]);
-        block_index = this->kernel->conveyor->queue.next_block_index( block_index );
-    }
-    blocks[2]->forward_pass(blocks[1],NULL);
+    Block* previous;
+    Block* current;
+    Block* next;
 
-}
+    current = &this->kernel->conveyor->queue.buffer[block_index];
+    current->recalculate_flag = true;
 
-// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
-// entry_speed for each junction and the entry_speed of the next junction. Must be called by
-// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
-// compute the two adjacent trapezoids to the junction, since the junction speed corresponds
-// to exit speed and entry speed of one another.
-void Planner::recalculate_trapezoids() {
-    int block_index = this->kernel->conveyor->queue.tail;
-    Block* current;
-    Block* next = NULL;
-
-    while(block_index != this->kernel->conveyor->queue.head){
-        current = next;
-        next = &this->kernel->conveyor->queue.buffer[block_index];
-        if( current ){
-            // Recalculate if current block entry or exit junction speed has changed.
-            if( current->recalculate_flag || next->recalculate_flag ){
-                current->calculate_trapezoid( current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed );
-                current->recalculate_flag = false;
-            }
+    while ((block_index != this->kernel->conveyor->queue.tail) && (current->recalculate_flag))
+    {
+        block_index = this->kernel->conveyor->queue.prev_block_index(block_index);
+
+        next = current;
+        current = &this->kernel->conveyor->queue.buffer[block_index];
+
+        current->recalculate_flag = false;
+
+        current->reverse_pass(next, NULL);
+    }
+
+    block_index = this->kernel->conveyor->queue.next_block_index(block_index);
+
+    // Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
+    // entry_speed for each junction and the entry_speed of the next junction. Must be called by
+    // planner_recalculate() after updating the blocks. Any recalulate flagged junction will
+    // compute the two adjacent trapezoids to the junction, since the junction speed corresponds
+    // to exit speed and entry speed of one another.
+    while (block_index != this->kernel->conveyor->queue.head)
+    {
+        previous = current;
+        current = &this->kernel->conveyor->queue.buffer[block_index];
+
+        current->forward_pass(previous, NULL);
+
+        // Recalculate if current block entry or exit junction speed has changed.
+        if (previous->recalculate_flag || current->recalculate_flag )
+        {
+            previous->calculate_trapezoid( previous->entry_speed/previous->nominal_speed, current->entry_speed/previous->nominal_speed );
+            previous->recalculate_flag = false;
         }
-        block_index = this->kernel->conveyor->queue.next_block_index( block_index );
+
+        block_index = this->kernel->conveyor->queue.next_block_index(block_index);
     }
 
     // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
-    next->calculate_trapezoid( next->entry_speed/next->nominal_speed, MINIMUM_PLANNER_SPEED/next->nominal_speed); //TODO: Make configuration option
-    next->recalculate_flag = false;
-
+    current->calculate_trapezoid( current->entry_speed/current->nominal_speed, MINIMUM_PLANNER_SPEED/current->nominal_speed );
+    current->recalculate_flag = false;
 }
 
 // Debug function

diff --git a/src/modules/robot/Planner.h b/src/modules/robot/Planner.h
index dafb72b..2270a8e 100644 (file)
--- a/src/modules/robot/Planner.h
+++ b/src/modules/robot/Planner.h
@@ -28,9 +28,6 @@ class Planner : public Module {
         void append_block( int target[], double feed_rate, double distance, double deltas[] );
         double max_allowable_speed( double acceleration, double target_velocity, double distance);
         void recalculate();
-        void reverse_pass();
-        void forward_pass();
-        void recalculate_trapezoids();
         void dump_queue();
         Block* get_current_block();
         void cleanup_queue();