Block.cpp

00001 /*
00002       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 
00008 #include "libs/Module.h"
00009 #include "libs/Kernel.h"
00010 #include "libs/nuts_bolts.h"
00011 #include <math.h>
00012 #include <string>
00013 #include "Block.h"
00014 #include "Planner.h"
00015 #include "Conveyor.h"
00016 #include "Gcode.h"
00017 
00018 #include "mri.h"
00019 
00020 using std::string;
00021 #include <vector>
00022 
00023 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
00024 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
00025 // Most of the accel math is also done in this class
00026 // And GCode objects for use in on_gcode_execute are also help in here
00027 
00028 Block::Block()
00029 {
00030     clear();
00031 }
00032 
00033 void Block::clear()
00034 {
00035     //commands.clear();
00036     //travel_distances.clear();
00037     gcodes.clear();
00038     clear_vector(this->steps);
00039 
00040     steps_event_count   = 0;
00041     nominal_rate        = 0;
00042     nominal_speed       = 0.0F;
00043     millimeters         = 0.0F;
00044     entry_speed         = 0.0F;
00045     exit_speed          = 0.0F;
00046     rate_delta          = 0.0F;
00047     initial_rate        = -1;
00048     final_rate          = -1;
00049     accelerate_until    = 0;
00050     decelerate_after    = 0;
00051     direction_bits      = 0;
00052     recalculate_flag    = false;
00053     nominal_length_flag = false;
00054     max_entry_speed     = 0.0F;
00055     is_ready            = false;
00056     times_taken         = 0;
00057 }
00058 
00059 void Block::debug()
00060 {
00061     THEKERNEL->streams->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",
00062                                this,
00063                                          this->steps[0],
00064                                                this->steps[1],
00065                                                       this->steps[2],
00066                                                                this->steps_event_count,
00067                                                                              this->nominal_rate,
00068                                                                                    this->nominal_speed,
00069                                                                                             this->millimeters,
00070                                                                                                          this->rate_delta,
00071                                                                                                                  this->accelerate_until,
00072                                                                                                                          this->decelerate_after,
00073                                                                                                                                    this->initial_rate,
00074                                                                                                                                         this->final_rate,
00075                                                                                                                                                           this->entry_speed,
00076                                                                                                                                                                 this->max_entry_speed,
00077                                                                                                                                                                              this->times_taken,
00078                                                                                                                                                                                       this->is_ready,
00079                                                                                                                                                                                                 recalculate_flag?1:0,
00080                                                                                                                                                                                                           nominal_length_flag?1:0
00081                              );
00082 }
00083 
00084 
00085 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
00086 // The factors represent a factor of braking and must be in the range 0.0-1.0.
00087 //                                +--------+ <- nominal_rate
00088 //                               /          \
00089 // nominal_rate*entry_factor -> +            \
00090 //                              |             + <- nominal_rate*exit_factor
00091 //                              +-------------+
00092 //                                  time -->
00093 */
00094 void Block::calculate_trapezoid( float entryspeed, float exitspeed )
00095 {
00096     // if block is currently executing, don't touch anything!
00097     if (times_taken)
00098         return;
00099 
00100     // The planner passes us factors, we need to transform them in rates
00101     this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed);   // (step/s)
00102     this->final_rate   = ceil(this->nominal_rate * exitspeed  / this->nominal_speed);   // (step/s)
00103 
00104     // How many steps to accelerate and decelerate
00105     float acceleration_per_second = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second; // ( step/s^2)
00106     int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) );
00107     int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_second ) );
00108 
00109     // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
00110     int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
00111 
00112     // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
00113     // have to use intersection_distance() to calculate when to abort acceleration and start braking
00114     // in order to reach the final_rate exactly at the end of this block.
00115     if (plateau_steps < 0) {
00116         accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count));
00117         accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
00118         accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
00119         plateau_steps = 0;
00120     }
00121     this->accelerate_until = accelerate_steps;
00122     this->decelerate_after = accelerate_steps + plateau_steps;
00123 
00124     this->exit_speed = exitspeed;
00125 }
00126 
00127 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
00128 // given acceleration:
00129 float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration)
00130 {
00131     return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration));
00132 }
00133 
00134 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
00135 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
00136 // a total travel of distance. This can be used to compute the intersection point between acceleration and
00137 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
00138 //
00139 /*                          + <- some maximum rate we don't care about
00140                            /|\
00141                           / | \
00142                          /  |  + <- final_rate
00143                         /   |  |
00144        initial_rate -> +----+--+
00145                             ^ ^
00146                             | |
00147         intersection_distance distance */
00148 float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance)
00149 {
00150     return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration));
00151 }
00152 
00153 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
00154 // acceleration within the allotted distance.
00155 inline float max_allowable_speed(float acceleration, float target_velocity, float distance)
00156 {
00157     return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance);
00158 }
00159 
00160 
00161 // Called by Planner::recalculate() when scanning the plan from last to first entry.
00162 float Block::reverse_pass(float exit_speed)
00163 {
00164     // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
00165     // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
00166     // check for maximum allowable speed reductions to ensure maximum possible planned speed.
00167     if (this->entry_speed != this->max_entry_speed)
00168     {
00169         // If nominal length true, max junction speed is guaranteed to be reached. Only compute
00170         // for max allowable speed if block is decelerating and nominal length is false.
00171         if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed))
00172         {
00173             float max_entry_speed = max_allowable_speed(-THEKERNEL->planner->acceleration, exit_speed, this->millimeters);
00174 
00175             this->entry_speed = min(max_entry_speed, this->max_entry_speed);
00176 
00177             return this->entry_speed;
00178         }
00179         else
00180             this->entry_speed = this->max_entry_speed;
00181     }
00182 
00183     return this->entry_speed;
00184 }
00185 
00186 
00187 // Called by Planner::recalculate() when scanning the plan from first to last entry.
00188 // returns maximum exit speed of this block
00189 float Block::forward_pass(float prev_max_exit_speed)
00190 {
00191     // If the previous block is an acceleration block, but it is not long enough to complete the
00192     // full speed change within the block, we need to adjust the entry speed accordingly. Entry
00193     // speeds have already been reset, maximized, and reverse planned by reverse planner.
00194     // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
00195 
00196     // TODO: find out if both of these checks are necessary
00197     if (prev_max_exit_speed > nominal_speed)
00198         prev_max_exit_speed = nominal_speed;
00199     if (prev_max_exit_speed > max_entry_speed)
00200         prev_max_exit_speed = max_entry_speed;
00201 
00202     if (prev_max_exit_speed <= entry_speed)
00203     {
00204         // accel limited
00205         entry_speed = prev_max_exit_speed;
00206         // since we're now acceleration or cruise limited
00207         // we don't need to recalculate our entry speed anymore
00208         recalculate_flag = false;
00209     }
00210     // else
00211     // // decel limited, do nothing
00212 
00213     return max_exit_speed();
00214 }
00215 
00216 float Block::max_exit_speed()
00217 {
00218     // if block is currently executing, return cached exit speed from calculate_trapezoid
00219     // this ensures that a block following a currently executing block will have correct entry speed
00220     if (times_taken)
00221         return exit_speed;
00222 
00223     // if nominal_length_flag is asserted
00224     // we are guaranteed to reach nominal speed regardless of entry speed
00225     // thus, max exit will always be nominal
00226     if (nominal_length_flag)
00227         return nominal_speed;
00228 
00229     // otherwise, we have to work out max exit speed based on entry and acceleration
00230     float max = max_allowable_speed(-THEKERNEL->planner->acceleration, this->entry_speed, this->millimeters);
00231 
00232     return min(max, nominal_speed);
00233 }
00234 
00235 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
00236 void Block::append_gcode(Gcode* gcode)
00237 {
00238     Gcode new_gcode = *gcode;
00239     gcodes.push_back(new_gcode);
00240 }
00241 
00242 void Block::begin()
00243 {
00244     recalculate_flag = false;
00245 
00246     if (!is_ready)
00247         __debugbreak();
00248 
00249     times_taken = -1;
00250 
00251     // execute all the gcodes related to this block
00252     for(unsigned int index = 0; index < gcodes.size(); index++)
00253         THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index]));
00254 
00255     THEKERNEL->call_event(ON_BLOCK_BEGIN, this);
00256 
00257     if (times_taken < 0)
00258         release();
00259 }
00260 
00261 // Signal the conveyor that this block is ready to be injected into the system
00262 void Block::ready()
00263 {
00264     this->is_ready = true;
00265 }
00266 
00267 // Mark the block as taken by one more module
00268 void Block::take()
00269 {
00270     if (times_taken < 0)
00271         times_taken = 0;
00272     times_taken++;
00273 }
00274 
00275 // Mark the block as no longer taken by one module, go to next block if this free's it
00276 void Block::release()
00277 {
00278     if (--this->times_taken <= 0)
00279     {
00280         times_taken = 0;
00281         if (is_ready)
00282         {
00283             is_ready = false;
00284             THEKERNEL->call_event(ON_BLOCK_END, this);
00285 
00286             // ensure conveyor gets called last
00287             THEKERNEL->conveyor->on_block_end(this);
00288         }
00289     }
00290 }