#include "Block.h"
#include "Planner.h"
#include "Conveyor.h"
+#include "Gcode.h"
+#include "libs/StreamOutputPool.h"
+#include "Stepper.h"
+
+#include "mri.h"
+
using std::string;
#include <vector>
-#include "../communication/utils/Gcode.h"
// A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
// It's stacked on a queue, and that queue is then executed in order, to move the motors.
gcodes.clear();
clear_vector(this->steps);
- steps_event_count= 0;
- nominal_rate= 0;
- nominal_speed= 0.0F;
- millimeters= 0.0F;
- entry_speed= 0.0F;
- rate_delta= 0.0F;
- initial_rate= -1;
- final_rate= -1;
- accelerate_until= 0;
- decelerate_after= 0;
- direction_bits= 0;
- recalculate_flag= false;
- nominal_length_flag= false;
- max_entry_speed= 0.0F;
- is_ready= false;
- times_taken= 0;
+ steps_event_count = 0;
+ nominal_rate = 0;
+ nominal_speed = 0.0F;
+ millimeters = 0.0F;
+ entry_speed = 0.0F;
+ exit_speed = 0.0F;
+ rate_delta = 0.0F;
+ initial_rate = -1;
+ final_rate = -1;
+ accelerate_until = 0;
+ decelerate_after = 0;
+ direction_bits = 0;
+ recalculate_flag = false;
+ nominal_length_flag = false;
+ max_entry_speed = 0.0F;
+ is_ready = false;
+ times_taken = 0;
}
void Block::debug()
{
- THEKERNEL->streams->printf("%p: steps:%4d|%4d|%4d(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 \r\n", this, this->steps[0], this->steps[1], this->steps[2], this->steps_event_count, this->nominal_rate, this->nominal_speed, this->millimeters, this->rate_delta, this->accelerate_until, this->decelerate_after, this->initial_rate, this->final_rate, this->entry_speed, this->max_entry_speed, this->times_taken, this->is_ready );
+ THEKERNEL->streams->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->steps[2],
+ this->steps_event_count,
+ this->nominal_rate,
+ this->nominal_speed,
+ this->millimeters,
+ this->rate_delta,
+ this->accelerate_until,
+ this->decelerate_after,
+ this->initial_rate,
+ this->final_rate,
+ this->entry_speed,
+ this->max_entry_speed,
+ this->times_taken,
+ this->is_ready,
+ 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 )
{
+ // if block is currently executing, don't touch anything!
+ if (times_taken)
+ return;
// 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/s)
+ this->final_rate = ceil(this->nominal_rate * exitspeed / this->nominal_speed); // (step/s)
// 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)
- int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
- int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
+ float acceleration_per_second = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second; // ( step/s^2)
+ int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) );
+ int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_second ) );
// Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
// have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
- accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
+ accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count));
accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
plateau_steps = 0;
this->accelerate_until = accelerate_steps;
this->decelerate_after = accelerate_steps + plateau_steps;
+ this->exit_speed = exitspeed;
}
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// 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;
- }
- this->recalculate_flag = true;
-
+ // 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;
- this->recalculate_flag = true;
- }
- }
+
+ // 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 block is currently executing, return cached exit speed from calculate_trapezoid
+ // this ensures that a block following a currently executing block will have correct entry speed
+ if (times_taken)
+ return 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::append_gcode(Gcode *gcode)
+void Block::append_gcode(Gcode* gcode)
{
- __disable_irq();
Gcode new_gcode = *gcode;
- this->gcodes.push_back(new_gcode);
- __enable_irq();
+ gcodes.push_back(new_gcode);
}
-// The attached gcodes are then poped and the on_gcode_execute event is called with them as a parameter
-void Block::pop_and_execute_gcode()
+void Block::begin()
{
- Block *block = const_cast<Block *>(this);
- for(unsigned short index = 0; index < block->gcodes.size(); index++) {
- THEKERNEL->call_event(ON_GCODE_EXECUTE, &(block->gcodes[index]));
- }
+ recalculate_flag = false;
+
+ if (!is_ready)
+ __debugbreak();
+
+ times_taken = -1;
+
+ // 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]));
+
+ THEKERNEL->call_event(ON_BLOCK_BEGIN, this);
+
+ if (times_taken < 0)
+ release();
}
// Signal the conveyor that this block is ready to be injected into the system
void Block::ready()
{
this->is_ready = true;
- THEKERNEL->conveyor->new_block_added();
}
// Mark the block as taken by one more module
void Block::take()
{
- this->times_taken++;
+ if (times_taken < 0)
+ times_taken = 0;
+ times_taken++;
}
// Mark the block as no longer taken by one module, go to next block if this free's it
-// This is one of the craziest bits in smoothie
void Block::release()
{
-
- // A block can be taken by several modules, we want to actually release it only when all modules have release()d it
- this->times_taken--;
- if( this->times_taken < 1 ) {
-
- // All modules are done with this block
- // Call the on_block_end event so all modules can act accordingly
- THEKERNEL->call_event(ON_BLOCK_END, this);
-
- // Gcodes corresponding to the *following* blocks are stored in this block.
- // We execute them all in order when this block is finished executing
- this->pop_and_execute_gcode();
-
- // 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;
-
- 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->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
- if( conveyor->current_block->times_taken < 1 ) {
- conveyor->current_block->times_taken = 1;
- conveyor->current_block->release();
- }
- } else {
- conveyor->current_block = NULL;
- }
- } else {
- conveyor->current_block = NULL;
- }
+ if (--this->times_taken <= 0)
+ {
+ times_taken = 0;
+ if (is_ready)
+ {
+ is_ready = false;
+ THEKERNEL->call_event(ON_BLOCK_END, this);
+
+ // ensure conveyor gets called last
+ THEKERNEL->conveyor->on_block_end(this);
}
}
}
-
-
-
HCoop / clinton / Smoothieware.git / blobdiff