From e918c030e3ced39ce7d6f8d7a66d1ce8bb78e7d9 Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Thu, 26 Dec 2013 17:46:30 +1100 Subject: [PATCH] Revert "Merge branch 'fix/Queue_scan_consolidation' into edge" This reverts commit b4f9b9b5b4186df5a681dc63cd4174c543cd41d7, reversing changes made to d97f94e3d8782e77793011430bb38784640b1f6f. needs more work --- src/modules/robot/Block.cpp | 4 +- src/modules/robot/Block.h | 4 +- src/modules/robot/Planner.cpp | 101 ++++++++++++++++++++-------------- src/modules/robot/Planner.h | 3 + 4 files changed, 68 insertions(+), 44 deletions(-) diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp index 2cfe9a57..2cfa1d46 100644 --- 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 3dc62935..8e217eca 100644 --- 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 80c7487c..da2fda81 100644 --- 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 2270a8e4..dafb72b6 100644 --- 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(); -- 2.20.1