From c501670b6ac2e7529af69e00c1600e46a90e2c68 Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Thu, 16 Jan 2014 09:31:05 +1100 Subject: [PATCH] Change Conveyor,Planner,Block to use HeapRing --- src/modules/robot/Block.cpp | 13 ++++++---- src/modules/robot/Conveyor.cpp | 46 ++++++++++++++++++---------------- src/modules/robot/Conveyor.h | 9 ++++--- src/modules/robot/Planner.cpp | 30 +++++++++++----------- 4 files changed, 55 insertions(+), 43 deletions(-) diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp index dd5ed791..f24f82f5 100644 --- 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 f91709fd..02449060 100644 --- 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; diff --git a/src/modules/robot/Conveyor.h b/src/modules/robot/Conveyor.h index 0ef79ce2..267938d6 100644 --- 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 @@ -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 BlockQueue_t; - BlockQueue_t queue; // Queue of Blocks + typedef HeapRing 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 bf38b952..a2ea42c5 100644 --- 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(); } } -- 2.20.1