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"
25 #define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency()
26 #define STEP_TICKER_FREQUENCY_2 (STEP_TICKER_FREQUENCY*STEP_TICKER_FREQUENCY)
28 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
29 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
30 // Most of the accel math is also done in this class
31 // And GCode objects for use in on_gcode_execute are also help in here
41 //travel_distances.clear();
43 //std::vector<Gcode>().swap(gcodes); // this resizes the vector releasing its memory
47 steps_event_count
= 0;
53 acceleration
= 100.0F
; // we don't want to get devide by zeroes if this is not set
58 recalculate_flag
= false;
59 nominal_length_flag
= false;
60 max_entry_speed
= 0.0F
;
63 acceleration_per_tick
= 0;
64 deceleration_per_tick
= 0;
68 void Block::debug() const
70 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 ready:%d recalc:%d nomlen:%d time:%f\r\n",
75 this->steps_event_count
,
79 this->accelerate_until
,
80 this->decelerate_after
,
83 this->max_entry_speed
,
85 recalculate_flag
? 1 : 0,
86 nominal_length_flag
? 1 : 0,
87 total_move_ticks
/STEP_TICKER_FREQUENCY
92 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
93 // The factors represent a factor of braking and must be in the range 0.0-1.0.
94 // +--------+ <- nominal_rate
96 // nominal_rate*entry_factor -> + \
97 // | + <- nominal_rate*exit_factor
101 void Block::calculate_trapezoid( float entryspeed
, float exitspeed
)
103 float initial_rate
= this->nominal_rate
* (entryspeed
/ this->nominal_speed
); // steps/sec
104 float final_rate
= this->nominal_rate
* (exitspeed
/ this->nominal_speed
);
105 //printf("Initial rate: %f, final_rate: %f\n", initial_rate, final_rate);
106 // How many steps ( can be fractions of steps, we need very precise values ) to accelerate and decelerate
107 // This is a simplification to get rid of rate_delta and get the steps/s² accel directly from the mm/s² accel
108 float acceleration_per_second
= (this->acceleration
* this->steps_event_count
) / this->millimeters
;
110 float maximum_possible_rate
= sqrtf( ( this->steps_event_count
* acceleration_per_second
) + ( ( powf(initial_rate
, 2) + powf(final_rate
, 2) ) / 2.0F
) );
112 //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);
114 // Now this is the maximum rate we'll achieve this move, either because
115 // it's the higher we can achieve, or because it's the higher we are
116 // allowed to achieve
117 this->maximum_rate
= std::min(maximum_possible_rate
, this->nominal_rate
);
119 // Now figure out how long it takes to accelerate in seconds
120 float time_to_accelerate
= ( this->maximum_rate
- initial_rate
) / acceleration_per_second
;
122 // Now figure out how long it takes to decelerate
123 float time_to_decelerate
= ( final_rate
- this->maximum_rate
) / -acceleration_per_second
;
125 // Now we know how long it takes to accelerate and decelerate, but we must
126 // also know how long the entire move takes so we can figure out how long
127 // is the plateau if there is one
128 float plateau_time
= 0;
130 // Only if there is actually a plateau ( we are limited by nominal_rate )
131 if(maximum_possible_rate
> this->nominal_rate
) {
132 // Figure out the acceleration and deceleration distances ( in steps )
133 float acceleration_distance
= ( ( initial_rate
+ this->maximum_rate
) / 2.0F
) * time_to_accelerate
;
134 float deceleration_distance
= ( ( this->maximum_rate
+ final_rate
) / 2.0F
) * time_to_decelerate
;
136 // Figure out the plateau steps
137 float plateau_distance
= this->steps_event_count
- acceleration_distance
- deceleration_distance
;
139 // Figure out the plateau time in seconds
140 plateau_time
= plateau_distance
/ this->maximum_rate
;
143 // Figure out how long the move takes total ( in seconds )
144 float total_move_time
= time_to_accelerate
+ time_to_decelerate
+ plateau_time
;
145 //puts "total move time: #{total_move_time}s time to accelerate: #{time_to_accelerate}, time to decelerate: #{time_to_decelerate}"
147 // We now have the full timing for acceleration, plateau and deceleration,
148 // yay \o/ Now this is very important these are in seconds, and we need to
149 // round them into ticks. This means instead of accelerating in 100.23
150 // ticks we'll accelerate in 100 ticks. Which means to reach the exact
151 // speed we want to reach, we must figure out a new/slightly different
152 // acceleration/deceleration to be sure we accelerate and decelerate at
153 // the exact rate we want
155 // First off round total time, acceleration time and deceleration time in ticks
156 uint32_t acceleration_ticks
= floorf( time_to_accelerate
* STEP_TICKER_FREQUENCY
);
157 uint32_t deceleration_ticks
= floorf( time_to_decelerate
* STEP_TICKER_FREQUENCY
);
158 uint32_t total_move_ticks
= floorf( total_move_time
* STEP_TICKER_FREQUENCY
);
160 // Now deduce the plateau time for those new values expressed in tick
161 //uint32_t plateau_ticks = total_move_ticks - acceleration_ticks - deceleration_ticks;
163 // Now we figure out the acceleration value to reach EXACTLY maximum_rate(steps/s) in EXACTLY acceleration_ticks(ticks) amount of time in seconds
164 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
165 float deceleration_time
= deceleration_ticks
/ STEP_TICKER_FREQUENCY
;
167 float acceleration_in_steps
= (acceleration_time
> 0.0F
) ? ( this->maximum_rate
- initial_rate
) / acceleration_time
: 0;
168 float deceleration_in_steps
= (deceleration_time
> 0.0F
) ? ( this->maximum_rate
- final_rate
) / deceleration_time
: 0;
170 // Now figure out the two acceleration ramp change events in ticks
171 this->accelerate_until
= acceleration_ticks
;
172 this->decelerate_after
= total_move_ticks
- deceleration_ticks
;
174 // 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
177 this->acceleration_per_tick
= acceleration_in_steps
/ STEP_TICKER_FREQUENCY_2
;
178 this->deceleration_per_tick
= deceleration_in_steps
/ STEP_TICKER_FREQUENCY_2
;
180 // We now have everything we need for this block to call a Steppermotor->move method !!!!
181 // Theorically, if accel is done per tick, the speed curve should be perfect.
183 // We need this to call move()
184 this->total_move_ticks
= total_move_ticks
;
186 //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}"
188 this->initial_rate
= initial_rate
;
189 //this->exit_speed = exitspeed;
192 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
193 // acceleration within the allotted distance.
194 float Block::max_allowable_speed(float acceleration
, float target_velocity
, float distance
)
196 return sqrtf(target_velocity
* target_velocity
- 2.0F
* acceleration
* distance
);
199 // Called by Planner::recalculate() when scanning the plan from last to first entry.
200 float Block::reverse_pass(float exit_speed
)
202 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
203 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
204 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
205 if (this->entry_speed
!= this->max_entry_speed
) {
206 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
207 // for max allowable speed if block is decelerating and nominal length is false.
208 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> exit_speed
)) {
209 float max_entry_speed
= max_allowable_speed(-this->acceleration
, exit_speed
, this->millimeters
);
211 this->entry_speed
= min(max_entry_speed
, this->max_entry_speed
);
213 return this->entry_speed
;
215 this->entry_speed
= this->max_entry_speed
;
218 return this->entry_speed
;
222 // Called by Planner::recalculate() when scanning the plan from first to last entry.
223 // returns maximum exit speed of this block
224 float Block::forward_pass(float prev_max_exit_speed
)
226 // If the previous block is an acceleration block, but it is not long enough to complete the
227 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
228 // speeds have already been reset, maximized, and reverse planned by reverse planner.
229 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
231 // TODO: find out if both of these checks are necessary
232 if (prev_max_exit_speed
> nominal_speed
)
233 prev_max_exit_speed
= nominal_speed
;
234 if (prev_max_exit_speed
> max_entry_speed
)
235 prev_max_exit_speed
= max_entry_speed
;
237 if (prev_max_exit_speed
<= entry_speed
) {
239 entry_speed
= prev_max_exit_speed
;
240 // since we're now acceleration or cruise limited
241 // we don't need to recalculate our entry speed anymore
242 recalculate_flag
= false;
245 // // decel limited, do nothing
247 return max_exit_speed();
250 float Block::max_exit_speed()
252 // if block is currently executing, return cached exit speed from calculate_trapezoid
253 // this ensures that a block following a currently executing block will have correct entry speed
256 // return exit_speed;
258 // if nominal_length_flag is asserted
259 // we are guaranteed to reach nominal speed regardless of entry speed
260 // thus, max exit will always be nominal
261 if (nominal_length_flag
)
262 return nominal_speed
;
264 // otherwise, we have to work out max exit speed based on entry and acceleration
265 float max
= max_allowable_speed(-this->acceleration
, this->entry_speed
, this->millimeters
);
267 return min(max
, nominal_speed
);
270 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
271 // void Block::append_gcode(Gcode* gcode)
273 // Gcode new_gcode = *gcode;
274 // new_gcode.strip_parameters(); // optimization to save memory we strip off the XYZIJK parameters from the saved command
275 // gcodes.push_back(new_gcode);
278 // void Block::begin()
280 // // can no longer be used in planning
281 // recalculate_flag = false;
283 // // TODO probably should remove this
289 // Mark the block as finished
290 //void Block::release()
294 // THEKERNEL->conveyor->on_block_end(this);