src/modules/robot/Block.cpp

   1 /*
   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/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "libs/nuts_bolts.h"
  11 #include <math.h>
  12 #include <string>
  13 #include "Block.h"
  14 #include "Planner.h"
  15 #include "Conveyor.h"
  16 #include "Gcode.h"
  17 #include "libs/StreamOutputPool.h"
  18 #include "Stepper.h"
  19
  20 #include "mri.h"
  21
  22 using std::string;
  23 #include <vector>
  24
  25 #ifdef BLOCK_DEBUG_PIN
  26 #include "gpio.h"
  27 extern GPIO block_debug_pin;
  28 #endif
  29
  30 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
  31 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
  32 // Most of the accel math is also done in this class
  33 // And GCode objects for use in on_gcode_execute are also help in here
  34
  35 Block::Block()
  36 {
  37     clear();
  38 }
  39
  40 void Block::clear()
  41 {
  42     //commands.clear();
  43     //travel_distances.clear();
  44     gcodes.clear();
  45     std::vector<Gcode>().swap(gcodes); // this resizes the vector releasing its memory
  46
  47     clear_vector(this->steps);
  48
  49     steps_event_count   = 0;
  50     nominal_rate        = 0;
  51     nominal_speed       = 0.0F;
  52     millimeters         = 0.0F;
  53     entry_speed         = 0.0F;
  54     exit_speed          = 0.0F;
  55     rate_delta          = 0.0F;
  56     acceleration        = 100.0F; // we don't want to get devide by zeroes if this is not set
  57     initial_rate        = -1;
  58     final_rate          = -1;
  59     accelerate_until    = 0;
  60     decelerate_after    = 0;
  61     direction_bits      = 0;
  62     recalculate_flag    = false;
  63     nominal_length_flag = false;
  64     max_entry_speed     = 0.0F;
  65     is_ready            = false;
  66     times_taken         = 0;
  67 }
  68
  69 void Block::debug()
  70 {
  71     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",
  72                                this,
  73                                          this->steps[0],
  74                                                this->steps[1],
  75                                                       this->steps[2],
  76                                                                this->steps_event_count,
  77                                                                              this->nominal_rate,
  78                                                                                    this->nominal_speed,
  79                                                                                             this->millimeters,
  80                                                                                                          this->rate_delta,
  81                                                                                                                  this->accelerate_until,
  82                                                                                                                          this->decelerate_after,
  83                                                                                                                                    this->initial_rate,
  84                                                                                                                                         this->final_rate,
  85                                                                                                                                                           this->entry_speed,
  86                                                                                                                                                                 this->max_entry_speed,
  87                                                                                                                                                                              this->times_taken,
  88                                                                                                                                                                                       this->is_ready,
  89                                                                                                                                                                                                 recalculate_flag?1:0,
  90                                                                                                                                                                                                           nominal_length_flag?1:0
  91                              );
  92 }
  93
  94
  95 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
  96 // The factors represent a factor of braking and must be in the range 0.0-1.0.
  97 //                                +--------+ <- nominal_rate
  98 //                               /          \
  99 // nominal_rate*entry_factor -> +            \
 100 //                              |             + <- nominal_rate*exit_factor
 101 //                              +-------------+
 102 //                                  time -->
 103 */
 104 void Block::calculate_trapezoid( float entryspeed, float exitspeed )
 105 {
 106     // if block is currently executing, don't touch anything!
 107     if (times_taken)
 108         return;
 109
 110     // The planner passes us factors, we need to transform them in rates
 111     this->initial_rate = ceilf(this->nominal_rate * entryspeed / this->nominal_speed);   // (step/s)
 112     this->final_rate   = ceilf(this->nominal_rate * exitspeed  / this->nominal_speed);   // (step/s)
 113
 114     // How many steps to accelerate and decelerate
 115     float acceleration_per_second = this->rate_delta * THEKERNEL->acceleration_ticks_per_second; // ( step/s^2)
 116     int accelerate_steps = ceilf( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) );
 117     int decelerate_steps = floorf( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_second ) );
 118
 119     // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
 120     int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
 121
 122     // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
 123     // have to use intersection_distance() to calculate when to abort acceleration and start braking
 124     // in order to reach the final_rate exactly at the end of this block.
 125     if (plateau_steps < 0) {
 126         accelerate_steps = ceilf(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count));
 127         accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
 128         accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
 129         plateau_steps = 0;
 130     }
 131     this->accelerate_until = accelerate_steps;
 132     this->decelerate_after = accelerate_steps + plateau_steps;
 133
 134     this->exit_speed = exitspeed;
 135 }
 136
 137 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
 138 // given acceleration:
 139 float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration)
 140 {
 141     return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration));
 142 }
 143
 144 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
 145 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 146 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 147 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
 148 //
 149 /*                          + <- some maximum rate we don't care about
 150                            /|\
 151                           / | \
 152                          /  |  + <- final_rate
 153                         /   |  |
 154        initial_rate -> +----+--+
 155                             ^ ^
 156                             | |
 157         intersection_distance distance */
 158 float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance)
 159 {
 160     return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration));
 161 }
 162
 163 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
 164 // acceleration within the allotted distance.
 165 float Block::max_allowable_speed(float acceleration, float target_velocity, float distance)
 166 {
 167     return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance);
 168 }
 169
 170
 171 // Called by Planner::recalculate() when scanning the plan from last to first entry.
 172 float Block::reverse_pass(float exit_speed)
 173 {
 174     // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
 175     // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
 176     // check for maximum allowable speed reductions to ensure maximum possible planned speed.
 177     if (this->entry_speed != this->max_entry_speed)
 178     {
 179         // If nominal length true, max junction speed is guaranteed to be reached. Only compute
 180         // for max allowable speed if block is decelerating and nominal length is false.
 181         if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed))
 182         {
 183             float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters);
 184
 185             this->entry_speed = min(max_entry_speed, this->max_entry_speed);
 186
 187             return this->entry_speed;
 188         }
 189         else
 190             this->entry_speed = this->max_entry_speed;
 191     }
 192
 193     return this->entry_speed;
 194 }
 195
 196
 197 // Called by Planner::recalculate() when scanning the plan from first to last entry.
 198 // returns maximum exit speed of this block
 199 float Block::forward_pass(float prev_max_exit_speed)
 200 {
 201     // If the previous block is an acceleration block, but it is not long enough to complete the
 202     // full speed change within the block, we need to adjust the entry speed accordingly. Entry
 203     // speeds have already been reset, maximized, and reverse planned by reverse planner.
 204     // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
 205
 206     // TODO: find out if both of these checks are necessary
 207     if (prev_max_exit_speed > nominal_speed)
 208         prev_max_exit_speed = nominal_speed;
 209     if (prev_max_exit_speed > max_entry_speed)
 210         prev_max_exit_speed = max_entry_speed;
 211
 212     if (prev_max_exit_speed <= entry_speed)
 213     {
 214         // accel limited
 215         entry_speed = prev_max_exit_speed;
 216         // since we're now acceleration or cruise limited
 217         // we don't need to recalculate our entry speed anymore
 218         recalculate_flag = false;
 219     }
 220     // else
 221     // // decel limited, do nothing
 222
 223     return max_exit_speed();
 224 }
 225
 226 float Block::max_exit_speed()
 227 {
 228     // if block is currently executing, return cached exit speed from calculate_trapezoid
 229     // this ensures that a block following a currently executing block will have correct entry speed
 230     if (times_taken)
 231         return exit_speed;
 232
 233     // if nominal_length_flag is asserted
 234     // we are guaranteed to reach nominal speed regardless of entry speed
 235     // thus, max exit will always be nominal
 236     if (nominal_length_flag)
 237         return nominal_speed;
 238
 239     // otherwise, we have to work out max exit speed based on entry and acceleration
 240     float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters);
 241
 242     return min(max, nominal_speed);
 243 }
 244
 245 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
 246 void Block::append_gcode(Gcode* gcode)
 247 {
 248     Gcode new_gcode = *gcode;
 249     new_gcode.strip_parameters(); // optimization to save memory we strip off the XYZIJK parameters from the saved command
 250     gcodes.push_back(new_gcode);
 251 }
 252
 253 void Block::begin()
 254 {
 255 #ifdef BLOCK_DEBUG_PIN
 256     block_debug_pin= 1;
 257 #endif
 258
 259     recalculate_flag = false;
 260
 261     if (!is_ready)
 262         __debugbreak();
 263
 264     times_taken = -1;
 265
 266     // execute all the gcodes related to this block
 267     for(unsigned int index = 0; index < gcodes.size(); index++)
 268         THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index]));
 269
 270
 271     THEKERNEL->call_event(ON_BLOCK_BEGIN, this);
 272
 273     if (times_taken < 0)
 274         release();
 275 }
 276
 277 // Signal the conveyor that this block is ready to be injected into the system
 278 void Block::ready()
 279 {
 280     this->is_ready = true;
 281 }
 282
 283 // Mark the block as taken by one more module
 284 void Block::take()
 285 {
 286     if (times_taken < 0)
 287         times_taken = 0;
 288     times_taken++;
 289 }
 290
 291 // Mark the block as no longer taken by one module, go to next block if this free's it
 292 void Block::release()
 293 {
 294     if (--this->times_taken <= 0)
 295     {
 296         times_taken = 0;
 297         if (is_ready)
 298         {
 299 #ifdef BLOCK_DEBUG_PIN
 300             block_debug_pin= 0;
 301 #endif
 302             is_ready = false;
 303             THEKERNEL->call_event(ON_BLOCK_END, this);
 304
 305             // ensure conveyor gets called last
 306             THEKERNEL->conveyor->on_block_end(this);
 307
 308         }
 309     }
 310 }