This version of the famous Grbl adds values in the work-coordinate-system (WCS) to the probing command. See x2grbl why you might want this.

planner.c 23KB

    /* planner.c - buffers movement commands and manages the acceleration profile plan Part of Grbl v0.9 Copyright (c) 2012-2014 Sungeun K. Jeon Grbl 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. Grbl 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. You should have received a copy of the GNU General Public License along with Grbl. If not, see <>. */ /* This file is based on work from Grbl v0.8, distributed under the terms of the MIT-license. See COPYING for more details. Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011-2012 Sungeun K. Jeon Copyright (c) 2011 Jens Geisler */ #include "system.h" #include "planner.h" #include "protocol.h" #include "stepper.h" #include "settings.h" #define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions static uint8_t block_buffer_tail; // Index of the block to process now static uint8_t block_buffer_head; // Index of the next block to be pushed static uint8_t next_buffer_head; // Index of the next buffer head static uint8_t block_buffer_planned; // Index of the optimally planned block // Define planner variables typedef struct { int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate // from g-code position for movements requiring multiple line motions, // i.e. arcs, canned cycles, and backlash compensation. float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment float previous_nominal_speed_sqr; // Nominal speed of previous path line segment } planner_t; static planner_t pl; // Returns the index of the next block in the ring buffer. Also called by stepper segment buffer. uint8_t plan_next_block_index(uint8_t block_index) { block_index++; if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } return(block_index); } // Returns the index of the previous block in the ring buffer static uint8_t plan_prev_block_index(uint8_t block_index) { if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } block_index--; return(block_index); } /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ | + <- next->entry_speed (aka exit speed) +-------------+ time --> Recalculates the motion plan according to the following basic guidelines: 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds (i.e. current->entry_speed) such that: a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of neighboring blocks. b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) with a maximum allowable deceleration over the block travel distance. c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). 2. Go over every block in chronological (forward) order and dial down junction speed values if a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable acceleration over the block travel distance. When these stages are complete, the planner will have maximized the velocity profiles throughout the all of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements are possible. If a new block is added to the buffer, the plan is recomputed according to the said guidelines for a new optimal plan. To increase computational efficiency of these guidelines, a set of planner block pointers have been created to indicate stop-compute points for when the planner guidelines cannot logically make any further changes or improvements to the plan when in normal operation and new blocks are streamed and added to the planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are bracketed by junction velocities at their maximums (or by the first planner block as well), no new block added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute point) are all accelerating, they are all optimal and can not be altered by a new block added to the planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum junction velocity is reached. However, if the operational conditions of the plan changes from infrequently used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is recomputed as stated in the general guidelines. Planner buffer index mapping: - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether the buffer is full or empty. As described for standard ring buffers, this block is always empty. - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the buffer tail, this indicates the buffer is full. - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the planner buffer that don't change with the addition of a new block, as describe above. In addition, this block can never be less than block_buffer_tail and will always be pushed forward and maintain this requirement when encountered by the plan_discard_current_block() routine during a cycle. NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use, the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance for the planner to compute over. It also increases the number of computations the planner has to perform to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. */ static void planner_recalculate() { // Initialize block index to the last block in the planner buffer. uint8_t block_index = plan_prev_block_index(block_buffer_head); // Bail. Can't do anything with one only one plan-able block. if (block_index == block_buffer_planned) { return; } // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last // block in buffer. Cease planning when the last optimal planned or tail pointer is reached. // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. float entry_speed_sqr; plan_block_t *next; plan_block_t *current = &block_buffer[block_index]; // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); block_index = plan_prev_block_index(block_index); if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. // Check if the first block is the tail. If so, notify stepper to update its current parameters. if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } } else { // Three or more plan-able blocks while (block_index != block_buffer_planned) { next = current; current = &block_buffer[block_index]; block_index = plan_prev_block_index(block_index); // Check if next block is the tail block(=planned block). If so, update current stepper parameters. if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } // Compute maximum entry speed decelerating over the current block from its exit speed. if (current->entry_speed_sqr != current->max_entry_speed_sqr) { entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; } else { current->entry_speed_sqr = current->max_entry_speed_sqr; } } } } // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer block_index = plan_next_block_index(block_buffer_planned); while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; // Any acceleration detected in the forward pass automatically moves the optimal planned // pointer forward, since everything before this is all optimal. In other words, nothing // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // If true, current block is full-acceleration and we can move the planned pointer forward. if (entry_speed_sqr < next->entry_speed_sqr) { next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. block_buffer_planned = block_index; // Set optimal plan pointer. } } // Any block set at its maximum entry speed also creates an optimal plan up to this // point in the buffer. When the plan is bracketed by either the beginning of the // buffer and a maximum entry speed or two maximum entry speeds, every block in between // cannot logically be further improved. Hence, we don't have to recompute them anymore. if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; } block_index = plan_next_block_index( block_index ); } } void plan_reset() { memset(&pl, 0, sizeof(pl)); // Clear planner struct block_buffer_tail = 0; block_buffer_head = 0; // Empty = tail next_buffer_head = 1; // plan_next_block_index(block_buffer_head) block_buffer_planned = 0; // = block_buffer_tail; } void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. uint8_t block_index = plan_next_block_index( block_buffer_tail ); // Push block_buffer_planned pointer, if encountered. if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; } block_buffer_tail = block_index; } } plan_block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty return(&block_buffer[block_buffer_tail]); } float plan_get_exec_block_exit_speed() { uint8_t block_index = plan_next_block_index(block_buffer_tail); if (block_index == block_buffer_head) { return( 0.0 ); } return( sqrt( block_buffer[block_index].entry_speed_sqr ) ); } // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { if (block_buffer_tail == next_buffer_head) { return(true); } return(false); } /* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. All position data passed to the planner must be in terms of machine position to keep the planner independent of any coordinate system changes and offsets, which are handled by the g-code parser. NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and invert_feed_rate always false). */ #ifdef USE_LINE_NUMBERS void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number) #else void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) #endif { // Prepare and initialize new block plan_block_t *block = &block_buffer[block_buffer_head]; block->step_event_count = 0; block->millimeters = 0; block->direction_bits = 0; block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later #ifdef USE_LINE_NUMBERS block->line_number = line_number; #endif // Compute and store initial move distance data. // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea // to try to keep these types of things completely separate from the planner for portability. int32_t target_steps[N_AXIS]; float unit_vec[N_AXIS], delta_mm; uint8_t idx; for (idx=0; idx<N_AXIS; idx++) { // Calculate target position in absolute steps. This conversion should be consistent throughout. target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]); // Number of steps for each axis and determine max step events block->steps[idx] = labs(target_steps[idx]-pl.position[idx]); block->step_event_count = max(block->step_event_count, block->steps[idx]); // Compute individual axes distance for move and prep unit vector calculations. // NOTE: Computes true distance from converted step values. delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. // Set direction bits. Bit enabled always means direction is negative. if (delta_mm < 0 ) { block->direction_bits |= get_direction_pin_mask(idx); } // Incrementally compute total move distance by Euclidean norm. First add square of each term. block->millimeters += delta_mm*delta_mm; } block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() // Bail if this is a zero-length block. Highly unlikely to occur. if (block->step_event_count == 0) { return; } // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } if (feed_rate < MINIMUM_FEED_RATE) { feed_rate = MINIMUM_FEED_RATE; } // Prevents step generation round-off condition. // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled // down such that no individual axes maximum values are exceeded with respect to the line direction. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. float inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides float junction_cos_theta = 0; for (idx=0; idx<N_AXIS; idx++) { if (unit_vec[idx] != 0) { // Avoid divide by zero. unit_vec[idx] *= inverse_millimeters; // Complete unit vector calculation inverse_unit_vec_value = fabs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides. // Check and limit feed rate against max individual axis velocities and accelerations feed_rate = min(feed_rate,settings.max_rate[idx]*inverse_unit_vec_value); block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction // between the current move and the previous move is simply the dot product of the two unit vectors, // where prev_unit_vec is negative. Used later to compute maximum junction speed. junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; } } // TODO: Need to check this method handling zero junction speeds when starting from rest. if (block_buffer_head == block_buffer_tail) { // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. block->entry_speed_sqr = 0.0; block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. } else { /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. Let a circle be tangent to both previous and current path line segments, where the junction deviation is defined as the distance from the junction to the closest edge of the circle, colinear with the circle center. The circular segment joining the two paths represents the path of centripetal acceleration. Solve for max velocity based on max acceleration about the radius of the circle, defined indirectly by junction deviation. This may be also viewed as path width or max_jerk in the previous grbl version. This approach does not actually deviate from path, but used as a robust way to compute cornering speeds, as it takes into account the nonlinearities of both the junction angle and junction velocity. NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here is exactly the same. Instead of motioning all the way to junction point, the machine will just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform a continuous mode path, but ARM-based microcontrollers most certainly do. NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be changed dynamically during operation nor can the line move geometry. This must be kept in memory in the event of a feedrate override changing the nominal speeds of blocks, which can change the overall maximum entry speed conditions of all blocks. */ // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the // two junctions. However, this shouldn't be a significant problem except in extreme circumstances. block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } // Store block nominal speed block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; // Update planner position memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] // New block is all set. Update buffer head and next buffer head indices. block_buffer_head = next_buffer_head; next_buffer_head = plan_next_block_index(block_buffer_head); // Finish up by recalculating the plan with the new block. planner_recalculate(); } // Reset the planner position vectors. Called by the system abort/initialization routine. void plan_sync_position() { uint8_t idx; for (idx=0; idx<N_AXIS; idx++) { pl.position[idx] = sys.position[idx]; } } // Returns the number of active blocks are in the planner buffer. uint8_t plan_get_block_buffer_count() { if (block_buffer_head >= block_buffer_tail) { return(block_buffer_head-block_buffer_tail); } return(BLOCK_BUFFER_SIZE - (block_buffer_tail-block_buffer_head)); } // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. void plan_cycle_reinitialize() { // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. st_update_plan_block_parameters(); block_buffer_planned = block_buffer_tail; planner_recalculate(); }