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 "mbed.h"
  13 #include <string>
  14 #include "Block.h"
  15 #include "Planner.h"
  16 #include "Player.h"
  17 using std::string;
  18 #include <vector>
  19 #include "../communication/utils/Gcode.h"
  20
  21 Block::Block(){
  22     clear_vector(this->steps);
  23     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.
  24 }
  25
  26 void Block::debug(Kernel* kernel){
  27     kernel->serial->printf(" steps:%4d|%4d|%4d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8d acc:%5d dec:%5d rates:%10d>%10d \r\n", 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 );
  28 }
  29
  30
  31 // Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the
  32 // speed under which you cannot exceed max_jerk no matter what you do.
  33 double Block::compute_factor_for_safe_speed(){
  34     return( this->planner->max_jerk / this->nominal_speed );
  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 void Block::calculate_trapezoid( double entryfactor, double exitfactor ){
  47
  48     this->initial_rate = ceil(this->nominal_rate * entryfactor);   // (step/min)
  49     this->final_rate   = ceil(this->nominal_rate * exitfactor);    // (step/min)
  50     double acceleration_per_minute = this->rate_delta * this->planner->kernel->stepper->acceleration_ticks_per_second * 60.0;
  51     int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
  52     int decelerate_steps = ceil( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_minute ) );
  53
  54     // Calculate the size of Plateau of Nominal Rate.
  55     int plateau_steps = this->steps_event_count-accelerate_steps-decelerate_steps;
  56
  57    // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
  58    // have to use intersection_distance() to calculate when to abort acceleration and start braking
  59    // in order to reach the final_rate exactly at the end of this block.
  60    if (plateau_steps < 0) {
  61        accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
  62        accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
  63        accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
  64        plateau_steps = 0;
  65    }
  66
  67    this->accelerate_until = accelerate_steps;
  68    this->decelerate_after = accelerate_steps+plateau_steps;
  69
  70 }
  71
  72 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
  73 // given acceleration:
  74 double Block::estimate_acceleration_distance(double initialrate, double targetrate, double acceleration) {
  75       return( (targetrate*targetrate-initialrate*initialrate)/(2L*acceleration));
  76 }
  77
  78 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
  79 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
  80 // a total travel of distance. This can be used to compute the intersection point between acceleration and
  81 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
  82 //
  83 /*                          + <- some maximum rate we don't care about
  84                            /|\
  85                           / | \
  86                          /  |  + <- final_rate
  87                         /   |  |
  88        initial_rate -> +----+--+
  89                             ^ ^
  90                             | |
  91         intersection_distance distance */
  92 double Block::intersection_distance(double initialrate, double finalrate, double acceleration, double distance) {
  93    return((2*acceleration*distance-initialrate*initialrate+finalrate*finalrate)/(4*acceleration));
  94 }
  95
  96 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
  97 // acceleration within the allotted distance.
  98 inline double max_allowable_speed(double acceleration, double target_velocity, double distance) {
  99   return(
 100     sqrt(target_velocity*target_velocity-2L*acceleration*60*60*distance)  //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
 101   );
 102 }
 103
 104
 105 // Called by Planner::recalculate() when scanning the plan from last to first entry.
 106 void Block::reverse_pass(Block* next, Block* previous){
 107
 108     if (next) {
 109         // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
 110         // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
 111         // check for maximum allowable speed reductions to ensure maximum possible planned speed.
 112         if (this->entry_speed != this->max_entry_speed) {
 113
 114             // If nominal length true, max junction speed is guaranteed to be reached. Only compute
 115             // for max allowable speed if block is decelerating and nominal length is false.
 116             if ((!this->nominal_length_flag) && (this->max_entry_speed > next->entry_speed)) {
 117                 this->entry_speed = min( this->max_entry_speed, max_allowable_speed(-this->planner->acceleration,next->entry_speed,this->millimeters));
 118             } else {
 119                 this->entry_speed = this->max_entry_speed;
 120             }
 121             this->recalculate_flag = true;
 122
 123         }
 124     } // Skip last block. Already initialized and set for recalculation.
 125
 126 }
 127
 128
 129 // Called by Planner::recalculate() when scanning the plan from first to last entry.
 130 void Block::forward_pass(Block* previous, Block* next){
 131
 132     if(!previous) { return; } // Begin planning after buffer_tail
 133
 134     // If the previous block is an acceleration block, but it is not long enough to complete the
 135     // full speed change within the block, we need to adjust the entry speed accordingly. Entry
 136     // speeds have already been reset, maximized, and reverse planned by reverse planner.
 137     // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
 138     if (!previous->nominal_length_flag) {
 139         if (previous->entry_speed < this->entry_speed) {
 140           double entry_speed = min( this->entry_speed,
 141             max_allowable_speed(-this->planner->acceleration,previous->entry_speed,previous->millimeters) );
 142
 143           // Check for junction speed change
 144           if (this->entry_speed != entry_speed) {
 145             this->entry_speed = entry_speed;
 146             this->recalculate_flag = true;
 147           }
 148         }
 149     }
 150
 151 }
 152
 153
 154 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
 155 void Block::append_gcode(Gcode* gcode){
 156    this->commands.push_back(gcode->command);
 157    this->travel_distances.push_back(gcode->millimeters_of_travel);
 158 }
 159
 160 // The attached gcodes are then poped and the on_gcode_execute event is called with them as a parameter
 161 void Block::pop_and_execute_gcode(Kernel* &kernel){
 162     for(unsigned short index=0; index<this->commands.size(); index++){
 163         string command = this->commands.at(index);
 164         double distance = this->travel_distances.at(index);
 165         Gcode gcode = Gcode();
 166         gcode.command = command;
 167         gcode.millimeters_of_travel = distance;
 168         kernel->call_event(ON_GCODE_EXECUTE, &gcode );
 169     }
 170 }
 171
 172 // Signal the player that this block is ready to be injected into the system
 173 void Block::ready(){
 174     this->player->new_block_added();
 175 }
 176
 177 // Mark the block as taken by one more module
 178 void Block::take(){
 179     this->times_taken++;
 180 }
 181
 182 // Mark the block as no longer taken by one module, go to next block if this free's it
 183 void Block::release(){
 184     this->times_taken--;
 185     if( this->times_taken < 1 ){
 186         this->player->kernel->call_event(ON_BLOCK_END, this);
 187         this->pop_and_execute_gcode(this->player->kernel);
 188         Player* player = this->player;
 189         if( player->queue.size() > 0 ){
 190             //player->kernel->serial->printf("before: %d\r\n", player->queue.size() );
 191             player->queue.delete_first();
 192         }
 193         //player->kernel->serial->printf("after: %d\r\n", player->queue.size() );
 194         player->current_block = NULL;
 195         player->pop_and_process_new_block();
 196     }
 197 }
 198
 199
 200