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