src/modules/robot/Planner.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) with additions from Sungeun K. Jeon (https://github.com/chamnit/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 using namespace std;
   9 #include <vector>
  10
  11 #include "mri.h"
  12 #include "nuts_bolts.h"
  13 #include "RingBuffer.h"
  14 #include "Gcode.h"
  15 #include "Module.h"
  16 #include "Kernel.h"
  17 #include "Block.h"
  18 #include "Planner.h"
  19 #include "Conveyor.h"
  20
  21 #define acceleration_checksum          CHECKSUM("acceleration")
  22 #define max_jerk_checksum              CHECKSUM("max_jerk")
  23 #define junction_deviation_checksum    CHECKSUM("junction_deviation")
  24 #define minimum_planner_speed_checksum CHECKSUM("minimum_planner_speed")
  25
  26 // The Planner does the acceleration math for the queue of Blocks ( movements ).
  27 // It makes sure the speed stays within the configured constraints ( acceleration, junction_deviation, etc )
  28 // It goes over the list in both direction, every time a block is added, re-doing the math to make sure everything is optimal
  29
  30 Planner::Planner(){
  31     clear_vector(this->position);
  32     clear_vector_float(this->previous_unit_vec);
  33     this->previous_nominal_speed = 0.0;
  34     this->has_deleted_block = false;
  35 }
  36
  37 void Planner::on_module_loaded(){
  38     register_for_event(ON_CONFIG_RELOAD);
  39     this->on_config_reload(this);
  40 }
  41
  42 // Configure acceleration
  43 void Planner::on_config_reload(void* argument){
  44     this->acceleration =       THEKERNEL->config->value(acceleration_checksum       )->by_default(100  )->as_number() * 60 * 60; // Acceleration is in mm/minute^2, see https://github.com/grbl/grbl/commit/9141ad282540eaa50a41283685f901f29c24ddbd#planner.c
  45     this->junction_deviation = THEKERNEL->config->value(junction_deviation_checksum )->by_default(0.05f)->as_number();
  46     this->minimum_planner_speed = THEKERNEL->config->value(minimum_planner_speed_checksum )->by_default(0.0f)->as_number();
  47 }
  48
  49
  50 // Append a block to the queue, compute it's speed factors
  51 void Planner::append_block( int target[], float feed_rate, float distance, float deltas[] ){
  52
  53     // Stall here if the queue is ful
  54     THEKERNEL->conveyor->wait_for_queue(2);
  55
  56     // Create ( recycle ) a new block
  57     Block* block = THEKERNEL->conveyor->new_block();
  58
  59     // Direction bits
  60     block->direction_bits = 0;
  61     for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){
  62         if( target[stepper] < position[stepper] ){ block->direction_bits |= (1<<stepper); }
  63     }
  64
  65     // Number of steps for each stepper
  66     for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){ block->steps[stepper] = labs(target[stepper] - this->position[stepper]); }
  67
  68     // Max number of steps, for all axes
  69     block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) );
  70
  71     block->millimeters = distance;
  72     float inverse_millimeters = 0.0F;
  73     if( distance > 0 ){ inverse_millimeters = 1.0F/distance; }
  74
  75     // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
  76     // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
  77     float inverse_minute = feed_rate * inverse_millimeters;
  78     if( distance > 0 ){
  79         block->nominal_speed = block->millimeters * inverse_minute;           // (mm/min) Always > 0
  80         block->nominal_rate = ceil(block->steps_event_count * inverse_minute); // (step/min) Always > 0
  81     }else{
  82         block->nominal_speed = 0;
  83         block->nominal_rate = 0;
  84     }
  85
  86     // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
  87     // average travel per step event changes. For a line along one axis the travel per step event
  88     // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
  89     // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
  90     // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
  91     // specifically for each line to compensate for this phenomenon:
  92     // Convert universal acceleration for direction-dependent stepper rate change parameter
  93     block->rate_delta = (float)( ( block->steps_event_count*inverse_millimeters * this->acceleration ) / ( THEKERNEL->stepper->acceleration_ticks_per_second * 60 ) ); // (step/min/acceleration_tick)
  94
  95     // Compute path unit vector
  96     float unit_vec[3];
  97     unit_vec[X_AXIS] = deltas[X_AXIS]*inverse_millimeters;
  98     unit_vec[Y_AXIS] = deltas[Y_AXIS]*inverse_millimeters;
  99     unit_vec[Z_AXIS] = deltas[Z_AXIS]*inverse_millimeters;
 100
 101     // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
 102     // Let a circle be tangent to both previous and current path line segments, where the junction
 103     // deviation is defined as the distance from the junction to the closest edge of the circle,
 104     // colinear with the circle center. The circular segment joining the two paths represents the
 105     // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
 106     // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
 107     // path width or max_jerk in the previous grbl version. This approach does not actually deviate
 108     // from path, but used as a robust way to compute cornering speeds, as it takes into account the
 109     // nonlinearities of both the junction angle and junction velocity.
 110     float vmax_junction = minimum_planner_speed; // Set default max junction speed
 111
 112     if (THEKERNEL->conveyor->queue.size() > 1 && (this->previous_nominal_speed > 0.0F)) {
 113       // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
 114       // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
 115       float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
 116                          - this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
 117                          - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
 118
 119       // Skip and use default max junction speed for 0 degree acute junction.
 120       if (cos_theta < 0.95F) {
 121         vmax_junction = min(this->previous_nominal_speed,block->nominal_speed);
 122         // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
 123         if (cos_theta > -0.95F) {
 124           // Compute maximum junction velocity based on maximum acceleration and junction deviation
 125           float sin_theta_d2 = sqrtf(0.5F*(1.0F-cos_theta)); // Trig half angle identity. Always positive.
 126           vmax_junction = min(vmax_junction,
 127             sqrtf(this->acceleration * this->junction_deviation * sin_theta_d2/(1.0F-sin_theta_d2)) );
 128         }
 129       }
 130     }
 131     block->max_entry_speed = vmax_junction;
 132
 133     // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed.
 134     float v_allowable = this->max_allowable_speed(-this->acceleration,minimum_planner_speed,block->millimeters); //TODO: Get from config
 135     block->entry_speed = min(vmax_junction, v_allowable);
 136
 137     // Initialize planner efficiency flags
 138     // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
 139     // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
 140     // the current block and next block junction speeds are guaranteed to always be at their maximum
 141     // junction speeds in deceleration and acceleration, respectively. This is due to how the current
 142     // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
 143     // the reverse and forward planners, the corresponding block junction speed will always be at the
 144     // the maximum junction speed and may always be ignored for any speed reduction checks.
 145     if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
 146     else { block->nominal_length_flag = false; }
 147     block->recalculate_flag = true; // Always calculate trapezoid for new block
 148
 149     // Update previous path unit_vector and nominal speed
 150     memcpy(this->previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
 151     this->previous_nominal_speed = block->nominal_speed;
 152
 153     // Update current position
 154     memcpy(this->position, target, sizeof(int)*3);
 155
 156     // Math-heavy re-computing of the whole queue to take the new
 157     this->recalculate();
 158
 159     // The block can now be used
 160     block->ready();
 161 }
 162
 163
 164 // Recalculates the motion plan according to the following algorithm:
 165 //
 166 // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
 167 // so that:
 168 //   a. The junction jerk is within the set limit
 169 //   b. No speed reduction within one block requires faster deceleration than the one, true constant
 170 //      acceleration.
 171 // 2. Go over every block in chronological order and dial down junction speed reduction values if
 172 //   a. The speed increase within one block would require faster accelleration than the one, true
 173 //      constant acceleration.
 174 //
 175 // When these stages are complete all blocks have an entry_factor that will allow all speed changes to
 176 // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
 177 // the set limit. Finally it will:
 178 //
 179 // 3. Recalculate trapezoids for all blocks.
 180 //
 181 void Planner::recalculate() {
 182     Conveyor::BlockQueue_t *queue = &THEKERNEL->conveyor->queue;
 183
 184     int newest = queue->prev_block_index(queue->head);
 185     int oldest = queue->tail;
 186
 187     int block_index = newest;
 188
 189     Block* previous;
 190     Block* current;
 191     Block* next;
 192
 193     current = &queue->buffer[block_index];
 194     current->recalculate_flag = true;
 195
 196     // if there's only one block in the queue, we fall through both while loops and this ends up in current
 197     // so we must set it here, or perform conditionals further down. this is easier
 198     next = current;
 199
 200     while ((block_index != oldest) && (current->recalculate_flag))
 201     {
 202         block_index = queue->prev_block_index(block_index);
 203
 204         next = current;
 205         current = &queue->buffer[block_index];
 206
 207         current->recalculate_flag = false;
 208
 209         current->reverse_pass(next);
 210     }
 211
 212     previous = current;
 213     current = next;
 214
 215     // Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
 216     // entry_speed for each junction and the entry_speed of the next junction. Must be called by
 217     // planner_recalculate() after updating the blocks. Any recalulate flagged junction will
 218     // compute the two adjacent trapezoids to the junction, since the junction speed corresponds
 219     // to exit speed and entry speed of one another.
 220     while (block_index != newest)
 221     {
 222         current->forward_pass(previous);
 223
 224         // Recalculate if current block entry or exit junction speed has changed.
 225         if (previous->recalculate_flag || current->recalculate_flag )
 226         {
 227             previous->calculate_trapezoid( previous->entry_speed/previous->nominal_speed, current->entry_speed/previous->nominal_speed );
 228             previous->recalculate_flag = false;
 229         }
 230
 231         block_index = queue->next_block_index(block_index);
 232         previous = current;
 233         current = &queue->buffer[block_index];
 234     }
 235
 236     // Last/newest block in buffer. Exit speed is set with minimum_planner_speed. Always recalculated.
 237     current->calculate_trapezoid( current->entry_speed/current->nominal_speed, minimum_planner_speed/current->nominal_speed );
 238     current->recalculate_flag = false;
 239 }
 240
 241 // Debug function
 242 void Planner::dump_queue(){
 243     for( int index = 0; index <= THEKERNEL->conveyor->queue.size()-1; index++ ){
 244        //if( index > 10 && index < THEKERNEL->conveyor->queue.size()-10 ){ continue; }
 245        THEKERNEL->streams->printf("block %03d > ", index);
 246        THEKERNEL->conveyor->queue.get_ref(index)->debug();
 247     }
 248 }
 249
 250 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
 251 // acceleration within the allotted distance.
 252 float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance) {
 253   return(
 254     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
 255   );
 256 }
 257
 258