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 using std::string;
  17 #include <vector>
  18 #include "../communication/utils/Gcode.h"
  19
  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
  24
  25 Block::Block()
  26 {
  27     clear();
  28 }
  29
  30 void Block::clear()
  31 {
  32     //commands.clear();
  33     //travel_distances.clear();
  34     gcodes.clear();
  35     clear_vector(this->steps);
  36
  37     steps_event_count= 0;
  38     nominal_rate= 0;
  39     nominal_speed= 0.0F;
  40     millimeters= 0.0F;
  41     entry_speed= 0.0F;
  42     rate_delta= 0.0F;
  43     initial_rate= -1;
  44     final_rate= -1;
  45     accelerate_until= 0;
  46     decelerate_after= 0;
  47     direction_bits= 0;
  48     recalculate_flag= false;
  49     nominal_length_flag= false;
  50     max_entry_speed= 0.0F;
  51     is_ready= false;
  52     times_taken= 0;
  53 }
  54
  55 void Block::debug()
  56 {
  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\r\n",
  58                                this,
  59                                          this->steps[0],
  60                                                this->steps[1],
  61                                                       this->steps[2],
  62                                                                this->steps_event_count,
  63                                                                              this->nominal_rate,
  64                                                                                    this->nominal_speed,
  65                                                                                             this->millimeters,
  66                                                                                                          this->rate_delta,
  67                                                                                                                  this->accelerate_until,
  68                                                                                                                          this->decelerate_after,
  69                                                                                                                                    this->initial_rate,
  70                                                                                                                                         this->final_rate,
  71                                                                                                                                                           this->entry_speed,
  72                                                                                                                                                                 this->max_entry_speed,
  73                                                                                                                                                                              this->times_taken,
  74                                                                                                                                                                                       this->is_ready,
  75                                                                                                                                                                                                 recalculate_flag?1:0
  76                              );
  77 }
  78
  79
  80 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
  81 // The factors represent a factor of braking and must be in the range 0.0-1.0.
  82 //                                +--------+ <- nominal_rate
  83 //                               /          \
  84 // nominal_rate*entry_factor -> +            \
  85 //                              |             + <- nominal_rate*exit_factor
  86 //                              +-------------+
  87 //                                  time -->
  88 */
  89 void Block::calculate_trapezoid( float entryfactor, float exitfactor )
  90 {
  91
  92     // The planner passes us factors, we need to transform them in rates
  93     this->initial_rate = ceil(this->nominal_rate * entryfactor);   // (step/min)
  94     this->final_rate   = ceil(this->nominal_rate * exitfactor);    // (step/min)
  95
  96     // How many steps to accelerate and decelerate
  97     float acceleration_per_minute = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second * 60.0; // ( step/min^2)
  98     int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
  99     int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_minute ) );
 100
 101     // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
 102     int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
 103
 104     // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
 105     // have to use intersection_distance() to calculate when to abort acceleration and start braking
 106     // in order to reach the final_rate exactly at the end of this block.
 107     if (plateau_steps < 0) {
 108         accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
 109         accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
 110         accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
 111         plateau_steps = 0;
 112     }
 113     this->accelerate_until = accelerate_steps;
 114     this->decelerate_after = accelerate_steps + plateau_steps;
 115
 116 }
 117
 118 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
 119 // given acceleration:
 120 float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration)
 121 {
 122     return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration));
 123 }
 124
 125 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
 126 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 127 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 128 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
 129 //
 130 /*                          + <- some maximum rate we don't care about
 131                            /|\
 132                           / | \
 133                          /  |  + <- final_rate
 134                         /   |  |
 135        initial_rate -> +----+--+
 136                             ^ ^
 137                             | |
 138         intersection_distance distance */
 139 float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance)
 140 {
 141     return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration));
 142 }
 143
 144 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
 145 // acceleration within the allotted distance.
 146 inline float max_allowable_speed(float acceleration, float target_velocity, float distance)
 147 {
 148     return(
 149               sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
 150           );
 151 }
 152
 153
 154 // Called by Planner::recalculate() when scanning the plan from last to first entry.
 155 void Block::reverse_pass(Block *next)
 156 {
 157
 158     if (next) {
 159         // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
 160         // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
 161         // check for maximum allowable speed reductions to ensure maximum possible planned speed.
 162         if (this->entry_speed != this->max_entry_speed) {
 163
 164             // If nominal length true, max junction speed is guaranteed to be reached. Only compute
 165             // for max allowable speed if block is decelerating and nominal length is false.
 166             if ((!this->nominal_length_flag) && (this->max_entry_speed > next->entry_speed)) {
 167                 this->entry_speed = min( this->max_entry_speed, max_allowable_speed(-THEKERNEL->planner->acceleration, next->entry_speed, this->millimeters));
 168             } else {
 169                 this->entry_speed = this->max_entry_speed;
 170             }
 171
 172         }
 173     } // Skip last block. Already initialized and set for recalculation.
 174
 175 }
 176
 177
 178 // Called by Planner::recalculate() when scanning the plan from first to last entry.
 179 void Block::forward_pass(Block *previous)
 180 {
 181
 182     if(!previous) {
 183         return;    // Begin planning after buffer_tail
 184     }
 185
 186     // If the previous block is an acceleration block, but it is not long enough to complete the
 187     // full speed change within the block, we need to adjust the entry speed accordingly. Entry
 188     // speeds have already been reset, maximized, and reverse planned by reverse planner.
 189     // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
 190     if (!previous->nominal_length_flag) {
 191         if (previous->entry_speed < this->entry_speed) {
 192             float entry_speed = min( this->entry_speed,
 193                                      max_allowable_speed(-THEKERNEL->planner->acceleration, previous->entry_speed, previous->millimeters) );
 194
 195             // Check for junction speed change
 196             if (this->entry_speed != entry_speed) {
 197                 this->entry_speed = entry_speed;
 198             }
 199         }
 200     }
 201
 202 }
 203
 204 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
 205 void Block::append_gcode(Gcode* gcode)
 206 {
 207     Gcode new_gcode = *gcode;
 208     gcodes.push_back(new_gcode);
 209 }
 210
 211 void Block::begin()
 212 {
 213     recalculate_flag = false;
 214
 215     // execute all the gcodes related to this block
 216     for(unsigned int index = 0; index < gcodes.size(); index++)
 217         THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index]));
 218
 219     THEKERNEL->call_event(ON_BLOCK_BEGIN, this);
 220 }
 221
 222 // Signal the conveyor that this block is ready to be injected into the system
 223 void Block::ready()
 224 {
 225     this->is_ready = true;
 226 }
 227
 228 // Mark the block as taken by one more module
 229 void Block::take()
 230 {
 231     this->times_taken++;
 232 }
 233
 234 // Mark the block as no longer taken by one module, go to next block if this free's it
 235 void Block::release()
 236 {
 237     if (--this->times_taken <= 0)
 238         THEKERNEL->call_event(ON_BLOCK_END, this);
 239
 240     // ensure conveyor gets called last
 241     THEKERNEL->conveyor->on_block_end(this);
 242 }