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" | |
dc872d90 | 11 | #include <cmath> |
4cff3ded AW |
12 | #include <string> |
13 | #include "Block.h" | |
14 | #include "Planner.h" | |
3fceb8eb | 15 | #include "Conveyor.h" |
9d005957 | 16 | #include "Gcode.h" |
61134a65 | 17 | #include "libs/StreamOutputPool.h" |
8b260c2c | 18 | #include "StepTicker.h" |
9840888d | 19 | #include "platform_memory.h" |
9d005957 MM |
20 | |
21 | #include "mri.h" | |
7671f21c | 22 | #include <inttypes.h> |
9d005957 | 23 | |
4cff3ded AW |
24 | using std::string; |
25 | #include <vector> | |
4cff3ded | 26 | |
8b260c2c | 27 | #define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency() |
8b260c2c | 28 | |
8a9f9313 | 29 | uint8_t Block::n_actuators= 0; |
c0f10f7d | 30 | double Block::fp_scale= 0; |
8a9f9313 | 31 | |
edac9072 AW |
32 | // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves. |
33 | // It's stacked on a queue, and that queue is then executed in order, to move the motors. | |
34 | // Most of the accel math is also done in this class | |
35 | // And GCode objects for use in on_gcode_execute are also help in here | |
36 | ||
1cf31736 JM |
37 | Block::Block() |
38 | { | |
9840888d | 39 | tick_info= nullptr; |
1cf31736 JM |
40 | clear(); |
41 | } | |
42 | ||
c0f10f7d JM |
43 | void Block::init(uint8_t n) |
44 | { | |
45 | n_actuators= n; | |
46 | fp_scale= (double)STEPTICKER_FPSCALE / pow((double)STEP_TICKER_FREQUENCY, 2.0); // we scale up by fixed point offset first to avoid tiny values | |
47 | } | |
48 | ||
1cf31736 JM |
49 | void Block::clear() |
50 | { | |
e70b6417 JM |
51 | is_ready = false; |
52 | ||
807b9b57 | 53 | this->steps.fill(0); |
1cf31736 | 54 | |
f539c22f | 55 | steps_event_count = 0; |
1598a726 | 56 | nominal_rate = 0.0F; |
f539c22f MM |
57 | nominal_speed = 0.0F; |
58 | millimeters = 0.0F; | |
59 | entry_speed = 0.0F; | |
f6542ad9 | 60 | exit_speed = 0.0F; |
374d0777 | 61 | acceleration = 100.0F; // we don't want to get divide by zeroes if this is not set |
1598a726 | 62 | initial_rate = 0.0F; |
f539c22f MM |
63 | accelerate_until = 0; |
64 | decelerate_after = 0; | |
65 | direction_bits = 0; | |
66 | recalculate_flag = false; | |
67 | nominal_length_flag = false; | |
68 | max_entry_speed = 0.0F; | |
f6542ad9 | 69 | is_ticking = false; |
23201534 | 70 | is_g123 = false; |
f6542ad9 | 71 | locked = false; |
23201534 | 72 | s_value = 0.0F; |
9e6014a6 | 73 | |
8b260c2c | 74 | total_move_ticks= 0; |
9840888d | 75 | if(tick_info == nullptr) { |
4f31b309 JM |
76 | // we create this once for this block |
77 | tick_info= (tickinfo_t *)malloc(sizeof(tickinfo_t) * n_actuators); | |
781dcb02 JM |
78 | if(tick_info == nullptr) { |
79 | // if we ran out of memory in AHB0 just stop here | |
80 | __debugbreak(); | |
81 | } | |
8a9f9313 | 82 | } |
9840888d JM |
83 | |
84 | for(int i = 0; i < n_actuators; ++i) { | |
85 | tick_info[i].steps_per_tick= 0; | |
86 | tick_info[i].counter= 0; | |
87 | tick_info[i].acceleration_change= 0; | |
88 | tick_info[i].deceleration_change= 0; | |
89 | tick_info[i].plateau_rate= 0; | |
90 | tick_info[i].steps_to_move= 0; | |
91 | tick_info[i].step_count= 0; | |
92 | tick_info[i].next_accel_event= 0; | |
a19a873f | 93 | } |
4cff3ded AW |
94 | } |
95 | ||
433d636f | 96 | void Block::debug() const |
1cf31736 | 97 | { |
05b3de04 | 98 | THEKERNEL->streams->printf("%p: steps-X:%lu Y:%lu Z:%lu ", this, this->steps[0], this->steps[1], this->steps[2]); |
8a9f9313 | 99 | for (size_t i = E_AXIS; i < n_actuators; ++i) { |
e571e24f | 100 | THEKERNEL->streams->printf("%c:%lu ", 'A' + i-E_AXIS, this->steps[i]); |
374d0777 | 101 | } |
e571e24f | 102 | THEKERNEL->streams->printf("(max:%lu) nominal:r%1.4f/s%1.4f mm:%1.4f acc:%1.2f accu:%lu decu:%lu ticks:%lu rates:%1.4f/%1.4f entry/max:%1.4f/%1.4f exit:%1.4f primary:%d ready:%d locked:%d ticking:%d recalc:%d nomlen:%d time:%f\r\n", |
1b5776bf JM |
103 | this->steps_event_count, |
104 | this->nominal_rate, | |
105 | this->nominal_speed, | |
106 | this->millimeters, | |
df56baf2 | 107 | this->acceleration, |
1b5776bf JM |
108 | this->accelerate_until, |
109 | this->decelerate_after, | |
05b3de04 | 110 | this->total_move_ticks, |
1b5776bf | 111 | this->initial_rate, |
e571e24f | 112 | this->maximum_rate, |
1b5776bf JM |
113 | this->entry_speed, |
114 | this->max_entry_speed, | |
05b3de04 | 115 | this->exit_speed, |
f41bc212 | 116 | this->primary_axis, |
1b5776bf | 117 | this->is_ready, |
a19a873f JM |
118 | this->locked, |
119 | this->is_ticking, | |
1b5776bf | 120 | recalculate_flag ? 1 : 0, |
121844b7 JM |
121 | nominal_length_flag ? 1 : 0, |
122 | total_move_ticks/STEP_TICKER_FREQUENCY | |
1b5776bf | 123 | ); |
4cff3ded AW |
124 | } |
125 | ||
126 | ||
69735c09 | 127 | /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. |
4cff3ded AW |
128 | // The factors represent a factor of braking and must be in the range 0.0-1.0. |
129 | // +--------+ <- nominal_rate | |
130 | // / \ | |
131 | // nominal_rate*entry_factor -> + \ | |
132 | // | + <- nominal_rate*exit_factor | |
133 | // +-------------+ | |
134 | // time --> | |
edac9072 | 135 | */ |
a617ac35 | 136 | void Block::calculate_trapezoid( float entryspeed, float exitspeed ) |
1cf31736 | 137 | { |
f6542ad9 JM |
138 | // if block is currently executing, don't touch anything! |
139 | if (is_ticking) return; | |
140 | ||
8b260c2c JM |
141 | float initial_rate = this->nominal_rate * (entryspeed / this->nominal_speed); // steps/sec |
142 | float final_rate = this->nominal_rate * (exitspeed / this->nominal_speed); | |
143 | //printf("Initial rate: %f, final_rate: %f\n", initial_rate, final_rate); | |
144 | // How many steps ( can be fractions of steps, we need very precise values ) to accelerate and decelerate | |
145 | // This is a simplification to get rid of rate_delta and get the steps/s² accel directly from the mm/s² accel | |
146 | float acceleration_per_second = (this->acceleration * this->steps_event_count) / this->millimeters; | |
147 | ||
148 | float maximum_possible_rate = sqrtf( ( this->steps_event_count * acceleration_per_second ) + ( ( powf(initial_rate, 2) + powf(final_rate, 2) ) / 2.0F ) ); | |
149 | ||
150 | //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); | |
151 | ||
152 | // Now this is the maximum rate we'll achieve this move, either because | |
153 | // it's the higher we can achieve, or because it's the higher we are | |
154 | // allowed to achieve | |
1ae56063 | 155 | this->maximum_rate = std::min(maximum_possible_rate, this->nominal_rate); |
8b260c2c JM |
156 | |
157 | // Now figure out how long it takes to accelerate in seconds | |
158 | float time_to_accelerate = ( this->maximum_rate - initial_rate ) / acceleration_per_second; | |
159 | ||
160 | // Now figure out how long it takes to decelerate | |
161 | float time_to_decelerate = ( final_rate - this->maximum_rate ) / -acceleration_per_second; | |
162 | ||
163 | // Now we know how long it takes to accelerate and decelerate, but we must | |
164 | // also know how long the entire move takes so we can figure out how long | |
165 | // is the plateau if there is one | |
166 | float plateau_time = 0; | |
167 | ||
168 | // Only if there is actually a plateau ( we are limited by nominal_rate ) | |
169 | if(maximum_possible_rate > this->nominal_rate) { | |
170 | // Figure out the acceleration and deceleration distances ( in steps ) | |
171 | float acceleration_distance = ( ( initial_rate + this->maximum_rate ) / 2.0F ) * time_to_accelerate; | |
172 | float deceleration_distance = ( ( this->maximum_rate + final_rate ) / 2.0F ) * time_to_decelerate; | |
173 | ||
174 | // Figure out the plateau steps | |
175 | float plateau_distance = this->steps_event_count - acceleration_distance - deceleration_distance; | |
176 | ||
177 | // Figure out the plateau time in seconds | |
178 | plateau_time = plateau_distance / this->maximum_rate; | |
1cf31736 | 179 | } |
4cff3ded | 180 | |
8b260c2c JM |
181 | // Figure out how long the move takes total ( in seconds ) |
182 | float total_move_time = time_to_accelerate + time_to_decelerate + plateau_time; | |
183 | //puts "total move time: #{total_move_time}s time to accelerate: #{time_to_accelerate}, time to decelerate: #{time_to_decelerate}" | |
184 | ||
185 | // We now have the full timing for acceleration, plateau and deceleration, | |
186 | // yay \o/ Now this is very important these are in seconds, and we need to | |
187 | // round them into ticks. This means instead of accelerating in 100.23 | |
188 | // ticks we'll accelerate in 100 ticks. Which means to reach the exact | |
189 | // speed we want to reach, we must figure out a new/slightly different | |
190 | // acceleration/deceleration to be sure we accelerate and decelerate at | |
191 | // the exact rate we want | |
192 | ||
193 | // First off round total time, acceleration time and deceleration time in ticks | |
194 | uint32_t acceleration_ticks = floorf( time_to_accelerate * STEP_TICKER_FREQUENCY ); | |
195 | uint32_t deceleration_ticks = floorf( time_to_decelerate * STEP_TICKER_FREQUENCY ); | |
196 | uint32_t total_move_ticks = floorf( total_move_time * STEP_TICKER_FREQUENCY ); | |
197 | ||
198 | // Now deduce the plateau time for those new values expressed in tick | |
199 | //uint32_t plateau_ticks = total_move_ticks - acceleration_ticks - deceleration_ticks; | |
200 | ||
201 | // Now we figure out the acceleration value to reach EXACTLY maximum_rate(steps/s) in EXACTLY acceleration_ticks(ticks) amount of time in seconds | |
202 | 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 | |
203 | float deceleration_time = deceleration_ticks / STEP_TICKER_FREQUENCY; | |
204 | ||
205 | float acceleration_in_steps = (acceleration_time > 0.0F ) ? ( this->maximum_rate - initial_rate ) / acceleration_time : 0; | |
206 | float deceleration_in_steps = (deceleration_time > 0.0F ) ? ( this->maximum_rate - final_rate ) / deceleration_time : 0; | |
207 | ||
f6542ad9 JM |
208 | // we have a potential race condition here as we could get interrupted anywhere in the middle of this call, we need to lock |
209 | // the updates to the blocks to get around it | |
210 | this->locked= true; | |
8b260c2c JM |
211 | // Now figure out the two acceleration ramp change events in ticks |
212 | this->accelerate_until = acceleration_ticks; | |
213 | this->decelerate_after = total_move_ticks - deceleration_ticks; | |
214 | ||
8b260c2c JM |
215 | // We now have everything we need for this block to call a Steppermotor->move method !!!! |
216 | // Theorically, if accel is done per tick, the speed curve should be perfect. | |
8b260c2c JM |
217 | this->total_move_ticks = total_move_ticks; |
218 | ||
8b260c2c | 219 | this->initial_rate = initial_rate; |
f6542ad9 JM |
220 | this->exit_speed = exitspeed; |
221 | ||
222 | // prepare the block for stepticker | |
dc872d90 | 223 | this->prepare(acceleration_in_steps, deceleration_in_steps); |
7671f21c | 224 | |
f6542ad9 | 225 | this->locked= false; |
4cff3ded AW |
226 | } |
227 | ||
4cff3ded AW |
228 | // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the |
229 | // acceleration within the allotted distance. | |
558e170c | 230 | float Block::max_allowable_speed(float acceleration, float target_velocity, float distance) |
1cf31736 | 231 | { |
a617ac35 | 232 | return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance); |
4cff3ded AW |
233 | } |
234 | ||
4cff3ded | 235 | // Called by Planner::recalculate() when scanning the plan from last to first entry. |
a617ac35 | 236 | float Block::reverse_pass(float exit_speed) |
1cf31736 | 237 | { |
a617ac35 MM |
238 | // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. |
239 | // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and | |
240 | // check for maximum allowable speed reductions to ensure maximum possible planned speed. | |
1b5776bf | 241 | if (this->entry_speed != this->max_entry_speed) { |
a617ac35 MM |
242 | // If nominal length true, max junction speed is guaranteed to be reached. Only compute |
243 | // for max allowable speed if block is decelerating and nominal length is false. | |
1b5776bf | 244 | if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed)) { |
4fdd2470 | 245 | float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters); |
a617ac35 MM |
246 | |
247 | this->entry_speed = min(max_entry_speed, this->max_entry_speed); | |
248 | ||
249 | return this->entry_speed; | |
1b5776bf | 250 | } else |
a617ac35 MM |
251 | this->entry_speed = this->max_entry_speed; |
252 | } | |
4cff3ded | 253 | |
a617ac35 | 254 | return this->entry_speed; |
aab6cbba | 255 | } |
4cff3ded AW |
256 | |
257 | ||
258 | // Called by Planner::recalculate() when scanning the plan from first to last entry. | |
a617ac35 MM |
259 | // returns maximum exit speed of this block |
260 | float Block::forward_pass(float prev_max_exit_speed) | |
1cf31736 | 261 | { |
aab6cbba AW |
262 | // If the previous block is an acceleration block, but it is not long enough to complete the |
263 | // full speed change within the block, we need to adjust the entry speed accordingly. Entry | |
264 | // speeds have already been reset, maximized, and reverse planned by reverse planner. | |
265 | // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. | |
a617ac35 MM |
266 | |
267 | // TODO: find out if both of these checks are necessary | |
268 | if (prev_max_exit_speed > nominal_speed) | |
269 | prev_max_exit_speed = nominal_speed; | |
270 | if (prev_max_exit_speed > max_entry_speed) | |
271 | prev_max_exit_speed = max_entry_speed; | |
272 | ||
1b5776bf | 273 | if (prev_max_exit_speed <= entry_speed) { |
a617ac35 MM |
274 | // accel limited |
275 | entry_speed = prev_max_exit_speed; | |
276 | // since we're now acceleration or cruise limited | |
277 | // we don't need to recalculate our entry speed anymore | |
278 | recalculate_flag = false; | |
aab6cbba | 279 | } |
a617ac35 MM |
280 | // else |
281 | // // decel limited, do nothing | |
7b49793d | 282 | |
a617ac35 MM |
283 | return max_exit_speed(); |
284 | } | |
285 | ||
286 | float Block::max_exit_speed() | |
287 | { | |
5de195be MM |
288 | // if block is currently executing, return cached exit speed from calculate_trapezoid |
289 | // this ensures that a block following a currently executing block will have correct entry speed | |
f6542ad9 JM |
290 | if(is_ticking) |
291 | return this->exit_speed; | |
5de195be | 292 | |
a617ac35 MM |
293 | // if nominal_length_flag is asserted |
294 | // we are guaranteed to reach nominal speed regardless of entry speed | |
295 | // thus, max exit will always be nominal | |
296 | if (nominal_length_flag) | |
297 | return nominal_speed; | |
298 | ||
299 | // otherwise, we have to work out max exit speed based on entry and acceleration | |
4fdd2470 | 300 | float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters); |
a617ac35 MM |
301 | |
302 | return min(max, nominal_speed); | |
4cff3ded AW |
303 | } |
304 | ||
f6542ad9 | 305 | // prepare block for the step ticker, called everytime the block changes |
8a9f9313 | 306 | // this is done during planning so does not delay tick generation and step ticker can simply grab the next block during the interrupt |
dc872d90 | 307 | void Block::prepare(float acceleration_in_steps, float deceleration_in_steps) |
f6542ad9 | 308 | { |
c0f10f7d | 309 | |
f6542ad9 | 310 | float inv = 1.0F / this->steps_event_count; |
c0f10f7d | 311 | |
df5f32d9 JM |
312 | // Now figure out the acceleration PER TICK, this should ideally be held as a double as it's very critical to the block timing |
313 | // steps/tick^2 | |
c0f10f7d JM |
314 | // was.... |
315 | // float acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2; // that is 100,000² too big for a float | |
316 | // float deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2; | |
317 | double acceleration_per_tick = acceleration_in_steps * fp_scale; // this is now scaled to fit a 2.30 fixed point number | |
318 | double deceleration_per_tick = deceleration_in_steps * fp_scale; | |
df5f32d9 | 319 | |
8a9f9313 | 320 | for (uint8_t m = 0; m < n_actuators; m++) { |
f6542ad9 JM |
321 | uint32_t steps = this->steps[m]; |
322 | this->tick_info[m].steps_to_move = steps; | |
323 | if(steps == 0) continue; | |
324 | ||
325 | float aratio = inv * steps; | |
dc872d90 JM |
326 | |
327 | this->tick_info[m].steps_per_tick = (int64_t)round((((double)this->initial_rate * aratio) / STEP_TICKER_FREQUENCY) * STEPTICKER_FPSCALE); // steps/sec / tick frequency to get steps per tick in 2.62 fixed point | |
60e250af | 328 | this->tick_info[m].counter = 0; // 2.62 fixed point |
f6542ad9 JM |
329 | this->tick_info[m].step_count = 0; |
330 | this->tick_info[m].next_accel_event = this->total_move_ticks + 1; | |
331 | ||
7671f21c | 332 | double acceleration_change = 0; |
f6542ad9 JM |
333 | if(this->accelerate_until != 0) { // If the next accel event is the end of accel |
334 | this->tick_info[m].next_accel_event = this->accelerate_until; | |
dc872d90 | 335 | acceleration_change = acceleration_per_tick; |
f6542ad9 JM |
336 | |
337 | } else if(this->decelerate_after == 0 /*&& this->accelerate_until == 0*/) { | |
338 | // we start off decelerating | |
dc872d90 | 339 | acceleration_change = -deceleration_per_tick; |
f6542ad9 JM |
340 | |
341 | } else if(this->decelerate_after != this->total_move_ticks /*&& this->accelerate_until == 0*/) { | |
342 | // If the next event is the start of decel ( don't set this if the next accel event is accel end ) | |
343 | this->tick_info[m].next_accel_event = this->decelerate_after; | |
344 | } | |
345 | ||
c0f10f7d | 346 | // already converted to fixed point just needs scaling by ratio |
dc872d90 | 347 | //#define STEPTICKER_TOFP(x) ((int64_t)round((double)(x)*STEPTICKER_FPSCALE)) |
c0f10f7d JM |
348 | this->tick_info[m].acceleration_change= (int64_t)round(acceleration_change * aratio); |
349 | this->tick_info[m].deceleration_change= -(int64_t)round(deceleration_per_tick * aratio); | |
dc872d90 | 350 | this->tick_info[m].plateau_rate= (int64_t)round(((this->maximum_rate * aratio) / STEP_TICKER_FREQUENCY) * STEPTICKER_FPSCALE); |
7671f21c JM |
351 | |
352 | #if 0 | |
353 | THEKERNEL->streams->printf("spt: %08lX %08lX, ac: %08lX %08lX, dc: %08lX %08lX, pr: %08lX %08lX\n", | |
354 | (uint32_t)(this->tick_info[m].steps_per_tick>>32), // 2.62 fixed point | |
355 | (uint32_t)(this->tick_info[m].steps_per_tick&0xFFFFFFFF), // 2.62 fixed point | |
356 | (uint32_t)(this->tick_info[m].acceleration_change>>32), // 2.62 fixed point signed | |
357 | (uint32_t)(this->tick_info[m].acceleration_change&0xFFFFFFFF), // 2.62 fixed point signed | |
358 | (uint32_t)(this->tick_info[m].deceleration_change>>32), // 2.62 fixed point | |
359 | (uint32_t)(this->tick_info[m].deceleration_change&0xFFFFFFFF), // 2.62 fixed point | |
360 | (uint32_t)(this->tick_info[m].plateau_rate>>32), // 2.62 fixed point | |
361 | (uint32_t)(this->tick_info[m].plateau_rate&0xFFFFFFFF) // 2.62 fixed point | |
362 | ); | |
363 | #endif | |
f6542ad9 JM |
364 | } |
365 | } | |
4245f826 JM |
366 | |
367 | // returns current rate (steps/sec) for the given actuator | |
368 | float Block::get_trapezoid_rate(int i) const | |
369 | { | |
370 | // convert steps per tick from fixed point to float and convert to steps/sec | |
371 | // FIXME steps_per_tick can change at any time, potential race condition if it changes while being read here | |
372 | return STEPTICKER_FROMFP(tick_info[i].steps_per_tick) * STEP_TICKER_FREQUENCY; | |
373 | } |