// Most of the accel math is also done in this class
// And GCode objects for use in on_gcode_execute are also help in here
-Block::Block(){
+Block::Block()
+{
+ clear();
+}
+
+void Block::clear()
+{
+ //commands.clear();
+ //travel_distances.clear();
+ gcodes.clear();
clear_vector(this->steps);
- this->times_taken = 0; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker.
- this->is_ready = false;
- this->initial_rate = -1;
- this->final_rate = -1;
+
+ 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;
}
-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 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->times_taken, this->is_ready );
+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: %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->times_taken, this->is_ready );
}
// +-------------+
// time -->
*/
-void Block::calculate_trapezoid( float entryfactor, float exitfactor ){
+void Block::calculate_trapezoid( float entryfactor, float exitfactor )
+{
// The planner passes us factors, we need to transform them in rates
this->initial_rate = ceil(this->nominal_rate * entryfactor); // (step/min)
int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
// 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;
-
- // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
- // 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 = 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;
+ int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
+
+ // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
+ // 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 = 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;
}
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
-float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration) {
- return( ((targetrate*targetrate)-(initialrate*initialrate))/(2L*acceleration));
+float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration)
+{
+ return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration));
}
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
^ ^
| |
intersection_distance distance */
-float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance) {
- return((2*acceleration*distance-initialrate*initialrate+finalrate*finalrate)/(4*acceleration));
+float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance)
+{
+ return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration));
}
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity 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-2L*acceleration*distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
- );
+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
+ );
}
// 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)
+{
if (next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// 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(-this->planner->acceleration,next->entry_speed,this->millimeters));
+ 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;
}
// 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)
+{
- if(!previous) { return; } // Begin planning after buffer_tail
+ 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
// 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(-this->planner->acceleration,previous->entry_speed,previous->millimeters) );
+ 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;
- }
+ // Check for junction speed change
+ if (this->entry_speed != entry_speed) {
+ this->entry_speed = entry_speed;
+ this->recalculate_flag = true;
+ }
}
}
}
-
// Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
-void Block::append_gcode(Gcode* gcode){
- __disable_irq();
- Gcode new_gcode = *gcode;
- this->gcodes.push_back(new_gcode);
- __enable_irq();
+void Block::append_gcode(Gcode *gcode)
+{
+ __disable_irq();
+ Gcode new_gcode = *gcode;
+ this->gcodes.push_back(new_gcode);
+ __enable_irq();
}
// 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(){
- Block* block = const_cast<Block*>(this);
- for(unsigned short index=0; index<block->gcodes.size(); index++){
+void Block::pop_and_execute_gcode()
+{
+ 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]));
}
}
// Signal the conveyor that this block is ready to be injected into the system
-void Block::ready(){
+void Block::ready()
+{
this->is_ready = true;
- this->conveyor->new_block_added();
+ THEKERNEL->conveyor->new_block_added();
}
// Mark the block as taken by one more module
-void Block::take(){
+void Block::take()
+{
this->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(){
+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 ){
+ 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.
+ // 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 = this->conveyor;
- if( conveyor->queue.size() > conveyor->flush_blocks ){
+ // 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++;
}
// 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( 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);
-
- // We only execute blocks that are ready ( their math is done )
- if( candidate->is_ready ){
+ if( conveyor->queue.size() > conveyor->flush_blocks ) {
+ Block *candidate = conveyor->queue.get_ref(conveyor->flush_blocks);
+
+ // We only execute blocks that are ready ( their math is done )
+ if( candidate->is_ready ) {
// Execute this candidate
conveyor->current_block = candidate;
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 ){
+ if( conveyor->current_block->times_taken < 1 ) {
conveyor->current_block->times_taken = 1;
conveyor->current_block->release();
}
- }else{
+ } else {
conveyor->current_block = NULL;
}
- }else{
+ } else {
conveyor->current_block = NULL;
}
}
HCoop / clinton / Smoothieware.git / blobdiff