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"
19 #include "../communication/utils/Gcode.h"
22 clear_vector(this->steps
);
23 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.
24 this->is_ready
= false;
25 this->initial_rate
= -1;
26 this->final_rate
= -1;
29 void Block::debug(Kernel
* kernel
){
30 kernel
->serial
->printf("%p: steps:%4d|%4d|%4d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8d 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
);
34 // Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the
35 // speed under which you cannot exceed max_jerk no matter what you do.
36 double Block::compute_factor_for_safe_speed(){
37 return( this->planner
->max_jerk
/ this->nominal_speed
);
41 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
42 // The factors represent a factor of braking and must be in the range 0.0-1.0.
43 // +--------+ <- nominal_rate
45 // nominal_rate*entry_factor -> + \
46 // | + <- nominal_rate*exit_factor
49 void Block::calculate_trapezoid( double entryfactor
, double exitfactor
){
51 this->initial_rate
= ceil(this->nominal_rate
* entryfactor
); // (step/min)
52 this->final_rate
= ceil(this->nominal_rate
* exitfactor
); // (step/min)
53 //this->player->kernel->serial->printf("%p: r:%d \r\n", this, this->initial_rate);
54 double acceleration_per_minute
= this->rate_delta
* this->planner
->kernel
->stepper
->acceleration_ticks_per_second
* 60.0;
55 int accelerate_steps
= ceil( this->estimate_acceleration_distance( this->initial_rate
, this->nominal_rate
, acceleration_per_minute
) );
56 int decelerate_steps
= ceil( this->estimate_acceleration_distance( this->nominal_rate
, this->final_rate
, -acceleration_per_minute
) );
58 // Calculate the size of Plateau of Nominal Rate.
59 int plateau_steps
= this->steps_event_count
-accelerate_steps
-decelerate_steps
;
61 // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
62 // have to use intersection_distance() to calculate when to abort acceleration and start braking
63 // in order to reach the final_rate exactly at the end of this block.
64 if (plateau_steps
< 0) {
65 accelerate_steps
= ceil(this->intersection_distance(this->initial_rate
, this->final_rate
, acceleration_per_minute
, this->steps_event_count
));
66 accelerate_steps
= max( accelerate_steps
, 0 ); // Check limits due to numerical round-off
67 accelerate_steps
= min( accelerate_steps
, int(this->steps_event_count
) );
71 this->accelerate_until
= accelerate_steps
;
72 this->decelerate_after
= accelerate_steps
+plateau_steps
;
74 // DIRTY HACK so that we don't end too early for blocks with 0 as final_rate. Doing the math right would be better. Probably fixed in latest grbl
75 if( this->final_rate
< 0.01 ){
76 this->decelerate_after
+= floor( this->nominal_rate
/ 60 / this->planner
->kernel
->stepper
->acceleration_ticks_per_second
) * 3;
81 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
82 // given acceleration:
83 double Block::estimate_acceleration_distance(double initialrate
, double targetrate
, double acceleration
) {
84 return( (targetrate
*targetrate
-initialrate
*initialrate
)/(2L*acceleration
));
87 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
88 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
89 // a total travel of distance. This can be used to compute the intersection point between acceleration and
90 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
92 /* + <- some maximum rate we don't care about
97 initial_rate -> +----+--+
100 intersection_distance distance */
101 double Block::intersection_distance(double initialrate
, double finalrate
, double acceleration
, double distance
) {
102 return((2*acceleration
*distance
-initialrate
*initialrate
+finalrate
*finalrate
)/(4*acceleration
));
105 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
106 // acceleration within the allotted distance.
107 inline double max_allowable_speed(double acceleration
, double target_velocity
, double distance
) {
109 sqrt(target_velocity
*target_velocity
-2L*acceleration
*60*60*distance
) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
114 // Called by Planner::recalculate() when scanning the plan from last to first entry.
115 void Block::reverse_pass(Block
* next
, Block
* previous
){
118 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
119 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
120 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
121 if (this->entry_speed
!= this->max_entry_speed
) {
123 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
124 // for max allowable speed if block is decelerating and nominal length is false.
125 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> next
->entry_speed
)) {
126 this->entry_speed
= min( this->max_entry_speed
, max_allowable_speed(-this->planner
->acceleration
,next
->entry_speed
,this->millimeters
));
128 this->entry_speed
= this->max_entry_speed
;
130 this->recalculate_flag
= true;
133 } // Skip last block. Already initialized and set for recalculation.
138 // Called by Planner::recalculate() when scanning the plan from first to last entry.
139 void Block::forward_pass(Block
* previous
, Block
* next
){
141 if(!previous
) { return; } // Begin planning after buffer_tail
143 // If the previous block is an acceleration block, but it is not long enough to complete the
144 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
145 // speeds have already been reset, maximized, and reverse planned by reverse planner.
146 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
147 if (!previous
->nominal_length_flag
) {
148 if (previous
->entry_speed
< this->entry_speed
) {
149 double entry_speed
= min( this->entry_speed
,
150 max_allowable_speed(-this->planner
->acceleration
,previous
->entry_speed
,previous
->millimeters
) );
152 // Check for junction speed change
153 if (this->entry_speed
!= entry_speed
) {
154 this->entry_speed
= entry_speed
;
155 this->recalculate_flag
= true;
163 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
164 void Block::append_gcode(Gcode
* gcode
){
165 //this->commands.push_back(gcode->command);
166 //this->travel_distances.push_back(gcode->millimeters_of_travel);
168 this->gcodes
.push_back(*gcode
);
172 // The attached gcodes are then poped and the on_gcode_execute event is called with them as a parameter
173 void Block::pop_and_execute_gcode(Kernel
* &kernel
){
174 Block
* block
= const_cast<Block
*>(this);
175 //for(unsigned short index=0; index<block->commands.size(); index++){
176 // Gcode gcode = Gcode();
177 // gcode.command = block->commands.at(index);
178 // gcode.millimeters_of_travel = block->travel_distances.at(index);
179 // kernel->call_event(ON_GCODE_EXECUTE, &gcode );
181 for(unsigned short index
=0; index
<block
->gcodes
.size(); index
++){
182 //this->player->kernel->serial->printf("exec: block:%p gcode:%p command:%p \r\n", block, &(block->gcodes[index]), &(block->gcodes[index].command) );
183 //this->player->kernel->serial->printf(" str:%s \r\n", block->gcodes[index].command.c_str() );
185 kernel
->call_event(ON_GCODE_EXECUTE
, &(block
->gcodes
[index
]));
189 // Signal the player that this block is ready to be injected into the system
191 this->is_ready
= true;
192 this->player
->new_block_added();
195 // Mark the block as taken by one more module
200 // Mark the block as no longer taken by one module, go to next block if this free's it
201 void Block::release(){
203 if( this->times_taken
< 1 ){
204 this->player
->kernel
->call_event(ON_BLOCK_END
, this);
205 this->pop_and_execute_gcode(this->player
->kernel
);
206 Player
* player
= this->player
;
208 //this->player->kernel->serial->printf("a %d\r\n", this->player->queue.size() );
209 if( player
->queue
.size() > 0 ){
210 player
->queue
.delete_first();
213 //this->player->kernel->serial->printf("b %d %d\r\n", this->player->queue.size(), player->looking_for_new_block );
215 if( player
->looking_for_new_block
== false ){
216 //player->pop_and_process_new_block(123);
217 if( player
->queue
.size() > 0 ){
218 Block
* candidate
= player
->queue
.get_ref(0);
219 if( candidate
->is_ready
){
220 //candidate->debug(player->kernel);
221 //this->player->kernel->serial->printf("c %d %d\r\n", this->player->queue.size(), player->looking_for_new_block );
222 player
->current_block
= candidate
;
223 player
->kernel
->call_event(ON_BLOCK_BEGIN
, player
->current_block
);
224 if( player
->current_block
->times_taken
< 1 ){
225 player
->current_block
->release();
229 player
->current_block
= NULL
;
233 //player->current_block->debug(player->kernel);
234 //this->player->kernel->serial->printf("d %d %d\r\n", this->player->queue.size(), player->looking_for_new_block );
236 player
->current_block
= NULL
;