Change Conveyor,Planner,Block to use HeapRing

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index dd5ed79..f24f82f 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -235,22 +235,25 @@ void Block::release()
         // We would normally delete this block directly here, but we can't, because this is interrupt context, no crazy memory stuff here
         // So instead we increment a counter, and it will be deleted in main loop context
         Conveyor *conveyor = THEKERNEL->conveyor;
-        if( conveyor->queue.size() > conveyor->flush_blocks ) {
-            conveyor->flush_blocks++;
-        }
+
+        unsigned int n = conveyor->queue.next(conveyor->gc_pending);
+        if (n != conveyor->queue.head_i)
+            conveyor->gc_pending = n;
 
         // We don't look for the next block to execute if the conveyor is already doing that itself
         if( conveyor->looking_for_new_block == false ) {
 
             // If there are still blocks to execute
-            if( conveyor->queue.size() > conveyor->flush_blocks ) {
-                Block *candidate =  conveyor->queue.get_ref(conveyor->flush_blocks);
+            if (conveyor->gc_pending != conveyor->queue.head_i)
+            {
+                Block *candidate =  conveyor->queue.item_ref(conveyor->gc_pending);
 
                 // We only execute blocks that are ready ( their math is done )
                 if( candidate->is_ready ) {
 
                     // Execute this candidate
                     conveyor->current_block = candidate;
+                    candidate->recalculate_flag = false; // make sure planner doesn't alter this block while we're running it
                     THEKERNEL->call_event(ON_BLOCK_BEGIN, conveyor->current_block);
 
                     // If no module took this block, release it ourselves, as nothing else will do it otherwise

diff --git a/src/modules/robot/Conveyor.cpp b/src/modules/robot/Conveyor.cpp
index f91709f..0244906 100644 (file)
--- a/src/modules/robot/Conveyor.cpp
+++ b/src/modules/robot/Conveyor.cpp
@@ -23,7 +23,8 @@ using namespace std;
 Conveyor::Conveyor(){
     this->current_block = NULL;
     this->looking_for_new_block = false;
-    flush_blocks = 0;
+    queue.resize(32);
+    gc_pending = queue.tail_i;
 }
 
 void Conveyor::on_module_loaded(){
@@ -32,31 +33,29 @@ void Conveyor::on_module_loaded(){
 
 // Delete blocks here, because they can't be deleted in interrupt context ( see Block.cpp:release )
 void Conveyor::on_idle(void* argument){
-    while (flush_blocks > 0){
+    while (queue.tail_i != gc_pending)
+    {
         // Cleanly delete block
-        Block* block = queue.get_tail_ref();
+        Block* block = queue.tail_ref();
         block->gcodes.clear();
-        queue.delete_tail();
-        __disable_irq();
-        flush_blocks--;
-        __enable_irq();
+        queue.consume_tail();
     }
 }
 
 void Conveyor::append_gcode(Gcode* gcode)
 {
     gcode->mark_as_taken();
-    if (queue.size() == 0)
+    if (queue.is_empty())
         THEKERNEL->call_event(ON_GCODE_EXECUTE, gcode);
     else
-        queue.get_ref(queue.size() - 1)->append_gcode(gcode);
+        queue.head_ref()->append_gcode(gcode);
 }
 
 // Append a block to the list
 Block* Conveyor::new_block(){
 
     // Take the next untaken block on the queue ( the one after the last one )
-    Block* block = this->queue.get_head_ref();
+    Block* block = this->queue.head_ref();
     // Then clean it up
     block->clear();
 
@@ -64,7 +63,7 @@ Block* Conveyor::new_block(){
     block->final_rate = -2;
 
     // Create a new virgin Block in the queue
-    this->queue.push_back(Block());
+    queue.produce_head();
 
     return block;
 }
@@ -84,7 +83,7 @@ void Conveyor::pop_and_process_new_block(int debug){
     if( this->current_block != NULL ){ this->looking_for_new_block = false; return; }
 
     // Return if queue is empty
-    if( this->queue.size() == 0 ){
+    if( queue.is_empty() ){
         this->current_block = NULL;
         // TODO : ON_QUEUE_EMPTY event
         this->looking_for_new_block = false;
@@ -92,7 +91,7 @@ void Conveyor::pop_and_process_new_block(int debug){
     }
 
     // Get a new block
-    this->current_block = this->queue.get_ref(0);
+    this->current_block = this->queue.tail_ref();
 
     // Tell all modules about it
     THEKERNEL->call_event(ON_BLOCK_BEGIN, this->current_block);
@@ -110,21 +109,26 @@ void Conveyor::pop_and_process_new_block(int debug){
 }
 
 // Wait for the queue to have a given number of free blocks
-void Conveyor::wait_for_queue(int free_blocks){
-    while( this->queue.size() >= this->queue.capacity()-free_blocks ){
+void Conveyor::wait_for_queue(int free_blocks)
+{
+    while (queue.is_full())
         THEKERNEL->call_event(ON_IDLE);
-    }
 }
 
 // Wait for the queue to be empty
-void Conveyor::wait_for_empty_queue(){
-    while( this->queue.size() > 0){
+void Conveyor::wait_for_empty_queue()
+{
+    while (!queue.is_empty())
         THEKERNEL->call_event(ON_IDLE);
-    }
 }
 
 // Return true if the queue is empty
-bool Conveyor::is_queue_empty(){
-    return (this->queue.size() == 0);
+bool Conveyor::is_queue_empty()
+{
+    return queue.is_empty();
 }
 
+
+// feels hacky, but apparently the way to do it
+#include "HeapRing.cpp"
+template class HeapRing<Block>;

diff --git a/src/modules/robot/Conveyor.h b/src/modules/robot/Conveyor.h
index 0ef79ce..267938d 100644 (file)
--- a/src/modules/robot/Conveyor.h
+++ b/src/modules/robot/Conveyor.h
@@ -10,6 +10,7 @@
 
 #include "libs/Module.h"
 #include "libs/Kernel.h"
+#include "HeapRing.h"
 
 using namespace std;
 #include <string>
@@ -35,12 +36,14 @@ public:
     void append_gcode(Gcode*);
 
     // right now block queue size can only be changed at compile time by changing the value below
-    typedef RingBuffer<Block, 32> BlockQueue_t;
-    BlockQueue_t queue;  // Queue of Blocks
+    typedef HeapRing<Block> Queue_t;
+
+    Queue_t queue;  // Queue of Blocks
+
     Block *current_block;
     bool looking_for_new_block;
 
-    volatile int flush_blocks;
+    volatile unsigned int gc_pending;
 };
 
 #endif // CONVEYOR_H

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index bf38b95..a2ea42c 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -109,7 +109,7 @@ void Planner::append_block( int target[], float feed_rate, float distance, float
     // nonlinearities of both the junction angle and junction velocity.
     float vmax_junction = minimum_planner_speed; // Set default max junction speed
 
-    if (THEKERNEL->conveyor->queue.size() > 1 && (this->previous_nominal_speed > 0.0F)) {
+    if ((THEKERNEL->conveyor->queue.is_empty() == false) && (this->previous_nominal_speed > 0.0F)) {
       // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
       // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
       float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@@ -179,18 +179,18 @@ void Planner::append_block( int target[], float feed_rate, float distance, float
 // 3. Recalculate trapezoids for all blocks.
 //
 void Planner::recalculate() {
-    Conveyor::BlockQueue_t *queue = &THEKERNEL->conveyor->queue;
+    Conveyor::Queue_t *queue = &THEKERNEL->conveyor->queue;
 
-    int newest = queue->prev_block_index(queue->head);
-    int oldest = queue->tail;
+    unsigned int newest = queue->prev(queue->head_i);
+    unsigned int oldest = queue->tail_i;
 
-    int block_index = newest;
+    unsigned int block_index = newest;
 
     Block* previous;
     Block* current;
     Block* next;
 
-    current = &queue->buffer[block_index];
+    current = queue->item_ref(block_index);
     current->recalculate_flag = true;
 
     // if there's only one block in the queue, we fall through both while loops and this ends up in current
@@ -199,10 +199,10 @@ void Planner::recalculate() {
 
     while ((block_index != oldest) && (current->recalculate_flag))
     {
-        block_index = queue->prev_block_index(block_index);
+        block_index = queue->prev(block_index);
 
         next = current;
-        current = &queue->buffer[block_index];
+        current = queue->item_ref(block_index);
 
         current->recalculate_flag = false;
 
@@ -228,9 +228,9 @@ void Planner::recalculate() {
             previous->recalculate_flag = false;
         }
 
-        block_index = queue->next_block_index(block_index);
+        block_index = queue->next(block_index);
         previous = current;
-        current = &queue->buffer[block_index];
+        current = queue->item_ref(block_index);
     }
 
     // Last/newest block in buffer. Exit speed is set with minimum_planner_speed. Always recalculated.
@@ -239,11 +239,13 @@ void Planner::recalculate() {
 }
 
 // Debug function
-void Planner::dump_queue(){
-    for( int index = 0; index <= THEKERNEL->conveyor->queue.size()-1; index++ ){
+void Planner::dump_queue()
+{
+    for (unsigned int index = THEKERNEL->conveyor->queue.tail_i, i = 0; index != THEKERNEL->conveyor->queue.head_i; index = THEKERNEL->conveyor->queue.next(index), i++ )
+    {
        //if( index > 10 && index < THEKERNEL->conveyor->queue.size()-10 ){ continue; }
-       THEKERNEL->streams->printf("block %03d > ", index);
-       THEKERNEL->conveyor->queue.get_ref(index)->debug();
+       THEKERNEL->streams->printf("block %03d > ", i);
+       THEKERNEL->conveyor->queue.item_ref(index)->debug();
     }
 }