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"
18 #include "StepTicker.h"
19 #include "platform_memory.h"
27 #define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency()
29 uint8_t Block::n_actuators
= 0;
30 double Block::fp_scale
= 0;
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
43 void Block::init(uint8_t 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
55 steps_event_count
= 0;
61 acceleration
= 100.0F
; // we don't want to get divide by zeroes if this is not set
66 recalculate_flag
= false;
67 nominal_length_flag
= false;
68 max_entry_speed
= 0.0F
;
75 if(tick_info
== nullptr) {
76 // we create this once for this block
77 tick_info
= new tickinfo_t
[n_actuators
]; //(tickinfo_t *)malloc(sizeof(tickinfo_t) * n_actuators);
78 if(tick_info
== nullptr) {
79 // if we ran out of memory in AHB0 just stop here
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;
96 void Block::debug() const
98 THEKERNEL
->streams
->printf("%p: steps-X:%lu Y:%lu Z:%lu ", this, this->steps
[0], this->steps
[1], this->steps
[2]);
99 for (size_t i
= E_AXIS
; i
< n_actuators
; ++i
) {
100 THEKERNEL
->streams
->printf("%c:%lu ", 'A' + i
-E_AXIS
, this->steps
[i
]);
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",
103 this->steps_event_count
,
108 this->accelerate_until
,
109 this->decelerate_after
,
110 this->total_move_ticks
,
114 this->max_entry_speed
,
120 recalculate_flag
? 1 : 0,
121 nominal_length_flag
? 1 : 0,
122 total_move_ticks
/STEP_TICKER_FREQUENCY
127 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
128 // The factors represent a factor of braking and must be in the range 0.0-1.0.
129 // +--------+ <- nominal_rate
131 // nominal_rate*entry_factor -> + \
132 // | + <- nominal_rate*exit_factor
136 void Block::calculate_trapezoid( float entryspeed
, float exitspeed
)
138 // if block is currently executing, don't touch anything!
139 if (is_ticking
) return;
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
;
148 float maximum_possible_rate
= sqrtf( ( this->steps_event_count
* acceleration_per_second
) + ( ( powf(initial_rate
, 2) + powf(final_rate
, 2) ) / 2.0F
) );
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);
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
155 this->maximum_rate
= std::min(maximum_possible_rate
, this->nominal_rate
);
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
;
160 // Now figure out how long it takes to decelerate
161 float time_to_decelerate
= ( final_rate
- this->maximum_rate
) / -acceleration_per_second
;
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;
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
;
174 // Figure out the plateau steps
175 float plateau_distance
= this->steps_event_count
- acceleration_distance
- deceleration_distance
;
177 // Figure out the plateau time in seconds
178 plateau_time
= plateau_distance
/ this->maximum_rate
;
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}"
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
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
);
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;
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
;
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;
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
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
;
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.
217 this->total_move_ticks
= total_move_ticks
;
219 this->initial_rate
= initial_rate
;
220 this->exit_speed
= exitspeed
;
222 // prepare the block for stepticker
223 this->prepare(acceleration_in_steps
, deceleration_in_steps
);
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.
230 float Block::max_allowable_speed(float acceleration
, float target_velocity
, float distance
)
232 return sqrtf(target_velocity
* target_velocity
- 2.0F
* acceleration
* distance
);
235 // Called by Planner::recalculate() when scanning the plan from last to first entry.
236 float Block::reverse_pass(float exit_speed
)
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.
241 if (this->entry_speed
!= this->max_entry_speed
) {
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.
244 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> exit_speed
)) {
245 float max_entry_speed
= max_allowable_speed(-this->acceleration
, exit_speed
, this->millimeters
);
247 this->entry_speed
= min(max_entry_speed
, this->max_entry_speed
);
249 return this->entry_speed
;
251 this->entry_speed
= this->max_entry_speed
;
254 return this->entry_speed
;
258 // Called by Planner::recalculate() when scanning the plan from first to last entry.
259 // returns maximum exit speed of this block
260 float Block::forward_pass(float prev_max_exit_speed
)
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.
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
;
273 if (prev_max_exit_speed
<= entry_speed
) {
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;
281 // // decel limited, do nothing
283 return max_exit_speed();
286 float Block::max_exit_speed()
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
291 return this->exit_speed
;
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
;
299 // otherwise, we have to work out max exit speed based on entry and acceleration
300 float max
= max_allowable_speed(-this->acceleration
, this->entry_speed
, this->millimeters
);
302 return min(max
, nominal_speed
);
305 // prepare block for the step ticker, called everytime the block changes
306 // this is done during planning so does not delay tick generation and step ticker can simply grab the next block during the interrupt
307 void Block::prepare(float acceleration_in_steps
, float deceleration_in_steps
)
310 float inv
= 1.0F
/ this->steps_event_count
;
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
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
;
320 for (uint8_t m
= 0; m
< n_actuators
; m
++) {
321 uint32_t steps
= this->steps
[m
];
322 this->tick_info
[m
].steps_to_move
= steps
;
323 if(steps
== 0) continue;
325 float aratio
= inv
* steps
;
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
328 this->tick_info
[m
].counter
= 0; // 2.62 fixed point
329 this->tick_info
[m
].step_count
= 0;
330 this->tick_info
[m
].next_accel_event
= this->total_move_ticks
+ 1;
332 double acceleration_change
= 0;
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
;
335 acceleration_change
= acceleration_per_tick
;
337 } else if(this->decelerate_after
== 0 /*&& this->accelerate_until == 0*/) {
338 // we start off decelerating
339 acceleration_change
= -deceleration_per_tick
;
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
;
346 // already converted to fixed point just needs scaling by ratio
347 //#define STEPTICKER_TOFP(x) ((int64_t)round((double)(x)*STEPTICKER_FPSCALE))
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
);
350 this->tick_info
[m
].plateau_rate
= (int64_t)round(((this->maximum_rate
* aratio
) / STEP_TICKER_FREQUENCY
) * STEPTICKER_FPSCALE
);
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
367 // returns current rate (steps/sec) for the given actuator
368 float Block::get_trapezoid_rate(int i
) const
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
;