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"
18 #include "../communication/utils/Gcode.h"
20 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
21 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
22 // Most of the accel math is also done in this class
23 // And GCode objects for use in on_gcode_execute are also help in here
33 //travel_distances.clear();
35 clear_vector(this->steps
);
48 recalculate_flag
= false;
49 nominal_length_flag
= false;
50 max_entry_speed
= 0.0F
;
57 THEKERNEL
->serial
->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",
62 this->steps_event_count
,
67 this->accelerate_until
,
68 this->decelerate_after
,
72 this->max_entry_speed
,
76 nominal_length_flag
?1:0
81 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
82 // The factors represent a factor of braking and must be in the range 0.0-1.0.
83 // +--------+ <- nominal_rate
85 // nominal_rate*entry_factor -> + \
86 // | + <- nominal_rate*exit_factor
90 void Block::calculate_trapezoid( float entryspeed
, float exitspeed
)
92 // if block is currently executing, don't touch anything!
96 // The planner passes us factors, we need to transform them in rates
97 this->initial_rate
= ceil(this->nominal_rate
* entryspeed
/ this->nominal_speed
); // (step/min)
98 this->final_rate
= ceil(this->nominal_rate
* exitspeed
/ this->nominal_speed
); // (step/min)
100 // How many steps to accelerate and decelerate
101 float acceleration_per_minute
= this->rate_delta
* THEKERNEL
->stepper
->acceleration_ticks_per_second
* 60.0; // ( step/min^2)
102 int accelerate_steps
= ceil( this->estimate_acceleration_distance( this->initial_rate
, this->nominal_rate
, acceleration_per_minute
) );
103 int decelerate_steps
= floor( this->estimate_acceleration_distance( this->nominal_rate
, this->final_rate
, -acceleration_per_minute
) );
105 // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
106 int plateau_steps
= this->steps_event_count
- accelerate_steps
- decelerate_steps
;
108 // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
109 // have to use intersection_distance() to calculate when to abort acceleration and start braking
110 // in order to reach the final_rate exactly at the end of this block.
111 if (plateau_steps
< 0) {
112 accelerate_steps
= ceil(this->intersection_distance(this->initial_rate
, this->final_rate
, acceleration_per_minute
, this->steps_event_count
));
113 accelerate_steps
= max( accelerate_steps
, 0 ); // Check limits due to numerical round-off
114 accelerate_steps
= min( accelerate_steps
, int(this->steps_event_count
) );
117 this->accelerate_until
= accelerate_steps
;
118 this->decelerate_after
= accelerate_steps
+ plateau_steps
;
120 this->exit_speed
= exitspeed
;
123 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
124 // given acceleration:
125 float Block::estimate_acceleration_distance(float initialrate
, float targetrate
, float acceleration
)
127 return( ((targetrate
* targetrate
) - (initialrate
* initialrate
)) / (2.0F
* acceleration
));
130 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
131 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
132 // a total travel of distance. This can be used to compute the intersection point between acceleration and
133 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
135 /* + <- some maximum rate we don't care about
140 initial_rate -> +----+--+
143 intersection_distance distance */
144 float Block::intersection_distance(float initialrate
, float finalrate
, float acceleration
, float distance
)
146 return((2 * acceleration
* distance
- initialrate
* initialrate
+ finalrate
* finalrate
) / (4 * acceleration
));
149 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
150 // acceleration within the allotted distance.
151 inline float max_allowable_speed(float acceleration
, float target_velocity
, float distance
)
153 return sqrtf(target_velocity
* target_velocity
- 2.0F
* acceleration
* distance
);
157 // Called by Planner::recalculate() when scanning the plan from last to first entry.
158 float Block::reverse_pass(float exit_speed
)
160 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
161 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
162 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
163 if (this->entry_speed
!= this->max_entry_speed
)
165 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
166 // for max allowable speed if block is decelerating and nominal length is false.
167 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> exit_speed
))
169 float max_entry_speed
= max_allowable_speed(-THEKERNEL
->planner
->acceleration
, exit_speed
, this->millimeters
);
171 this->entry_speed
= min(max_entry_speed
, this->max_entry_speed
);
173 return this->entry_speed
;
176 this->entry_speed
= this->max_entry_speed
;
179 return this->entry_speed
;
183 // Called by Planner::recalculate() when scanning the plan from first to last entry.
184 // returns maximum exit speed of this block
185 float Block::forward_pass(float prev_max_exit_speed
)
187 // If the previous block is an acceleration block, but it is not long enough to complete the
188 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
189 // speeds have already been reset, maximized, and reverse planned by reverse planner.
190 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
192 // TODO: find out if both of these checks are necessary
193 if (prev_max_exit_speed
> nominal_speed
)
194 prev_max_exit_speed
= nominal_speed
;
195 if (prev_max_exit_speed
> max_entry_speed
)
196 prev_max_exit_speed
= max_entry_speed
;
198 if (prev_max_exit_speed
<= entry_speed
)
201 entry_speed
= prev_max_exit_speed
;
202 // since we're now acceleration or cruise limited
203 // we don't need to recalculate our entry speed anymore
204 recalculate_flag
= false;
207 // // decel limited, do nothing
209 return max_exit_speed();
212 float Block::max_exit_speed()
214 // if block is currently executing, return cached exit speed from calculate_trapezoid
215 // this ensures that a block following a currently executing block will have correct entry speed
219 // if nominal_length_flag is asserted
220 // we are guaranteed to reach nominal speed regardless of entry speed
221 // thus, max exit will always be nominal
222 if (nominal_length_flag
)
223 return nominal_speed
;
225 // otherwise, we have to work out max exit speed based on entry and acceleration
226 float max
= max_allowable_speed(-THEKERNEL
->planner
->acceleration
, this->entry_speed
, this->millimeters
);
228 return min(max
, nominal_speed
);
231 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
232 void Block::append_gcode(Gcode
* gcode
)
234 Gcode new_gcode
= *gcode
;
235 gcodes
.push_back(new_gcode
);
240 recalculate_flag
= false;
242 // execute all the gcodes related to this block
243 for(unsigned int index
= 0; index
< gcodes
.size(); index
++)
244 THEKERNEL
->call_event(ON_GCODE_EXECUTE
, &(gcodes
[index
]));
246 THEKERNEL
->call_event(ON_BLOCK_BEGIN
, this);
248 if (times_taken
<= 0)
252 // Signal the conveyor that this block is ready to be injected into the system
255 this->is_ready
= true;
258 // Mark the block as taken by one more module
264 // Mark the block as no longer taken by one module, go to next block if this free's it
265 void Block::release()
267 if (--this->times_taken
<= 0)
269 THEKERNEL
->call_event(ON_BLOCK_END
, this);
271 // ensure conveyor gets called last
272 THEKERNEL
->conveyor
->on_block_end(this);