Commit | Line | Data |
---|---|---|
7b49793d | 1 | /* |
4cff3ded AW |
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. | |
7b49793d | 5 | You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. |
4cff3ded AW |
6 | */ |
7 | ||
8 | #include "libs/Module.h" | |
9 | #include "libs/Kernel.h" | |
10 | #include "libs/nuts_bolts.h" | |
11 | #include <math.h> | |
4cff3ded AW |
12 | #include <string> |
13 | #include "Block.h" | |
14 | #include "Planner.h" | |
3fceb8eb | 15 | #include "Conveyor.h" |
9d005957 | 16 | #include "Gcode.h" |
61134a65 JM |
17 | #include "libs/StreamOutputPool.h" |
18 | #include "Stepper.h" | |
8b260c2c | 19 | #include "StepTicker.h" |
9d005957 MM |
20 | |
21 | #include "mri.h" | |
22 | ||
4cff3ded AW |
23 | using std::string; |
24 | #include <vector> | |
4cff3ded | 25 | |
8b260c2c JM |
26 | #define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency() |
27 | #define STEP_TICKER_FREQUENCY_2 (STEP_TICKER_FREQUENCY*STEP_TICKER_FREQUENCY) | |
28 | ||
edac9072 AW |
29 | // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves. |
30 | // It's stacked on a queue, and that queue is then executed in order, to move the motors. | |
31 | // Most of the accel math is also done in this class | |
32 | // And GCode objects for use in on_gcode_execute are also help in here | |
33 | ||
1cf31736 JM |
34 | Block::Block() |
35 | { | |
36 | clear(); | |
37 | } | |
38 | ||
39 | void Block::clear() | |
40 | { | |
41 | //commands.clear(); | |
42 | //travel_distances.clear(); | |
43 | gcodes.clear(); | |
b64cb3dd JM |
44 | std::vector<Gcode>().swap(gcodes); // this resizes the vector releasing its memory |
45 | ||
807b9b57 | 46 | this->steps.fill(0); |
1cf31736 | 47 | |
f539c22f | 48 | steps_event_count = 0; |
1598a726 | 49 | nominal_rate = 0.0F; |
f539c22f MM |
50 | nominal_speed = 0.0F; |
51 | millimeters = 0.0F; | |
52 | entry_speed = 0.0F; | |
528c2e16 | 53 | exit_speed = 0.0F; |
3eadcfee | 54 | acceleration = 100.0F; // we don't want to get devide by zeroes if this is not set |
1598a726 | 55 | initial_rate = 0.0F; |
f539c22f MM |
56 | accelerate_until = 0; |
57 | decelerate_after = 0; | |
58 | direction_bits = 0; | |
59 | recalculate_flag = false; | |
60 | nominal_length_flag = false; | |
61 | max_entry_speed = 0.0F; | |
62 | is_ready = false; | |
63 | times_taken = 0; | |
8b260c2c JM |
64 | acceleration_per_tick= 0; |
65 | deceleration_per_tick= 0; | |
66 | total_move_ticks= 0; | |
4cff3ded AW |
67 | } |
68 | ||
1cf31736 JM |
69 | void Block::debug() |
70 | { | |
1598a726 | 71 | THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%6.1f/s%6.1f mm:%9.6f acc:%5lu dec:%5lu rates:%10.4f entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n", |
2134bcf2 | 72 | this, |
1b5776bf JM |
73 | this->steps[0], |
74 | this->steps[1], | |
75 | this->steps[2], | |
76 | this->steps_event_count, | |
77 | this->nominal_rate, | |
78 | this->nominal_speed, | |
79 | this->millimeters, | |
1b5776bf JM |
80 | this->accelerate_until, |
81 | this->decelerate_after, | |
82 | this->initial_rate, | |
1b5776bf JM |
83 | this->entry_speed, |
84 | this->max_entry_speed, | |
85 | this->times_taken, | |
86 | this->is_ready, | |
87 | recalculate_flag ? 1 : 0, | |
88 | nominal_length_flag ? 1 : 0 | |
89 | ); | |
4cff3ded AW |
90 | } |
91 | ||
92 | ||
69735c09 | 93 | /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. |
4cff3ded AW |
94 | // The factors represent a factor of braking and must be in the range 0.0-1.0. |
95 | // +--------+ <- nominal_rate | |
96 | // / \ | |
97 | // nominal_rate*entry_factor -> + \ | |
98 | // | + <- nominal_rate*exit_factor | |
99 | // +-------------+ | |
100 | // time --> | |
edac9072 | 101 | */ |
a617ac35 | 102 | void Block::calculate_trapezoid( float entryspeed, float exitspeed ) |
1cf31736 | 103 | { |
5de195be MM |
104 | // if block is currently executing, don't touch anything! |
105 | if (times_taken) | |
106 | return; | |
2bb8b390 | 107 | |
8b260c2c JM |
108 | float initial_rate = this->nominal_rate * (entryspeed / this->nominal_speed); // steps/sec |
109 | float final_rate = this->nominal_rate * (exitspeed / this->nominal_speed); | |
110 | //printf("Initial rate: %f, final_rate: %f\n", initial_rate, final_rate); | |
111 | // How many steps ( can be fractions of steps, we need very precise values ) to accelerate and decelerate | |
112 | // This is a simplification to get rid of rate_delta and get the steps/s² accel directly from the mm/s² accel | |
113 | float acceleration_per_second = (this->acceleration * this->steps_event_count) / this->millimeters; | |
114 | ||
115 | float maximum_possible_rate = sqrtf( ( this->steps_event_count * acceleration_per_second ) + ( ( powf(initial_rate, 2) + powf(final_rate, 2) ) / 2.0F ) ); | |
116 | ||
117 | //printf("id %d: acceleration_per_second: %f, maximum_possible_rate: %f steps/sec, %f mm/sec\n", this->id, acceleration_per_second, maximum_possible_rate, maximum_possible_rate/100); | |
118 | ||
119 | // Now this is the maximum rate we'll achieve this move, either because | |
120 | // it's the higher we can achieve, or because it's the higher we are | |
121 | // allowed to achieve | |
1ae56063 | 122 | this->maximum_rate = std::min(maximum_possible_rate, this->nominal_rate); |
8b260c2c JM |
123 | |
124 | // Now figure out how long it takes to accelerate in seconds | |
125 | float time_to_accelerate = ( this->maximum_rate - initial_rate ) / acceleration_per_second; | |
126 | ||
127 | // Now figure out how long it takes to decelerate | |
128 | float time_to_decelerate = ( final_rate - this->maximum_rate ) / -acceleration_per_second; | |
129 | ||
130 | // Now we know how long it takes to accelerate and decelerate, but we must | |
131 | // also know how long the entire move takes so we can figure out how long | |
132 | // is the plateau if there is one | |
133 | float plateau_time = 0; | |
134 | ||
135 | // Only if there is actually a plateau ( we are limited by nominal_rate ) | |
136 | if(maximum_possible_rate > this->nominal_rate) { | |
137 | // Figure out the acceleration and deceleration distances ( in steps ) | |
138 | float acceleration_distance = ( ( initial_rate + this->maximum_rate ) / 2.0F ) * time_to_accelerate; | |
139 | float deceleration_distance = ( ( this->maximum_rate + final_rate ) / 2.0F ) * time_to_decelerate; | |
140 | ||
141 | // Figure out the plateau steps | |
142 | float plateau_distance = this->steps_event_count - acceleration_distance - deceleration_distance; | |
143 | ||
144 | // Figure out the plateau time in seconds | |
145 | plateau_time = plateau_distance / this->maximum_rate; | |
1cf31736 | 146 | } |
4cff3ded | 147 | |
8b260c2c JM |
148 | // Figure out how long the move takes total ( in seconds ) |
149 | float total_move_time = time_to_accelerate + time_to_decelerate + plateau_time; | |
150 | //puts "total move time: #{total_move_time}s time to accelerate: #{time_to_accelerate}, time to decelerate: #{time_to_decelerate}" | |
151 | ||
152 | // We now have the full timing for acceleration, plateau and deceleration, | |
153 | // yay \o/ Now this is very important these are in seconds, and we need to | |
154 | // round them into ticks. This means instead of accelerating in 100.23 | |
155 | // ticks we'll accelerate in 100 ticks. Which means to reach the exact | |
156 | // speed we want to reach, we must figure out a new/slightly different | |
157 | // acceleration/deceleration to be sure we accelerate and decelerate at | |
158 | // the exact rate we want | |
159 | ||
160 | // First off round total time, acceleration time and deceleration time in ticks | |
161 | uint32_t acceleration_ticks = floorf( time_to_accelerate * STEP_TICKER_FREQUENCY ); | |
162 | uint32_t deceleration_ticks = floorf( time_to_decelerate * STEP_TICKER_FREQUENCY ); | |
163 | uint32_t total_move_ticks = floorf( total_move_time * STEP_TICKER_FREQUENCY ); | |
164 | ||
165 | // Now deduce the plateau time for those new values expressed in tick | |
166 | //uint32_t plateau_ticks = total_move_ticks - acceleration_ticks - deceleration_ticks; | |
167 | ||
168 | // Now we figure out the acceleration value to reach EXACTLY maximum_rate(steps/s) in EXACTLY acceleration_ticks(ticks) amount of time in seconds | |
169 | float acceleration_time = acceleration_ticks / STEP_TICKER_FREQUENCY; // This can be moved into the operation below, separated for clarity, note we need to do this instead of using time_to_accelerate(seconds) directly because time_to_accelerate(seconds) and acceleration_ticks(seconds) do not have the same value anymore due to the rounding | |
170 | float deceleration_time = deceleration_ticks / STEP_TICKER_FREQUENCY; | |
171 | ||
172 | float acceleration_in_steps = (acceleration_time > 0.0F ) ? ( this->maximum_rate - initial_rate ) / acceleration_time : 0; | |
173 | float deceleration_in_steps = (deceleration_time > 0.0F ) ? ( this->maximum_rate - final_rate ) / deceleration_time : 0; | |
174 | ||
8b260c2c JM |
175 | // Now figure out the two acceleration ramp change events in ticks |
176 | this->accelerate_until = acceleration_ticks; | |
177 | this->decelerate_after = total_move_ticks - deceleration_ticks; | |
178 | ||
179 | // Now figure out the acceleration PER TICK, this should ideally be held as a float, even a double if possible as it's very critical to the block timing | |
180 | // steps/tick^2 | |
181 | ||
182 | this->acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2; | |
183 | this->deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2; | |
184 | ||
185 | // We now have everything we need for this block to call a Steppermotor->move method !!!! | |
186 | // Theorically, if accel is done per tick, the speed curve should be perfect. | |
187 | ||
188 | // We need this to call move() | |
189 | this->total_move_ticks = total_move_ticks; | |
190 | ||
191 | //puts "accelerate_until: #{this->accelerate_until}, decelerate_after: #{this->decelerate_after}, acceleration_per_tick: #{this->acceleration_per_tick}, total_move_ticks: #{this->total_move_ticks}" | |
192 | ||
193 | this->initial_rate = initial_rate; | |
5de195be | 194 | this->exit_speed = exitspeed; |
4cff3ded AW |
195 | } |
196 | ||
4cff3ded AW |
197 | // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the |
198 | // acceleration within the allotted distance. | |
558e170c | 199 | float Block::max_allowable_speed(float acceleration, float target_velocity, float distance) |
1cf31736 | 200 | { |
a617ac35 | 201 | return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance); |
4cff3ded AW |
202 | } |
203 | ||
4cff3ded | 204 | // Called by Planner::recalculate() when scanning the plan from last to first entry. |
a617ac35 | 205 | float Block::reverse_pass(float exit_speed) |
1cf31736 | 206 | { |
a617ac35 MM |
207 | // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. |
208 | // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and | |
209 | // check for maximum allowable speed reductions to ensure maximum possible planned speed. | |
1b5776bf | 210 | if (this->entry_speed != this->max_entry_speed) { |
a617ac35 MM |
211 | // If nominal length true, max junction speed is guaranteed to be reached. Only compute |
212 | // for max allowable speed if block is decelerating and nominal length is false. | |
1b5776bf | 213 | if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed)) { |
4fdd2470 | 214 | float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters); |
a617ac35 MM |
215 | |
216 | this->entry_speed = min(max_entry_speed, this->max_entry_speed); | |
217 | ||
218 | return this->entry_speed; | |
1b5776bf | 219 | } else |
a617ac35 MM |
220 | this->entry_speed = this->max_entry_speed; |
221 | } | |
4cff3ded | 222 | |
a617ac35 | 223 | return this->entry_speed; |
aab6cbba | 224 | } |
4cff3ded AW |
225 | |
226 | ||
227 | // Called by Planner::recalculate() when scanning the plan from first to last entry. | |
a617ac35 MM |
228 | // returns maximum exit speed of this block |
229 | float Block::forward_pass(float prev_max_exit_speed) | |
1cf31736 | 230 | { |
aab6cbba AW |
231 | // If the previous block is an acceleration block, but it is not long enough to complete the |
232 | // full speed change within the block, we need to adjust the entry speed accordingly. Entry | |
233 | // speeds have already been reset, maximized, and reverse planned by reverse planner. | |
234 | // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. | |
a617ac35 MM |
235 | |
236 | // TODO: find out if both of these checks are necessary | |
237 | if (prev_max_exit_speed > nominal_speed) | |
238 | prev_max_exit_speed = nominal_speed; | |
239 | if (prev_max_exit_speed > max_entry_speed) | |
240 | prev_max_exit_speed = max_entry_speed; | |
241 | ||
1b5776bf | 242 | if (prev_max_exit_speed <= entry_speed) { |
a617ac35 MM |
243 | // accel limited |
244 | entry_speed = prev_max_exit_speed; | |
245 | // since we're now acceleration or cruise limited | |
246 | // we don't need to recalculate our entry speed anymore | |
247 | recalculate_flag = false; | |
aab6cbba | 248 | } |
a617ac35 MM |
249 | // else |
250 | // // decel limited, do nothing | |
7b49793d | 251 | |
a617ac35 MM |
252 | return max_exit_speed(); |
253 | } | |
254 | ||
255 | float Block::max_exit_speed() | |
256 | { | |
5de195be MM |
257 | // if block is currently executing, return cached exit speed from calculate_trapezoid |
258 | // this ensures that a block following a currently executing block will have correct entry speed | |
259 | if (times_taken) | |
260 | return exit_speed; | |
261 | ||
a617ac35 MM |
262 | // if nominal_length_flag is asserted |
263 | // we are guaranteed to reach nominal speed regardless of entry speed | |
264 | // thus, max exit will always be nominal | |
265 | if (nominal_length_flag) | |
266 | return nominal_speed; | |
267 | ||
268 | // otherwise, we have to work out max exit speed based on entry and acceleration | |
4fdd2470 | 269 | float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters); |
a617ac35 MM |
270 | |
271 | return min(max, nominal_speed); | |
4cff3ded AW |
272 | } |
273 | ||
4cff3ded | 274 | // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it |
2134bcf2 | 275 | void Block::append_gcode(Gcode* gcode) |
1cf31736 | 276 | { |
1cf31736 | 277 | Gcode new_gcode = *gcode; |
fb7956a9 | 278 | new_gcode.strip_parameters(); // optimization to save memory we strip off the XYZIJK parameters from the saved command |
2134bcf2 | 279 | gcodes.push_back(new_gcode); |
4cff3ded AW |
280 | } |
281 | ||
2134bcf2 | 282 | void Block::begin() |
1cf31736 | 283 | { |
2134bcf2 | 284 | recalculate_flag = false; |
a617ac35 | 285 | |
9d005957 MM |
286 | if (!is_ready) |
287 | __debugbreak(); | |
288 | ||
f2bb3f9f MM |
289 | times_taken = -1; |
290 | ||
2134bcf2 MM |
291 | // execute all the gcodes related to this block |
292 | for(unsigned int index = 0; index < gcodes.size(); index++) | |
293 | THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index])); | |
294 | ||
9e089978 | 295 | |
2134bcf2 | 296 | THEKERNEL->call_event(ON_BLOCK_BEGIN, this); |
1366cafd | 297 | |
f2bb3f9f | 298 | if (times_taken < 0) |
1366cafd | 299 | release(); |
4cff3ded AW |
300 | } |
301 | ||
3fceb8eb | 302 | // Signal the conveyor that this block is ready to be injected into the system |
1cf31736 JM |
303 | void Block::ready() |
304 | { | |
13e4a3f9 | 305 | this->is_ready = true; |
3a4fa0c1 AW |
306 | } |
307 | ||
308 | // Mark the block as taken by one more module | |
1cf31736 JM |
309 | void Block::take() |
310 | { | |
f2bb3f9f MM |
311 | if (times_taken < 0) |
312 | times_taken = 0; | |
313 | times_taken++; | |
3a4fa0c1 | 314 | } |
4cff3ded | 315 | |
a3be54e3 | 316 | // Mark the block as no longer taken by one module, go to next block if this frees it |
1cf31736 JM |
317 | void Block::release() |
318 | { | |
3b1acdaa | 319 | if (--this->times_taken <= 0) { |
9d005957 | 320 | times_taken = 0; |
3b1acdaa | 321 | if (is_ready) { |
9d005957 MM |
322 | is_ready = false; |
323 | THEKERNEL->call_event(ON_BLOCK_END, this); | |
06a96473 | 324 | |
9d005957 MM |
325 | // ensure conveyor gets called last |
326 | THEKERNEL->conveyor->on_block_end(this); | |
327 | } | |
d5a58071 | 328 | } |
3a4fa0c1 | 329 | } |