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" | |
9d005957 MM |
19 | |
20 | #include "mri.h" | |
21 | ||
4cff3ded AW |
22 | using std::string; |
23 | #include <vector> | |
4cff3ded | 24 | |
edac9072 AW |
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 | |
29 | ||
1cf31736 JM |
30 | Block::Block() |
31 | { | |
32 | clear(); | |
33 | } | |
34 | ||
35 | void Block::clear() | |
36 | { | |
37 | //commands.clear(); | |
38 | //travel_distances.clear(); | |
39 | gcodes.clear(); | |
b64cb3dd JM |
40 | std::vector<Gcode>().swap(gcodes); // this resizes the vector releasing its memory |
41 | ||
807b9b57 | 42 | this->steps.fill(0); |
1cf31736 | 43 | |
f539c22f MM |
44 | steps_event_count = 0; |
45 | nominal_rate = 0; | |
46 | nominal_speed = 0.0F; | |
47 | millimeters = 0.0F; | |
48 | entry_speed = 0.0F; | |
528c2e16 | 49 | exit_speed = 0.0F; |
f539c22f | 50 | rate_delta = 0.0F; |
3eadcfee | 51 | acceleration = 100.0F; // we don't want to get devide by zeroes if this is not set |
f539c22f MM |
52 | initial_rate = -1; |
53 | final_rate = -1; | |
54 | accelerate_until = 0; | |
55 | decelerate_after = 0; | |
56 | direction_bits = 0; | |
57 | recalculate_flag = false; | |
58 | nominal_length_flag = false; | |
59 | max_entry_speed = 0.0F; | |
60 | is_ready = false; | |
61 | times_taken = 0; | |
4cff3ded AW |
62 | } |
63 | ||
1cf31736 JM |
64 | void Block::debug() |
65 | { | |
1b5776bf | 66 | THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%10lu/s%6.1f mm:%9.6f rdelta:%8f acc:%5lu dec:%5lu rates:%10lu>%10lu entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n", |
2134bcf2 | 67 | this, |
1b5776bf JM |
68 | this->steps[0], |
69 | this->steps[1], | |
70 | this->steps[2], | |
71 | this->steps_event_count, | |
72 | this->nominal_rate, | |
73 | this->nominal_speed, | |
74 | this->millimeters, | |
75 | this->rate_delta, | |
76 | this->accelerate_until, | |
77 | this->decelerate_after, | |
78 | this->initial_rate, | |
79 | this->final_rate, | |
80 | this->entry_speed, | |
81 | this->max_entry_speed, | |
82 | this->times_taken, | |
83 | this->is_ready, | |
84 | recalculate_flag ? 1 : 0, | |
85 | nominal_length_flag ? 1 : 0 | |
86 | ); | |
4cff3ded AW |
87 | } |
88 | ||
89 | ||
69735c09 | 90 | /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. |
4cff3ded AW |
91 | // The factors represent a factor of braking and must be in the range 0.0-1.0. |
92 | // +--------+ <- nominal_rate | |
93 | // / \ | |
94 | // nominal_rate*entry_factor -> + \ | |
95 | // | + <- nominal_rate*exit_factor | |
96 | // +-------------+ | |
97 | // time --> | |
edac9072 | 98 | */ |
a617ac35 | 99 | void Block::calculate_trapezoid( float entryspeed, float exitspeed ) |
1cf31736 | 100 | { |
5de195be MM |
101 | // if block is currently executing, don't touch anything! |
102 | if (times_taken) | |
103 | return; | |
2bb8b390 | 104 | |
edac9072 | 105 | // The planner passes us factors, we need to transform them in rates |
9502f9d5 JM |
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) | |
813727fb | 108 | |
edac9072 | 109 | // How many steps to accelerate and decelerate |
a157d099 | 110 | float acceleration_per_second = this->rate_delta * THEKERNEL->acceleration_ticks_per_second; // ( step/s^2) |
9502f9d5 | 111 | int accelerate_steps = ceilf( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) ); |
c8f4ee77 | 112 | int decelerate_steps = floorf( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_second ) ); |
4cff3ded | 113 | |
edac9072 | 114 | // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise ) |
1cf31736 JM |
115 | int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps; |
116 | ||
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) { | |
9502f9d5 | 121 | accelerate_steps = ceilf(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count)); |
1cf31736 JM |
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) ); | |
124 | plateau_steps = 0; | |
125 | } | |
126 | this->accelerate_until = accelerate_steps; | |
127 | this->decelerate_after = accelerate_steps + plateau_steps; | |
4cff3ded | 128 | |
5de195be | 129 | this->exit_speed = exitspeed; |
4cff3ded AW |
130 | } |
131 | ||
132 | // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the | |
133 | // given acceleration: | |
1cf31736 JM |
134 | float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration) |
135 | { | |
136 | return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration)); | |
4cff3ded AW |
137 | } |
138 | ||
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) | |
143 | // | |
144 | /* + <- some maximum rate we don't care about | |
145 | /|\ | |
146 | / | \ | |
147 | / | + <- final_rate | |
148 | / | | | |
149 | initial_rate -> +----+--+ | |
150 | ^ ^ | |
151 | | | | |
152 | intersection_distance distance */ | |
1cf31736 JM |
153 | float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance) |
154 | { | |
155 | return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration)); | |
4cff3ded AW |
156 | } |
157 | ||
4cff3ded AW |
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. | |
558e170c | 160 | float Block::max_allowable_speed(float acceleration, float target_velocity, float distance) |
1cf31736 | 161 | { |
a617ac35 | 162 | return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance); |
4cff3ded AW |
163 | } |
164 | ||
165 | ||
166 | // Called by Planner::recalculate() when scanning the plan from last to first entry. | |
a617ac35 | 167 | float Block::reverse_pass(float exit_speed) |
1cf31736 | 168 | { |
a617ac35 MM |
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. | |
1b5776bf | 172 | if (this->entry_speed != this->max_entry_speed) { |
a617ac35 MM |
173 | // If nominal length true, max junction speed is guaranteed to be reached. Only compute |
174 | // for max allowable speed if block is decelerating and nominal length is false. | |
1b5776bf | 175 | if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed)) { |
4fdd2470 | 176 | float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters); |
a617ac35 MM |
177 | |
178 | this->entry_speed = min(max_entry_speed, this->max_entry_speed); | |
179 | ||
180 | return this->entry_speed; | |
1b5776bf | 181 | } else |
a617ac35 MM |
182 | this->entry_speed = this->max_entry_speed; |
183 | } | |
4cff3ded | 184 | |
a617ac35 | 185 | return this->entry_speed; |
aab6cbba | 186 | } |
4cff3ded AW |
187 | |
188 | ||
189 | // Called by Planner::recalculate() when scanning the plan from first to last entry. | |
a617ac35 MM |
190 | // returns maximum exit speed of this block |
191 | float Block::forward_pass(float prev_max_exit_speed) | |
1cf31736 | 192 | { |
aab6cbba AW |
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. | |
a617ac35 MM |
197 | |
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; | |
203 | ||
1b5776bf | 204 | if (prev_max_exit_speed <= entry_speed) { |
a617ac35 MM |
205 | // accel limited |
206 | entry_speed = prev_max_exit_speed; | |
207 | // since we're now acceleration or cruise limited | |
208 | // we don't need to recalculate our entry speed anymore | |
209 | recalculate_flag = false; | |
aab6cbba | 210 | } |
a617ac35 MM |
211 | // else |
212 | // // decel limited, do nothing | |
7b49793d | 213 | |
a617ac35 MM |
214 | return max_exit_speed(); |
215 | } | |
216 | ||
217 | float Block::max_exit_speed() | |
218 | { | |
5de195be MM |
219 | // if block is currently executing, return cached exit speed from calculate_trapezoid |
220 | // this ensures that a block following a currently executing block will have correct entry speed | |
221 | if (times_taken) | |
222 | return exit_speed; | |
223 | ||
a617ac35 MM |
224 | // if nominal_length_flag is asserted |
225 | // we are guaranteed to reach nominal speed regardless of entry speed | |
226 | // thus, max exit will always be nominal | |
227 | if (nominal_length_flag) | |
228 | return nominal_speed; | |
229 | ||
230 | // otherwise, we have to work out max exit speed based on entry and acceleration | |
4fdd2470 | 231 | float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters); |
a617ac35 MM |
232 | |
233 | return min(max, nominal_speed); | |
4cff3ded AW |
234 | } |
235 | ||
4cff3ded | 236 | // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it |
2134bcf2 | 237 | void Block::append_gcode(Gcode* gcode) |
1cf31736 | 238 | { |
1cf31736 | 239 | Gcode new_gcode = *gcode; |
fb7956a9 | 240 | new_gcode.strip_parameters(); // optimization to save memory we strip off the XYZIJK parameters from the saved command |
2134bcf2 | 241 | gcodes.push_back(new_gcode); |
4cff3ded AW |
242 | } |
243 | ||
2134bcf2 | 244 | void Block::begin() |
1cf31736 | 245 | { |
2134bcf2 | 246 | recalculate_flag = false; |
a617ac35 | 247 | |
9d005957 MM |
248 | if (!is_ready) |
249 | __debugbreak(); | |
250 | ||
f2bb3f9f MM |
251 | times_taken = -1; |
252 | ||
2134bcf2 MM |
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])); | |
256 | ||
9e089978 | 257 | |
2134bcf2 | 258 | THEKERNEL->call_event(ON_BLOCK_BEGIN, this); |
1366cafd | 259 | |
f2bb3f9f | 260 | if (times_taken < 0) |
1366cafd | 261 | release(); |
4cff3ded AW |
262 | } |
263 | ||
3fceb8eb | 264 | // Signal the conveyor that this block is ready to be injected into the system |
1cf31736 JM |
265 | void Block::ready() |
266 | { | |
13e4a3f9 | 267 | this->is_ready = true; |
3a4fa0c1 AW |
268 | } |
269 | ||
270 | // Mark the block as taken by one more module | |
1cf31736 JM |
271 | void Block::take() |
272 | { | |
f2bb3f9f MM |
273 | if (times_taken < 0) |
274 | times_taken = 0; | |
275 | times_taken++; | |
3a4fa0c1 | 276 | } |
4cff3ded | 277 | |
3a4fa0c1 | 278 | // Mark the block as no longer taken by one module, go to next block if this free's it |
1cf31736 JM |
279 | void Block::release() |
280 | { | |
3b1acdaa | 281 | if (--this->times_taken <= 0) { |
9d005957 | 282 | times_taken = 0; |
3b1acdaa | 283 | if (is_ready) { |
9d005957 MM |
284 | is_ready = false; |
285 | THEKERNEL->call_event(ON_BLOCK_END, this); | |
06a96473 | 286 | |
9d005957 MM |
287 | // ensure conveyor gets called last |
288 | THEKERNEL->conveyor->on_block_end(this); | |
289 | } | |
d5a58071 | 290 | } |
3a4fa0c1 | 291 | } |