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"
18 #include "../communication/utils/Gcode.h"
20 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
21 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
22 // Most of the accel math is also done in this class
23 // And GCode objects for use in on_gcode_execute are also help in here
33 //travel_distances.clear();
35 clear_vector(this->steps
);
48 recalculate_flag
= false;
49 nominal_length_flag
= false;
50 max_entry_speed
= 0.0F
;
57 THEKERNEL
->serial
->printf("%p: steps:X%04d Y%04d Z%04d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8f acc:%5d dec:%5d rates:%10d>%10d entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
62 this->steps_event_count
,
67 this->accelerate_until
,
68 this->decelerate_after
,
72 this->max_entry_speed
,
76 nominal_length_flag
?1:0
81 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
82 // The factors represent a factor of braking and must be in the range 0.0-1.0.
83 // +--------+ <- nominal_rate
85 // nominal_rate*entry_factor -> + \
86 // | + <- nominal_rate*exit_factor
90 void Block::calculate_trapezoid( float entryspeed
, float exitspeed
)
93 // The planner passes us factors, we need to transform them in rates
94 this->initial_rate
= ceil(this->nominal_rate
* entryspeed
/ this->nominal_speed
); // (step/min)
95 this->final_rate
= ceil(this->nominal_rate
* exitspeed
/ this->nominal_speed
); // (step/min)
97 // How many steps to accelerate and decelerate
98 float acceleration_per_minute
= this->rate_delta
* THEKERNEL
->stepper
->acceleration_ticks_per_second
* 60.0; // ( step/min^2)
99 int accelerate_steps
= ceil( this->estimate_acceleration_distance( this->initial_rate
, this->nominal_rate
, acceleration_per_minute
) );
100 int decelerate_steps
= floor( this->estimate_acceleration_distance( this->nominal_rate
, this->final_rate
, -acceleration_per_minute
) );
102 // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
103 int plateau_steps
= this->steps_event_count
- accelerate_steps
- decelerate_steps
;
105 // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
106 // have to use intersection_distance() to calculate when to abort acceleration and start braking
107 // in order to reach the final_rate exactly at the end of this block.
108 if (plateau_steps
< 0) {
109 accelerate_steps
= ceil(this->intersection_distance(this->initial_rate
, this->final_rate
, acceleration_per_minute
, this->steps_event_count
));
110 accelerate_steps
= max( accelerate_steps
, 0 ); // Check limits due to numerical round-off
111 accelerate_steps
= min( accelerate_steps
, int(this->steps_event_count
) );
114 this->accelerate_until
= accelerate_steps
;
115 this->decelerate_after
= accelerate_steps
+ plateau_steps
;
119 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
120 // given acceleration:
121 float Block::estimate_acceleration_distance(float initialrate
, float targetrate
, float acceleration
)
123 return( ((targetrate
* targetrate
) - (initialrate
* initialrate
)) / (2.0F
* acceleration
));
126 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
127 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
128 // a total travel of distance. This can be used to compute the intersection point between acceleration and
129 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
131 /* + <- some maximum rate we don't care about
136 initial_rate -> +----+--+
139 intersection_distance distance */
140 float Block::intersection_distance(float initialrate
, float finalrate
, float acceleration
, float distance
)
142 return((2 * acceleration
* distance
- initialrate
* initialrate
+ finalrate
* finalrate
) / (4 * acceleration
));
145 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
146 // acceleration within the allotted distance.
147 inline float max_allowable_speed(float acceleration
, float target_velocity
, float distance
)
149 return sqrtf(target_velocity
* target_velocity
- 2.0F
* acceleration
* distance
);
153 // Called by Planner::recalculate() when scanning the plan from last to first entry.
154 float Block::reverse_pass(float exit_speed
)
156 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
157 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
158 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
159 if (this->entry_speed
!= this->max_entry_speed
)
161 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
162 // for max allowable speed if block is decelerating and nominal length is false.
163 if ((!this->nominal_length_flag
) && (this->max_entry_speed
> exit_speed
))
165 float max_entry_speed
= max_allowable_speed(-THEKERNEL
->planner
->acceleration
, exit_speed
, this->millimeters
);
167 this->entry_speed
= min(max_entry_speed
, this->max_entry_speed
);
169 return this->entry_speed
;
172 this->entry_speed
= this->max_entry_speed
;
175 return this->entry_speed
;
179 // Called by Planner::recalculate() when scanning the plan from first to last entry.
180 // returns maximum exit speed of this block
181 float Block::forward_pass(float prev_max_exit_speed
)
183 // If the previous block is an acceleration block, but it is not long enough to complete the
184 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
185 // speeds have already been reset, maximized, and reverse planned by reverse planner.
186 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
188 // TODO: find out if both of these checks are necessary
189 if (prev_max_exit_speed
> nominal_speed
)
190 prev_max_exit_speed
= nominal_speed
;
191 if (prev_max_exit_speed
> max_entry_speed
)
192 prev_max_exit_speed
= max_entry_speed
;
194 if (prev_max_exit_speed
<= entry_speed
)
197 entry_speed
= prev_max_exit_speed
;
198 // since we're now acceleration or cruise limited
199 // we don't need to recalculate our entry speed anymore
200 recalculate_flag
= false;
203 // // decel limited, do nothing
205 return max_exit_speed();
208 float Block::max_exit_speed()
210 // if nominal_length_flag is asserted
211 // we are guaranteed to reach nominal speed regardless of entry speed
212 // thus, max exit will always be nominal
213 if (nominal_length_flag
)
214 return nominal_speed
;
216 // otherwise, we have to work out max exit speed based on entry and acceleration
217 float max
= max_allowable_speed(-THEKERNEL
->planner
->acceleration
, this->entry_speed
, this->millimeters
);
219 return min(max
, nominal_speed
);
222 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
223 void Block::append_gcode(Gcode
* gcode
)
225 Gcode new_gcode
= *gcode
;
226 gcodes
.push_back(new_gcode
);
231 recalculate_flag
= false;
233 // execute all the gcodes related to this block
234 for(unsigned int index
= 0; index
< gcodes
.size(); index
++)
235 THEKERNEL
->call_event(ON_GCODE_EXECUTE
, &(gcodes
[index
]));
237 THEKERNEL
->call_event(ON_BLOCK_BEGIN
, this);
240 // Signal the conveyor that this block is ready to be injected into the system
243 this->is_ready
= true;
246 // Mark the block as taken by one more module
252 // Mark the block as no longer taken by one module, go to next block if this free's it
253 void Block::release()
255 if (--this->times_taken
<= 0)
256 THEKERNEL
->call_event(ON_BLOCK_END
, this);
258 // ensure conveyor gets called last
259 THEKERNEL
->conveyor
->on_block_end(this);