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 clear_vector(this->steps
);
42 steps_event_count
= 0;
54 recalculate_flag
= false;
55 nominal_length_flag
= false;
56 max_entry_speed
= 0.0F
;
63 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",
68 this->steps_event_count
,
73 this->accelerate_until
,
74 this->decelerate_after
,
78 this->max_entry_speed
,
82 nominal_length_flag
?1:0
87 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
88 // The factors represent a factor of braking and must be in the range 0.0-1.0.
89 // +--------+ <- nominal_rate
91 // nominal_rate*entry_factor -> + \
92 // | + <- nominal_rate*exit_factor
96 void Block::calculate_trapezoid( float entryspeed
, float exitspeed
)
98 // if block is currently executing, don't touch anything!
102 // The planner passes us factors, we need to transform them in rates
103 this->initial_rate
= ceil(this->nominal_rate
* entryspeed
/ this->nominal_speed
); // (step/s)
104 this->final_rate
= ceil(this->nominal_rate
* exitspeed
/ this->nominal_speed
); // (step/s)
106 // How many steps to accelerate and decelerate
107 float acceleration_per_second
= this->rate_delta
* THEKERNEL
->stepper
->get_acceleration_ticks_per_second(); // ( step/s^2)
108 int accelerate_steps
= ceil( this->estimate_acceleration_distance( this->initial_rate
, this->nominal_rate
, acceleration_per_second
) );
109 int decelerate_steps
= floor( this->estimate_acceleration_distance( this->nominal_rate
, this->final_rate
, -acceleration_per_second
) );
111 // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
112 int plateau_steps
= this->steps_event_count
- accelerate_steps
- decelerate_steps
;
114 // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
115 // have to use intersection_distance() to calculate when to abort acceleration and start braking
116 // in order to reach the final_rate exactly at the end of this block.
117 if (plateau_steps
< 0) {
118 accelerate_steps
= ceil(this->intersection_distance(this->initial_rate
, this->final_rate
, acceleration_per_second
, this->steps_event_count
));
119 accelerate_steps
= max( accelerate_steps
, 0 ); // Check limits due to numerical round-off
120 accelerate_steps
= min( accelerate_steps
, int(this->steps_event_count
) );
123 this->accelerate_until
= accelerate_steps
;
124 this->decelerate_after
= accelerate_steps
+ plateau_steps
;
126 this->exit_speed
= exitspeed
;
129 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
130 // given acceleration:
131 float Block::estimate_acceleration_distance(float initialrate
, float targetrate
, float acceleration
)
133 return( ((targetrate
* targetrate
) - (initialrate
* initialrate
)) / (2.0F
* acceleration
));
136 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
137 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
138 // a total travel of distance. This can be used to compute the intersection point between acceleration and
139 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
141 /* + <- some maximum rate we don't care about
146 initial_rate -> +----+--+
149 intersection_distance distance */
150 float Block::intersection_distance(float initialrate
, float finalrate
, float acceleration
, float distance
)
152 return((2 * acceleration
* distance
- initialrate
* initialrate
+ finalrate
* finalrate
) / (4 * acceleration
));
155 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
156 // acceleration within the allotted distance.
157 inline float max_allowable_speed(float acceleration
, float target_velocity
, float distance
)
159 return sqrtf(target_velocity
* target_velocity
- 2.0F
* acceleration
* distance
);
163 // Called by Planner::recalculate() when scanning the plan from last to first entry.
164 float Block::reverse_pass(float exit_speed
)
166 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
167 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
168 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
169 if (this->entry_speed
!= this->max_entry_speed
)
171 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
172 // for max allowable speed if block is decelerating and nominal length is false.
173 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> exit_speed
))
175 float max_entry_speed
= max_allowable_speed(-THEKERNEL
->planner
->get_acceleration(), exit_speed
, this->millimeters
);
177 this->entry_speed
= min(max_entry_speed
, this->max_entry_speed
);
179 return this->entry_speed
;
182 this->entry_speed
= this->max_entry_speed
;
185 return this->entry_speed
;
189 // Called by Planner::recalculate() when scanning the plan from first to last entry.
190 // returns maximum exit speed of this block
191 float Block::forward_pass(float prev_max_exit_speed
)
193 // If the previous block is an acceleration block, but it is not long enough to complete the
194 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
195 // speeds have already been reset, maximized, and reverse planned by reverse planner.
196 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
198 // TODO: find out if both of these checks are necessary
199 if (prev_max_exit_speed
> nominal_speed
)
200 prev_max_exit_speed
= nominal_speed
;
201 if (prev_max_exit_speed
> max_entry_speed
)
202 prev_max_exit_speed
= max_entry_speed
;
204 if (prev_max_exit_speed
<= entry_speed
)
207 entry_speed
= prev_max_exit_speed
;
208 // since we're now acceleration or cruise limited
209 // we don't need to recalculate our entry speed anymore
210 recalculate_flag
= false;
213 // // decel limited, do nothing
215 return max_exit_speed();
218 float Block::max_exit_speed()
220 // if block is currently executing, return cached exit speed from calculate_trapezoid
221 // this ensures that a block following a currently executing block will have correct entry speed
225 // if nominal_length_flag is asserted
226 // we are guaranteed to reach nominal speed regardless of entry speed
227 // thus, max exit will always be nominal
228 if (nominal_length_flag
)
229 return nominal_speed
;
231 // otherwise, we have to work out max exit speed based on entry and acceleration
232 float max
= max_allowable_speed(-THEKERNEL
->planner
->get_acceleration(), this->entry_speed
, this->millimeters
);
234 return min(max
, nominal_speed
);
237 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
238 void Block::append_gcode(Gcode
* gcode
)
240 Gcode new_gcode
= *gcode
;
241 gcodes
.push_back(new_gcode
);
246 recalculate_flag
= false;
253 // execute all the gcodes related to this block
254 for(unsigned int index
= 0; index
< gcodes
.size(); index
++)
255 THEKERNEL
->call_event(ON_GCODE_EXECUTE
, &(gcodes
[index
]));
257 THEKERNEL
->call_event(ON_BLOCK_BEGIN
, this);
263 // Signal the conveyor that this block is ready to be injected into the system
266 this->is_ready
= true;
269 // Mark the block as taken by one more module
277 // Mark the block as no longer taken by one module, go to next block if this free's it
278 void Block::release()
280 if (--this->times_taken
<= 0)
286 THEKERNEL
->call_event(ON_BLOCK_END
, this);
288 // ensure conveyor gets called last
289 THEKERNEL
->conveyor
->on_block_end(this);