Revert "Merge branch 'fix/Queue_scan_consolidation' into edge"

This reverts commit b4f9b9b5b4186df5a681dc63cd4174c543cd41d7, reversing
changes made to d97f94e3d8782e77793011430bb38784640b1f6f.

needs more work

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index 2cfe9a5..2cfa1d4 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -106,7 +106,7 @@ inline double max_allowable_speed(double acceleration, double target_velocity, d
 
 
 // Called by Planner::recalculate() when scanning the plan from last to first entry.
-void Block::reverse_pass(Block* next){
+void Block::reverse_pass(Block* next, Block* previous){
 
     if (next) {
         // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
@@ -130,7 +130,7 @@ void Block::reverse_pass(Block* next){
 
 
 // Called by Planner::recalculate() when scanning the plan from first to last entry.
-void Block::forward_pass(Block* previous){
+void Block::forward_pass(Block* previous, Block* next){
 
     if(!previous) { return; } // Begin planning after buffer_tail
 

diff --git a/src/modules/robot/Block.h b/src/modules/robot/Block.h
index 3dc6293..8e217ec 100644 (file)
--- a/src/modules/robot/Block.h
+++ b/src/modules/robot/Block.h
@@ -26,8 +26,8 @@ class Block {
         void calculate_trapezoid( double entry_factor, double exit_factor );
         double estimate_acceleration_distance( double initial_rate, double target_rate, double acceleration );
         double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance);
-        void reverse_pass(Block* previous);
-        void forward_pass(Block* next);
+        void reverse_pass(Block* previous, Block* next);
+        void forward_pass(Block* previous, Block* next);
         void debug(Kernel* kernel);
         void append_gcode(Gcode* gcode);
         void pop_and_execute_gcode(Kernel* &kernel);

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index 80c7487..da2fda8 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -173,54 +173,75 @@ void Planner::append_block( int target[], double feed_rate, double distance, dou
 // 3. Recalculate trapezoids for all blocks.
 //
 void Planner::recalculate() {
-    int block_index = this->kernel->conveyor->queue.head;
-
-    Block* previous;
-    Block* current;
-    Block* next;
-
-    current = &this->kernel->conveyor->queue.buffer[block_index];
-    current->recalculate_flag = true;
-
-    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];
+   this->reverse_pass();
+   this->forward_pass();
+   this->recalculate_trapezoids();
+}
 
-        current->recalculate_flag = false;
+// 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]);
+    }
+    
+}
 
-        current->reverse_pass(next);
+// 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_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);
-
-        // 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);
+// 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;
+            }
+        }
+        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.
-    current->calculate_trapezoid( current->entry_speed/current->nominal_speed, MINIMUM_PLANNER_SPEED/current->nominal_speed );
-    current->recalculate_flag = false;
+    next->calculate_trapezoid( next->entry_speed/next->nominal_speed, MINIMUM_PLANNER_SPEED/next->nominal_speed); //TODO: Make configuration option
+    next->recalculate_flag = false;
+
 }
 
 // Debug function

diff --git a/src/modules/robot/Planner.h b/src/modules/robot/Planner.h
index 2270a8e..dafb72b 100644 (file)
--- a/src/modules/robot/Planner.h
+++ b/src/modules/robot/Planner.h
@@ -28,6 +28,9 @@ 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();