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->streams->printf("%p: steps:%4d|%4d|%4d(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 \r\n", this, this->steps[0], this->steps[1], this->steps[2], this->steps_event_count, this->nominal_rate, this->nominal_speed, this->millimeters, this->rate_delta, this->accelerate_until, this->decelerate_after, this->initial_rate, this->final_rate, this->entry_speed, this->max_entry_speed, this->times_taken, this->is_ready );
  58 }
  59
  60
  61 /* Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
  62 // The factors represent a factor of braking and must be in the range 0.0-1.0.
  63 //                                +--------+ <- nominal_rate
  64 //                               /          \
  65 // nominal_rate*entry_factor -> +            \
  66 //                              |             + <- nominal_rate*exit_factor
  67 //                              +-------------+
  68 //                                  time -->
  69 */
  70 void Block::calculate_trapezoid( float entryfactor, float exitfactor )
  71 {
  72
  73     // The planner passes us factors, we need to transform them in rates
  74     this->initial_rate = ceil(this->nominal_rate * entryfactor);   // (step/min)
  75     this->final_rate   = ceil(this->nominal_rate * exitfactor);    // (step/min)
  76
  77     // How many steps to accelerate and decelerate
  78     float acceleration_per_minute = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second * 60.0; // ( step/min^2)
  79     int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
  80     int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_minute ) );
  81
  82     // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
  83     int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
  84
  85     // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
  86     // have to use intersection_distance() to calculate when to abort acceleration and start braking
  87     // in order to reach the final_rate exactly at the end of this block.
  88     if (plateau_steps < 0) {
  89         accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
  90         accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
  91         accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
  92         plateau_steps = 0;
  93     }
  94     this->accelerate_until = accelerate_steps;
  95     this->decelerate_after = accelerate_steps + plateau_steps;
  96
  97 }
  98
  99 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
 100 // given acceleration:
 101 float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration)
 102 {
 103     return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration));
 104 }
 105
 106 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
 107 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 108 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 109 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
 110 //
 111 /*                          + <- some maximum rate we don't care about
 112                            /|\
 113                           / | \
 114                          /  |  + <- final_rate
 115                         /   |  |
 116        initial_rate -> +----+--+
 117                             ^ ^
 118                             | |
 119         intersection_distance distance */
 120 float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance)
 121 {
 122     return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration));
 123 }
 124
 125 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
 126 // acceleration within the allotted distance.
 127 inline float max_allowable_speed(float acceleration, float target_velocity, float distance)
 128 {
 129     return(
 130               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
 131           );
 132 }
 133
 134
 135 // Called by Planner::recalculate() when scanning the plan from last to first entry.
 136 void Block::reverse_pass(Block *next)
 137 {
 138
 139     if (next) {
 140         // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
 141         // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
 142         // check for maximum allowable speed reductions to ensure maximum possible planned speed.
 143         if (this->entry_speed != this->max_entry_speed) {
 144
 145             // If nominal length true, max junction speed is guaranteed to be reached. Only compute
 146             // for max allowable speed if block is decelerating and nominal length is false.
 147             if ((!this->nominal_length_flag) && (this->max_entry_speed > next->entry_speed)) {
 148                 this->entry_speed = min( this->max_entry_speed, max_allowable_speed(-THEKERNEL->planner->acceleration, next->entry_speed, this->millimeters));
 149             } else {
 150                 this->entry_speed = this->max_entry_speed;
 151             }
 152             this->recalculate_flag = true;
 153
 154         }
 155     } // Skip last block. Already initialized and set for recalculation.
 156
 157 }
 158
 159
 160 // Called by Planner::recalculate() when scanning the plan from first to last entry.
 161 void Block::forward_pass(Block *previous)
 162 {
 163
 164     if(!previous) {
 165         return;    // Begin planning after buffer_tail
 166     }
 167
 168     // If the previous block is an acceleration block, but it is not long enough to complete the
 169     // full speed change within the block, we need to adjust the entry speed accordingly. Entry
 170     // speeds have already been reset, maximized, and reverse planned by reverse planner.
 171     // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
 172     if (!previous->nominal_length_flag) {
 173         if (previous->entry_speed < this->entry_speed) {
 174             float entry_speed = min( this->entry_speed,
 175                                      max_allowable_speed(-THEKERNEL->planner->acceleration, previous->entry_speed, previous->millimeters) );
 176
 177             // Check for junction speed change
 178             if (this->entry_speed != entry_speed) {
 179                 this->entry_speed = entry_speed;
 180                 this->recalculate_flag = true;
 181             }
 182         }
 183     }
 184
 185 }
 186
 187 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
 188 void Block::append_gcode(Gcode *gcode)
 189 {
 190     __disable_irq();
 191     Gcode new_gcode = *gcode;
 192     this->gcodes.push_back(new_gcode);
 193     __enable_irq();
 194 }
 195
 196 // The attached gcodes are then poped and the on_gcode_execute event is called with them as a parameter
 197 void Block::pop_and_execute_gcode()
 198 {
 199     Block *block = const_cast<Block *>(this);
 200     for(unsigned short index = 0; index < block->gcodes.size(); index++) {
 201         THEKERNEL->call_event(ON_GCODE_EXECUTE, &(block->gcodes[index]));
 202     }
 203 }
 204
 205 // Signal the conveyor that this block is ready to be injected into the system
 206 void Block::ready()
 207 {
 208     this->is_ready = true;
 209     THEKERNEL->conveyor->new_block_added();
 210 }
 211
 212 // Mark the block as taken by one more module
 213 void Block::take()
 214 {
 215     this->times_taken++;
 216 }
 217
 218 // Mark the block as no longer taken by one module, go to next block if this free's it
 219 // This is one of the craziest bits in smoothie
 220 void Block::release()
 221 {
 222
 223     // A block can be taken by several modules, we want to actually release it only when all modules have release()d it
 224     this->times_taken--;
 225     if( this->times_taken < 1 ) {
 226
 227         // All modules are done with this block
 228         // Call the on_block_end event so all modules can act accordingly
 229         THEKERNEL->call_event(ON_BLOCK_END, this);
 230
 231         // Gcodes corresponding to the *following* blocks are stored in this block.
 232         // We execute them all in order when this block is finished executing
 233         this->pop_and_execute_gcode();
 234
 235         // We would normally delete this block directly here, but we can't, because this is interrupt context, no crazy memory stuff here
 236         // So instead we increment a counter, and it will be deleted in main loop context
 237         Conveyor *conveyor = THEKERNEL->conveyor;
 238         if( conveyor->queue.size() > conveyor->flush_blocks ) {
 239             conveyor->flush_blocks++;
 240         }
 241
 242         // We don't look for the next block to execute if the conveyor is already doing that itself
 243         if( conveyor->looking_for_new_block == false ) {
 244
 245             // If there are still blocks to execute
 246             if( conveyor->queue.size() > conveyor->flush_blocks ) {
 247                 Block *candidate =  conveyor->queue.get_ref(conveyor->flush_blocks);
 248
 249                 // We only execute blocks that are ready ( their math is done )
 250                 if( candidate->is_ready ) {
 251
 252                     // Execute this candidate
 253                     conveyor->current_block = candidate;
 254                     THEKERNEL->call_event(ON_BLOCK_BEGIN, conveyor->current_block);
 255
 256                     // If no module took this block, release it ourselves, as nothing else will do it otherwise
 257                     if( conveyor->current_block->times_taken < 1 ) {
 258                         conveyor->current_block->times_taken = 1;
 259                         conveyor->current_block->release();
 260                     }
 261                 } else {
 262                     conveyor->current_block = NULL;
 263                 }
 264             } else {
 265                 conveyor->current_block = NULL;
 266             }
 267         }
 268     }
 269 }
 270
 271
 272