void Block::debug()
{
- THEKERNEL->serial->printf("%p: steps:X%04d Y%04d Z%04d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8f acc:%5d dec:%5d rates:%10d>%10d entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d\r\n",
+ THEKERNEL->serial->printf("%p: steps:X%04d Y%04d Z%04d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8f acc:%5d dec:%5d rates:%10d>%10d entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
this,
this->steps[0],
this->steps[1],
this->max_entry_speed,
this->times_taken,
this->is_ready,
- recalculate_flag?1:0
+ recalculate_flag?1:0,
+ nominal_length_flag?1:0
);
}
// +-------------+
// time -->
*/
-void Block::calculate_trapezoid( float entryfactor, float exitfactor )
+void Block::calculate_trapezoid( float entryspeed, float exitspeed )
{
// The planner passes us factors, we need to transform them in rates
- this->initial_rate = ceil(this->nominal_rate * entryfactor); // (step/min)
- this->final_rate = ceil(this->nominal_rate * exitfactor); // (step/min)
+ this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed); // (step/min)
+ this->final_rate = ceil(this->nominal_rate * exitspeed / this->nominal_speed); // (step/min)
// How many steps to accelerate and decelerate
float acceleration_per_minute = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second * 60.0; // ( step/min^2)
// acceleration within the allotted distance.
inline float max_allowable_speed(float acceleration, float target_velocity, float distance)
{
- return(
- sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
- );
+ return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance);
}
// Called by Planner::recalculate() when scanning the plan from last to first entry.
-void Block::reverse_pass(Block *next)
+float Block::reverse_pass(float exit_speed)
{
-
- if (next) {
- // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
- // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
- // check for maximum allowable speed reductions to ensure maximum possible planned speed.
- if (this->entry_speed != this->max_entry_speed) {
-
- // If nominal length true, max junction speed is guaranteed to be reached. Only compute
- // for max allowable speed if block is decelerating and nominal length is false.
- if ((!this->nominal_length_flag) && (this->max_entry_speed > next->entry_speed)) {
- this->entry_speed = min( this->max_entry_speed, max_allowable_speed(-THEKERNEL->planner->acceleration, next->entry_speed, this->millimeters));
- } else {
- this->entry_speed = this->max_entry_speed;
- }
-
+ // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+ // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
+ // check for maximum allowable speed reductions to ensure maximum possible planned speed.
+ if (this->entry_speed != this->max_entry_speed)
+ {
+ // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+ // for max allowable speed if block is decelerating and nominal length is false.
+ if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed))
+ {
+ float max_entry_speed = max_allowable_speed(-THEKERNEL->planner->acceleration, exit_speed, this->millimeters);
+
+ this->entry_speed = min(max_entry_speed, this->max_entry_speed);
+
+ return this->entry_speed;
}
- } // Skip last block. Already initialized and set for recalculation.
+ else
+ this->entry_speed = this->max_entry_speed;
+ }
+ return this->entry_speed;
}
// Called by Planner::recalculate() when scanning the plan from first to last entry.
-void Block::forward_pass(Block *previous)
+// returns maximum exit speed of this block
+float Block::forward_pass(float prev_max_exit_speed)
{
-
- if(!previous) {
- return; // Begin planning after buffer_tail
- }
-
// If the previous block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the entry speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
- if (!previous->nominal_length_flag) {
- if (previous->entry_speed < this->entry_speed) {
- float entry_speed = min( this->entry_speed,
- max_allowable_speed(-THEKERNEL->planner->acceleration, previous->entry_speed, previous->millimeters) );
-
- // Check for junction speed change
- if (this->entry_speed != entry_speed) {
- this->entry_speed = entry_speed;
- }
- }
+
+ // TODO: find out if both of these checks are necessary
+ if (prev_max_exit_speed > nominal_speed)
+ prev_max_exit_speed = nominal_speed;
+ if (prev_max_exit_speed > max_entry_speed)
+ prev_max_exit_speed = max_entry_speed;
+
+ if (prev_max_exit_speed <= entry_speed)
+ {
+ // accel limited
+ entry_speed = prev_max_exit_speed;
+ // since we're now acceleration or cruise limited
+ // we don't need to recalculate our entry speed anymore
+ recalculate_flag = false;
}
+ // else
+ // // decel limited, do nothing
+ return max_exit_speed();
+}
+
+float Block::max_exit_speed()
+{
+ // if nominal_length_flag is asserted
+ // we are guaranteed to reach nominal speed regardless of entry speed
+ // thus, max exit will always be nominal
+ if (nominal_length_flag)
+ return nominal_speed;
+
+ // otherwise, we have to work out max exit speed based on entry and acceleration
+ float max = max_allowable_speed(-THEKERNEL->planner->acceleration, this->entry_speed, this->millimeters);
+
+ return min(max, nominal_speed);
}
// Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
void Block::begin()
{
recalculate_flag = false;
-
+
// execute all the gcodes related to this block
for(unsigned int index = 0; index < gcodes.size(); index++)
THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index]));
class Block {
public:
Block();
- void calculate_trapezoid( float entry_factor, float exit_factor );
+ void calculate_trapezoid( float entry_speed, float exit_speed );
float estimate_acceleration_distance( float initial_rate, float target_rate, float acceleration );
float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance);
float get_duration_left(unsigned int already_taken_steps);
- void reverse_pass(Block* previous);
- void forward_pass(Block* next);
+ float reverse_pass(float exit_speed);
+ float forward_pass(float next_entry_speed);
+
+ float max_exit_speed();
void debug();
// Cleanly delete block
Block* block = queue.tail_ref();
block->gcodes.clear();
- THEKERNEL->serial->printf("Popped: ");
- block->debug();
queue.consume_tail();
}
}
}
}
+// Debug function
+void Conveyor::dump_queue()
+{
+ for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ )
+ {
+ THEKERNEL->streams->printf("block %03d > ", i);
+ queue.item_ref(index)->debug();
+
+ if (index == queue.head_i)
+ break;
+ }
+}
+
// feels hacky, but apparently the way to do it
#include "HeapRing.cpp"
template class HeapRing<Block>;
void append_gcode(Gcode*);
void queue_head_block(void);
+ void dump_queue(void);
+
// right now block queue size can only be changed at compile time by changing the value below
typedef HeapRing<Block> Queue_t;
// 3. Recalculate trapezoids for all blocks.
//
void Planner::recalculate() {
- Conveyor::Queue_t *queue = &THEKERNEL->conveyor->queue;
+ Conveyor::Queue_t &queue = THEKERNEL->conveyor->queue;
- unsigned int newest = queue->head_i; // head has been previously prepared in append_block above
- unsigned int oldest = queue->tail_i;
-
- unsigned int block_index = newest;
+ unsigned int block_index;
Block* previous;
Block* current;
- Block* next;
-
- current = queue->item_ref(block_index);
-
- // if there's only one block in the queue, we fall through both while loops and this ends up in current
- // so we must set it here, or perform conditionals further down. this is easier
- next = current;
-
- while ((block_index != oldest) && (current->recalculate_flag))
- {
- next = current;
- block_index = queue->prev(block_index);
- current = queue->item_ref(block_index);
-
- current->reverse_pass(next);
- }
- previous = current;
- block_index = queue->next(block_index);
- current = queue->item_ref(block_index);
-
- previous->calculate_trapezoid( previous->entry_speed/previous->nominal_speed, current->entry_speed/previous->nominal_speed );
-
- // 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.
- do
+ /*
+ * a newly added block is decel limited
+ *
+ * we find its max entry speed given its exit speed
+ *
+ * if max entry speed == current entry speed
+ * then we can set recalculate to false, since clearly adding another block didn't allow us to enter faster
+ *
+ * once recalculate is false, we must find the max exit speed
+ *
+ * given the exit speed of the previous block and our own max entry speed
+ * we can tell if we're accel or decel limited (or coasting)
+ *
+ * if prev_exit > max_entry
+ * then we're still decel limited. update previous traps with our max entry for prev exit
+ * if max_entry >= prev_exit
+ * then we're accel limited. set recalculate to false, work out max exit speed
+ *
+ *
+ */
+
+ /*
+ * Step 1:
+ * For each block, given the exit speed and acceleration, find the maximum entry speed
+ */
+
+ float entry_speed = minimum_planner_speed;
+
+ block_index = queue.head_i;
+ current = queue.item_ref(block_index);
+
+ if (!queue.is_empty())
{
- current->forward_pass(previous);
-
- if (block_index != newest)
+ while ((block_index != queue.tail_i) && current->recalculate_flag)
{
- current->calculate_trapezoid( previous->entry_speed/previous->nominal_speed, current->entry_speed/previous->nominal_speed );
+ entry_speed = current->reverse_pass(entry_speed);
- previous = current;
- block_index = queue->next(block_index);
- current = queue->item_ref(block_index);
+ block_index = queue.prev(block_index);
+ current = queue.item_ref(block_index);
}
- } while (block_index != newest);
- // 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 );
+ // now current points to either tail or first non-recalculate block
+ // and has not had its reverse_pass called
+ // or its calc trap
+ // entry_speed is set to the *exit* speed of current.
+ // each block from current to head has its entry speed set to its max entry speed- limited by decel or nominal_rate
- THEKERNEL->serial->printf("Queue: (head:%u tail:%u)\n", queue->head_i, queue->tail_i);
- dump_queue();
-}
+ // TODO: if current is being executed, use its trapezoidal exit speed instead of max exit speed
+ float exit_speed = current->max_exit_speed();
-// Debug function
-void Planner::dump_queue()
-{
- for (unsigned int index = THEKERNEL->conveyor->queue.tail_i, i = 0; true; index = THEKERNEL->conveyor->queue.next(index), i++ )
- {
- THEKERNEL->streams->printf("block %03d > ", i);
- THEKERNEL->conveyor->queue.item_ref(index)->debug();
+ while (block_index != queue.head_i)
+ {
+ previous = current;
+ block_index = queue.next(block_index);
+ current = queue.item_ref(block_index);
+
+ // we pass the exit speed of the previous block
+ // so this block can decide if it's accel or decel limited and update its fields as appropriate
+ exit_speed = current->forward_pass(exit_speed);
- if (index == THEKERNEL->conveyor->queue.head_i)
- break;
+ // TODO: don't touch previous if it's already being executed
+ previous->calculate_trapezoid(previous->entry_speed, current->entry_speed);
+ }
}
+
+ // now current points to the head item
+ // which has not had calculate_trapezoid run yet
+ current->calculate_trapezoid(current->entry_speed, minimum_planner_speed);
}
+
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance) {
void append_block( int target[], float feed_rate, float distance, float deltas[] );
float max_allowable_speed( float acceleration, float target_velocity, float distance);
void recalculate();
- void dump_queue();
Block* get_current_block();
void cleanup_queue();
void on_module_loaded();
HCoop / clinton / Smoothieware.git / commitdiff