planner.c

00001 /*
00002   planner.c - buffers movement commands and manages the acceleration profile plan
00003   Part of Grbl
00004 
00005   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
00006   Copyright (c) 2009-2011 Simen Svale Skogsrud
00007   Copyright (c) 2011 Jens Geisler
00008 
00009   Grbl is free software: you can redistribute it and/or modify
00010   it under the terms of the GNU General Public License as published by
00011   the Free Software Foundation, either version 3 of the License, or
00012   (at your option) any later version.
00013 
00014   Grbl is distributed in the hope that it will be useful,
00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017   GNU General Public License for more details.
00018 
00019   You should have received a copy of the GNU General Public License
00020   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
00021 */
00022 
00023 #include "grbl.h"
00024 
00025 
00026 static plan_block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructions
00027 static uint8_t block_buffer_tail;     // Index of the block to process now
00028 static uint8_t block_buffer_head;     // Index of the next block to be pushed
00029 static uint8_t next_buffer_head;      // Index of the next buffer head
00030 static uint8_t block_buffer_planned;  // Index of the optimally planned block
00031 
00032 // Define planner variables
00033 typedef struct {
00034   int32_t position[N_AXIS];          // The planner position of the tool in absolute steps. Kept separate
00035                                      // from g-code position for movements requiring multiple line motions,
00036                                      // i.e. arcs, canned cycles, and backlash compensation.
00037   float previous_unit_vec[N_AXIS];   // Unit vector of previous path line segment
00038   float previous_nominal_speed;  // Nominal speed of previous path line segment
00039 } planner_t;
00040 static planner_t pl;
00041 
00042 
00043 // Returns the index of the next block in the ring buffer. Also called by stepper segment buffer.
00044 uint8_t plan_next_block_index(uint8_t block_index)
00045 {
00046   block_index++;
00047   if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
00048   return(block_index);
00049 }
00050 
00051 
00052 // Returns the index of the previous block in the ring buffer
00053 static uint8_t plan_prev_block_index(uint8_t block_index)
00054 {
00055   if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
00056   block_index--;
00057   return(block_index);
00058 }
00059 
00060 
00061 /*                            PLANNER SPEED DEFINITION
00062                                      +--------+   <- current->nominal_speed
00063                                     /          \
00064          current->entry_speed ->   +            \
00065                                    |             + <- next->entry_speed (aka exit speed)
00066                                    +-------------+
00067                                        time -->
00068 
00069   Recalculates the motion plan according to the following basic guidelines:
00070 
00071     1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
00072         (i.e. current->entry_speed) such that:
00073       a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
00074          neighboring blocks.
00075       b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
00076          with a maximum allowable deceleration over the block travel distance.
00077       c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
00078     2. Go over every block in chronological (forward) order and dial down junction speed values if
00079       a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
00080          acceleration over the block travel distance.
00081 
00082   When these stages are complete, the planner will have maximized the velocity profiles throughout the all
00083   of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
00084   other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
00085   are possible. If a new block is added to the buffer, the plan is recomputed according to the said
00086   guidelines for a new optimal plan.
00087 
00088   To increase computational efficiency of these guidelines, a set of planner block pointers have been
00089   created to indicate stop-compute points for when the planner guidelines cannot logically make any further
00090   changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
00091   planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
00092   bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
00093   added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
00094   them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
00095   point) are all accelerating, they are all optimal and can not be altered by a new block added to the
00096   planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
00097   junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
00098   used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
00099   recomputed as stated in the general guidelines.
00100 
00101   Planner buffer index mapping:
00102   - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
00103   - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
00104       the buffer is full or empty. As described for standard ring buffers, this block is always empty.
00105   - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the
00106       buffer tail, this indicates the buffer is full.
00107   - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
00108       streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
00109       planner buffer that don't change with the addition of a new block, as describe above. In addition,
00110       this block can never be less than block_buffer_tail and will always be pushed forward and maintain
00111       this requirement when encountered by the plan_discard_current_block() routine during a cycle.
00112 
00113   NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
00114   line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
00115   enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
00116   decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
00117   becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
00118   will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
00119   motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
00120   the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
00121   for the planner to compute over. It also increases the number of computations the planner has to perform
00122   to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future
00123   ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more.
00124 
00125 */
00126 static void planner_recalculate()
00127 {
00128   // Initialize block index to the last block in the planner buffer.
00129   uint8_t block_index = plan_prev_block_index(block_buffer_head);
00130 
00131   // Bail. Can't do anything with one only one plan-able block.
00132   if (block_index == block_buffer_planned) { return; }
00133 
00134   // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
00135   // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
00136   // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
00137   float entry_speed_sqr;
00138   plan_block_t *next;
00139   plan_block_t *current = &block_buffer[block_index];
00140 
00141   // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
00142   current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
00143 
00144   block_index = plan_prev_block_index(block_index);
00145   if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete.
00146     // Check if the first block is the tail. If so, notify stepper to update its current parameters.
00147     if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
00148   } else { // Three or more plan-able blocks
00149     while (block_index != block_buffer_planned) {
00150       next = current;
00151       current = &block_buffer[block_index];
00152       block_index = plan_prev_block_index(block_index);
00153 
00154       // Check if next block is the tail block(=planned block). If so, update current stepper parameters.
00155       if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
00156 
00157       // Compute maximum entry speed decelerating over the current block from its exit speed.
00158       if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
00159         entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
00160         if (entry_speed_sqr < current->max_entry_speed_sqr) {
00161           current->entry_speed_sqr = entry_speed_sqr;
00162         } else {
00163           current->entry_speed_sqr = current->max_entry_speed_sqr;
00164         }
00165       }
00166     }
00167   }
00168 
00169   // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
00170   // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
00171   next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
00172   block_index = plan_next_block_index(block_buffer_planned);
00173   while (block_index != block_buffer_head) {
00174     current = next;
00175     next = &block_buffer[block_index];
00176 
00177     // Any acceleration detected in the forward pass automatically moves the optimal planned
00178     // pointer forward, since everything before this is all optimal. In other words, nothing
00179     // can improve the plan from the buffer tail to the planned pointer by logic.
00180     if (current->entry_speed_sqr < next->entry_speed_sqr) {
00181       entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
00182       // If true, current block is full-acceleration and we can move the planned pointer forward.
00183       if (entry_speed_sqr < next->entry_speed_sqr) {
00184         next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
00185         block_buffer_planned = block_index; // Set optimal plan pointer.
00186       }
00187     }
00188 
00189     // Any block set at its maximum entry speed also creates an optimal plan up to this
00190     // point in the buffer. When the plan is bracketed by either the beginning of the
00191     // buffer and a maximum entry speed or two maximum entry speeds, every block in between
00192     // cannot logically be further improved. Hence, we don't have to recompute them anymore.
00193     if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; }
00194     block_index = plan_next_block_index( block_index );
00195   }
00196 }
00197 
00198 
00199 void plan_reset()
00200 {
00201   memset(&pl, 0, sizeof(planner_t)); // Clear planner struct
00202   plan_reset_buffer();
00203 }
00204 
00205 
00206 void plan_reset_buffer()
00207 {
00208   block_buffer_tail = 0;
00209   block_buffer_head = 0; // Empty = tail
00210   next_buffer_head = 1; // plan_next_block_index(block_buffer_head)
00211   block_buffer_planned = 0; // = block_buffer_tail;
00212 }
00213 
00214 
00215 void plan_discard_current_block()
00216 {
00217   if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
00218     uint8_t block_index = plan_next_block_index( block_buffer_tail );
00219     // Push block_buffer_planned pointer, if encountered.
00220     if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; }
00221     block_buffer_tail = block_index;
00222   }
00223 }
00224 
00225 
00226 // Returns address of planner buffer block used by system motions. Called by segment generator.
00227 plan_block_t *plan_get_system_motion_block()
00228 {
00229   return(&block_buffer[block_buffer_head]);
00230 }
00231 
00232 
00233 // Returns address of first planner block, if available. Called by various main program functions.
00234 plan_block_t *plan_get_current_block()
00235 {
00236   if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty
00237   return(&block_buffer[block_buffer_tail]);
00238 }
00239 
00240 
00241 float plan_get_exec_block_exit_speed_sqr()
00242 {
00243   uint8_t block_index = plan_next_block_index(block_buffer_tail);
00244   if (block_index == block_buffer_head) { return( 0.0 ); }
00245   return( block_buffer[block_index].entry_speed_sqr );
00246 }
00247 
00248 
00249 // Returns the availability status of the block ring buffer. True, if full.
00250 uint8_t plan_check_full_buffer()
00251 {
00252   if (block_buffer_tail == next_buffer_head) { return(true); }
00253   return(false);
00254 }
00255 
00256 
00257 // Computes and returns block nominal speed based on running condition and override values.
00258 // NOTE: All system motion commands, such as homing/parking, are not subject to overrides.
00259 float plan_compute_profile_nominal_speed(plan_block_t *block)
00260 {
00261   float nominal_speed = block->programmed_rate;
00262   if (block->condition & PL_COND_FLAG_RAPID_MOTION) { nominal_speed *= (0.01f*sys.r_override); }
00263   else {
00264     if (!(block->condition & PL_COND_FLAG_NO_FEED_OVERRIDE)) { nominal_speed *= (0.01f*sys.f_override); }
00265     if (nominal_speed > block->rapid_rate) { nominal_speed = block->rapid_rate; }
00266   }
00267   if (nominal_speed > MINIMUM_FEED_RATE) { return(nominal_speed); }
00268   return(MINIMUM_FEED_RATE);
00269 }
00270 
00271 
00272 // Computes and updates the max entry speed (sqr) of the block, based on the minimum of the junction's
00273 // previous and current nominal speeds and max junction speed.
00274 static void plan_compute_profile_parameters(plan_block_t *block, float nominal_speed, float prev_nominal_speed)
00275 {
00276   // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
00277   if (nominal_speed > prev_nominal_speed) { block->max_entry_speed_sqr = prev_nominal_speed*prev_nominal_speed; }
00278   else { block->max_entry_speed_sqr = nominal_speed*nominal_speed; }
00279   if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) { block->max_entry_speed_sqr = block->max_junction_speed_sqr; }
00280 }
00281 
00282 
00283 // Re-calculates buffered motions profile parameters upon a motion-based override change.
00284 void plan_update_velocity_profile_parameters()
00285 {
00286   uint8_t block_index = block_buffer_tail;
00287   plan_block_t *block;
00288   float nominal_speed;
00289   float prev_nominal_speed = SOME_LARGE_VALUE; // Set high for first block nominal speed calculation.
00290   while (block_index != block_buffer_head) {
00291     block = &block_buffer[block_index];
00292     nominal_speed = plan_compute_profile_nominal_speed(block);
00293     plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed);
00294     prev_nominal_speed = nominal_speed;
00295     block_index = plan_next_block_index(block_index);
00296   }
00297   pl.previous_nominal_speed = prev_nominal_speed; // Update prev nominal speed for next incoming block.
00298 }
00299 
00300 
00301 /* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
00302    in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
00303    rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
00304    All position data passed to the planner must be in terms of machine position to keep the planner
00305    independent of any coordinate system changes and offsets, which are handled by the g-code parser.
00306    NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
00307    In other words, the buffer head is never equal to the buffer tail.  Also the feed rate input value
00308    is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
00309    invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
00310    invert_feed_rate always false).
00311    The system motion condition tells the planner to plan a motion in the always unused block buffer
00312    head. It avoids changing the planner state and preserves the buffer to ensure subsequent gcode
00313    motions are still planned correctly, while the stepper module only points to the block buffer head
00314    to execute the special system motion. */
00315 uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
00316 {
00317   // Prepare and initialize new block. Copy relevant pl_data for block execution.
00318   plan_block_t *block = &block_buffer[block_buffer_head];
00319   memset(block,0,sizeof(plan_block_t)); // Zero all block values.
00320   block->condition = pl_data->condition;
00321   #ifdef VARIABLE_SPINDLE
00322     block->spindle_speed = pl_data->spindle_speed;
00323   #endif
00324   #ifdef USE_LINE_NUMBERS
00325     block->line_number = pl_data->line_number;
00326   #endif
00327 
00328   // Compute and store initial move distance data.
00329   int32_t target_steps[N_AXIS], position_steps[N_AXIS];
00330   float unit_vec[N_AXIS], delta_mm;
00331   uint8_t idx;
00332 
00333   // Copy position data based on type of motion being planned.
00334   if (block->condition & PL_COND_FLAG_SYSTEM_MOTION) {
00335 #ifdef COREXY
00336     position_steps[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position);
00337     position_steps[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position);
00338     position_steps[Z_AXIS] = sys_position[Z_AXIS];
00339 #else
00340     memcpy(position_steps, sys_position, sizeof(sys_position));
00341 #endif
00342   }
00343   else { memcpy(position_steps, pl.position, sizeof(pl.position)); }
00344 
00345   #ifdef COREXY
00346     target_steps[A_MOTOR] = lround(target[A_MOTOR]*settings.steps_per_mm[A_MOTOR]);
00347     target_steps[B_MOTOR] = lround(target[B_MOTOR]*settings.steps_per_mm[B_MOTOR]);
00348     block->steps[A_MOTOR] = labs((target_steps[X_AXIS]-position_steps[X_AXIS]) + (target_steps[Y_AXIS]-position_steps[Y_AXIS]));
00349     block->steps[B_MOTOR] = labs((target_steps[X_AXIS]-position_steps[X_AXIS]) - (target_steps[Y_AXIS]-position_steps[Y_AXIS]));
00350   #endif
00351 
00352   for (idx=0; idx<N_AXIS; idx++) {
00353     // Calculate target position in absolute steps, number of steps for each axis, and determine max step events.
00354     // Also, compute individual axes distance for move and prep unit vector calculations.
00355     // NOTE: Computes true distance from converted step values.
00356     #ifdef COREXY
00357       if ( !(idx == A_MOTOR) && !(idx == B_MOTOR) ) {
00358         target_steps[idx] = lroundf(target[idx]*settings.steps_per_mm[idx]);
00359         block->steps[idx] = fabsf(target_steps[idx]-position_steps[idx]);
00360       }
00361       block->step_event_count = max(block->step_event_count, block->steps[idx]);
00362       if (idx == A_MOTOR) {
00363         delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] + target_steps[Y_AXIS]-position_steps[Y_AXIS])/settings.steps_per_mm[idx];
00364       } else if (idx == B_MOTOR) {
00365         delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] - target_steps[Y_AXIS]+position_steps[Y_AXIS])/settings.steps_per_mm[idx];
00366       } else {
00367         delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx];
00368       }
00369     #else
00370       target_steps[idx] = lroundf(target[idx]*settings.steps_per_mm[idx]);
00371       block->steps[idx] = fabsf(target_steps[idx]-position_steps[idx]);
00372       block->step_event_count = max(block->step_event_count, block->steps[idx]);
00373       delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx];
00374       #endif
00375     unit_vec[idx] = delta_mm; // Store unit vector numerator
00376 
00377     // Set direction bits. Bit enabled always means direction is negative.
00378     if (delta_mm < 0.0f ) { block->direction_bits |= direction_pin_mask[idx]; }
00379   }
00380 
00381   // Bail if this is a zero-length block. Highly unlikely to occur.
00382   if (block->step_event_count == 0) { return(PLAN_EMPTY_BLOCK); }
00383 
00384   // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled
00385   // down such that no individual axes maximum values are exceeded with respect to the line direction.
00386   // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
00387   // if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
00388   block->millimeters = convert_delta_vector_to_unit_vector(unit_vec);
00389   block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec);
00390   block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec);
00391 
00392   // Store programmed rate.
00393   if (block->condition & PL_COND_FLAG_RAPID_MOTION) { block->programmed_rate = block->rapid_rate; }
00394   else { 
00395     block->programmed_rate = pl_data->feed_rate;
00396     if (block->condition & PL_COND_FLAG_INVERSE_TIME) { block->programmed_rate *= block->millimeters; }
00397   }
00398 
00399   // TODO: Need to check this method handling zero junction speeds when starting from rest.
00400   if ((block_buffer_head == block_buffer_tail) || (block->condition & PL_COND_FLAG_SYSTEM_MOTION)) {
00401 
00402     // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.
00403     // If system motion, the system motion block always is assumed to start from rest and end at a complete stop.
00404     block->entry_speed_sqr = 0.0f;
00405     block->max_junction_speed_sqr = 0.0f; // Starting from rest. Enforce start from zero velocity.
00406 
00407   } else {
00408     // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
00409     // Let a circle be tangent to both previous and current path line segments, where the junction
00410     // deviation is defined as the distance from the junction to the closest edge of the circle,
00411     // colinear with the circle center. The circular segment joining the two paths represents the
00412     // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
00413     // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
00414     // path width or max_jerk in the previous Grbl version. This approach does not actually deviate
00415     // from path, but used as a robust way to compute cornering speeds, as it takes into account the
00416     // nonlinearities of both the junction angle and junction velocity.
00417     //
00418     // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
00419     // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
00420     // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
00421     // is exactly the same. Instead of motioning all the way to junction point, the machine will
00422     // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
00423     // a continuous mode path, but ARM-based microcontrollers most certainly do.
00424     //
00425     // NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
00426     // changed dynamically during operation nor can the line move geometry. This must be kept in
00427     // memory in the event of a feedrate override changing the nominal speeds of blocks, which can
00428     // change the overall maximum entry speed conditions of all blocks.
00429 
00430     float junction_unit_vec[N_AXIS];
00431     float junction_cos_theta = 0.0f;
00432     for (idx=0; idx<N_AXIS; idx++) {
00433       junction_cos_theta -= pl.previous_unit_vec[idx]*unit_vec[idx];
00434       junction_unit_vec[idx] = unit_vec[idx]-pl.previous_unit_vec[idx];
00435     }
00436 
00437     // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
00438     if (junction_cos_theta > 0.999999f) {
00439       //  For a 0 degree acute junction, just set minimum junction speed.
00440       block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED;
00441     } else {
00442       if (junction_cos_theta < -0.999999f) {
00443         // Junction is a straight line or 180 degrees. Junction speed is infinite.
00444         block->max_junction_speed_sqr = SOME_LARGE_VALUE;
00445       } else {
00446         convert_delta_vector_to_unit_vector(junction_unit_vec);
00447         float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec);
00448         float sin_theta_d2 = sqrtf(0.5f*(1.0f-junction_cos_theta)); // Trig half angle identity. Always positive.
00449         block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
00450                        (junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0f-sin_theta_d2) );
00451       }
00452     }
00453   }
00454 
00455   // Block system motion from updating this data to ensure next g-code motion is computed correctly.
00456   if (!(block->condition & PL_COND_FLAG_SYSTEM_MOTION)) {
00457     float nominal_speed = plan_compute_profile_nominal_speed(block);
00458     plan_compute_profile_parameters(block, nominal_speed, pl.previous_nominal_speed);
00459     pl.previous_nominal_speed = nominal_speed;
00460 
00461     // Update previous path unit_vector and planner position.
00462     memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
00463     memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
00464 
00465     // New block is all set. Update buffer head and next buffer head indices.
00466     block_buffer_head = next_buffer_head;
00467     next_buffer_head = plan_next_block_index(block_buffer_head);
00468 
00469     // Finish up by recalculating the plan with the new block.
00470     planner_recalculate();
00471   }
00472   return(PLAN_OK);
00473 }
00474 
00475 
00476 // Reset the planner position vectors. Called by the system abort/initialization routine.
00477 void plan_sync_position()
00478 {
00479   // TODO: For motor configurations not in the same coordinate frame as the machine position,
00480   // this function needs to be updated to accomodate the difference.
00481   uint8_t idx;
00482   for (idx=0; idx<N_AXIS; idx++) {
00483     #ifdef COREXY
00484       if (idx==X_AXIS) {
00485         pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position);
00486       } else if (idx==Y_AXIS) {
00487         pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position);
00488       } else {
00489         pl.position[idx] = sys_position[idx];
00490       }
00491     #else
00492       pl.position[idx] = sys_position[idx];
00493     #endif
00494   }
00495 }
00496 
00497 
00498 // Returns the number of available blocks are in the planner buffer.
00499 uint8_t plan_get_block_buffer_available()
00500 {
00501   if (block_buffer_head >= block_buffer_tail) { return((BLOCK_BUFFER_SIZE-1)-(block_buffer_head-block_buffer_tail)); }
00502   return((block_buffer_tail-block_buffer_head-1));
00503 }
00504 
00505 
00506 // Returns the number of active blocks are in the planner buffer.
00507 // NOTE: Deprecated. Not used unless classic status reports are enabled in config.h
00508 uint8_t plan_get_block_buffer_count()
00509 {
00510   if (block_buffer_head >= block_buffer_tail) { return(block_buffer_head-block_buffer_tail); }
00511   return(BLOCK_BUFFER_SIZE - (block_buffer_tail-block_buffer_head));
00512 }
00513 
00514 
00515 // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
00516 // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
00517 void plan_cycle_reinitialize()
00518 {
00519   // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
00520   st_update_plan_block_parameters();
00521   block_buffer_planned = block_buffer_tail;
00522   planner_recalculate();
00523 }