2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
3 Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
4 Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
10 #include "libs/nuts_bolts.h"
17 #include "libs/StreamOutputPool.h"
25 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
26 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
27 // Most of the accel math is also done in this class
28 // And GCode objects for use in on_gcode_execute are also help in here
38 //travel_distances.clear();
40 std::vector
<Gcode
>().swap(gcodes
); // this resizes the vector releasing its memory
42 clear_vector(this->steps
);
44 steps_event_count
= 0;
51 acceleration
= 100.0F
; // we don't want to get devide by zeroes if this is not set
57 recalculate_flag
= false;
58 nominal_length_flag
= false;
59 max_entry_speed
= 0.0F
;
66 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",
71 this->steps_event_count
,
76 this->accelerate_until
,
77 this->decelerate_after
,
81 this->max_entry_speed
,
85 nominal_length_flag
?1:0
90 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
91 // The factors represent a factor of braking and must be in the range 0.0-1.0.
92 // +--------+ <- nominal_rate
94 // nominal_rate*entry_factor -> + \
95 // | + <- nominal_rate*exit_factor
99 void Block::calculate_trapezoid( float entryspeed
, float exitspeed
)
101 // if block is currently executing, don't touch anything!
105 // The planner passes us factors, we need to transform them in rates
106 this->initial_rate
= ceilf(this->nominal_rate
* entryspeed
/ this->nominal_speed
); // (step/s)
107 this->final_rate
= ceilf(this->nominal_rate
* exitspeed
/ this->nominal_speed
); // (step/s)
109 // How many steps to accelerate and decelerate
110 float acceleration_per_second
= this->rate_delta
* THEKERNEL
->stepper
->get_acceleration_ticks_per_second(); // ( step/s^2)
111 int accelerate_steps
= ceilf( this->estimate_acceleration_distance( this->initial_rate
, this->nominal_rate
, acceleration_per_second
) );
112 int decelerate_steps
= floor( this->estimate_acceleration_distance( this->nominal_rate
, this->final_rate
, -acceleration_per_second
) );
114 // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
115 int plateau_steps
= this->steps_event_count
- accelerate_steps
- decelerate_steps
;
117 // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
118 // have to use intersection_distance() to calculate when to abort acceleration and start braking
119 // in order to reach the final_rate exactly at the end of this block.
120 if (plateau_steps
< 0) {
121 accelerate_steps
= ceilf(this->intersection_distance(this->initial_rate
, this->final_rate
, acceleration_per_second
, this->steps_event_count
));
122 accelerate_steps
= max( accelerate_steps
, 0 ); // Check limits due to numerical round-off
123 accelerate_steps
= min( accelerate_steps
, int(this->steps_event_count
) );
126 this->accelerate_until
= accelerate_steps
;
127 this->decelerate_after
= accelerate_steps
+ plateau_steps
;
129 this->exit_speed
= exitspeed
;
132 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
133 // given acceleration:
134 float Block::estimate_acceleration_distance(float initialrate
, float targetrate
, float acceleration
)
136 return( ((targetrate
* targetrate
) - (initialrate
* initialrate
)) / (2.0F
* acceleration
));
139 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
140 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
141 // a total travel of distance. This can be used to compute the intersection point between acceleration and
142 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
144 /* + <- some maximum rate we don't care about
149 initial_rate -> +----+--+
152 intersection_distance distance */
153 float Block::intersection_distance(float initialrate
, float finalrate
, float acceleration
, float distance
)
155 return((2 * acceleration
* distance
- initialrate
* initialrate
+ finalrate
* finalrate
) / (4 * acceleration
));
158 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
159 // acceleration within the allotted distance.
160 float Block::max_allowable_speed(float acceleration
, float target_velocity
, float distance
)
162 return sqrtf(target_velocity
* target_velocity
- 2.0F
* acceleration
* distance
);
166 // Called by Planner::recalculate() when scanning the plan from last to first entry.
167 float Block::reverse_pass(float exit_speed
)
169 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
170 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
171 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
172 if (this->entry_speed
!= this->max_entry_speed
)
174 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
175 // for max allowable speed if block is decelerating and nominal length is false.
176 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> exit_speed
))
178 float max_entry_speed
= max_allowable_speed(-this->acceleration
, exit_speed
, this->millimeters
);
180 this->entry_speed
= min(max_entry_speed
, this->max_entry_speed
);
182 return this->entry_speed
;
185 this->entry_speed
= this->max_entry_speed
;
188 return this->entry_speed
;
192 // Called by Planner::recalculate() when scanning the plan from first to last entry.
193 // returns maximum exit speed of this block
194 float Block::forward_pass(float prev_max_exit_speed
)
196 // If the previous block is an acceleration block, but it is not long enough to complete the
197 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
198 // speeds have already been reset, maximized, and reverse planned by reverse planner.
199 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
201 // TODO: find out if both of these checks are necessary
202 if (prev_max_exit_speed
> nominal_speed
)
203 prev_max_exit_speed
= nominal_speed
;
204 if (prev_max_exit_speed
> max_entry_speed
)
205 prev_max_exit_speed
= max_entry_speed
;
207 if (prev_max_exit_speed
<= entry_speed
)
210 entry_speed
= prev_max_exit_speed
;
211 // since we're now acceleration or cruise limited
212 // we don't need to recalculate our entry speed anymore
213 recalculate_flag
= false;
216 // // decel limited, do nothing
218 return max_exit_speed();
221 float Block::max_exit_speed()
223 // if block is currently executing, return cached exit speed from calculate_trapezoid
224 // this ensures that a block following a currently executing block will have correct entry speed
228 // if nominal_length_flag is asserted
229 // we are guaranteed to reach nominal speed regardless of entry speed
230 // thus, max exit will always be nominal
231 if (nominal_length_flag
)
232 return nominal_speed
;
234 // otherwise, we have to work out max exit speed based on entry and acceleration
235 float max
= max_allowable_speed(-this->acceleration
, this->entry_speed
, this->millimeters
);
237 return min(max
, nominal_speed
);
240 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
241 void Block::append_gcode(Gcode
* gcode
)
243 Gcode new_gcode
= *gcode
;
244 new_gcode
.strip_parameters(); // optimization to save memory we strip off the XYZIJK parameters from the saved command
245 gcodes
.push_back(new_gcode
);
250 recalculate_flag
= false;
257 // execute all the gcodes related to this block
258 for(unsigned int index
= 0; index
< gcodes
.size(); index
++)
259 THEKERNEL
->call_event(ON_GCODE_EXECUTE
, &(gcodes
[index
]));
261 THEKERNEL
->call_event(ON_BLOCK_BEGIN
, this);
267 // Signal the conveyor that this block is ready to be injected into the system
270 this->is_ready
= true;
273 // Mark the block as taken by one more module
281 // Mark the block as no longer taken by one module, go to next block if this free's it
282 void Block::release()
284 if (--this->times_taken
<= 0)
290 THEKERNEL
->call_event(ON_BLOCK_END
, this);
292 // ensure conveyor gets called last
293 THEKERNEL
->conveyor
->on_block_end(this);