Further planner improvements and misc minor bug fixes. Memory savings and increased buffer size.

- Update grbl version and settings version to automatically reset
eeprom. FYI, this will reset your grbl settings. - Saved
3*BLOCK_BUFFER_SIZE doubles in static memory by removing obsolete
variables: speed_x, speed_y, and speed_z. - Increased buffer size
conservatively to 18 from 16. (Probably can do 20). - Removed expensive!
modulo operator from block indexing function. Reduces significant
computational overhead. - Re-organized some sqrt() calls to be more
efficient during time critical planning cases, rather than non-time
critical. - Minor bug fix in planner max junction velocity logic. -
Simplified arc logic and removed need to multiply for CW or CCW
direction.
This commit is contained in:
Sonny J 2011-09-13 21:57:16 -06:00
parent ffcc3470a3
commit 4d03c4febc
10 changed files with 103 additions and 104 deletions

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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
@ -328,13 +328,13 @@ uint8_t gc_execute_line(char *line) {
}
// Set clockwise/counter-clockwise sign for mc_arc computations
int8_t clockwise_sign = 1;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { clockwise_sign = -1; }
int8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, clockwise_sign);
r, isclockwise);
break;
#endif

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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
@ -48,7 +48,7 @@ void mc_dwell(uint32_t milliseconds)
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
// segment is configured in settings.mm_per_arc_segment.
void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign)
uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise)
{
// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
@ -64,7 +64,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (angular_travel < 0) { angular_travel += 2*M_PI; }
if (clockwise_sign < 0) { angular_travel = 2*M_PI-angular_travel; }
if (isclockwise) { angular_travel -= 2*M_PI; }
double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (millimeters_of_travel == 0.0) { return; }
@ -104,7 +104,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
*/
// Vector rotation matrix values
double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
double sin_T = clockwise_sign*theta_per_segment;
double sin_T = theta_per_segment;
double trajectory[3];
double sin_Ti;
@ -128,7 +128,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = clockwise_sign*sin(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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
@ -41,7 +41,7 @@
// the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used
// for vector transformation direction.
void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign);
uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise);
#endif
// Dwell for a couple of time units

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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

162
planner.c
View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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
@ -33,7 +33,7 @@
// The number of linear motions that can be in the plan at any give time
#ifdef __AVR_ATmega328P__
#define BLOCK_BUFFER_SIZE 16
#define BLOCK_BUFFER_SIZE 18
#else
#define BLOCK_BUFFER_SIZE 5
#endif
@ -52,15 +52,18 @@ static uint8_t acceleration_manager_enabled; // Acceleration management active
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static int8_t next_block_index(int8_t block_index) {
return( (block_index + 1) % BLOCK_BUFFER_SIZE );
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 int8_t prev_block_index(int8_t block_index) {
block_index--;
if (block_index < 0) { block_index = BLOCK_BUFFER_SIZE-1; }
if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE-1; }
else { block_index--; }
return(block_index);
}
@ -90,41 +93,38 @@ static double intersection_distance(double initial_rate, double final_rate, doub
}
// Calculates the square of the maximum allowable speed at this point when you must be able to reach
// target_velocity using the acceleration within the allotted distance.
// NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
static double max_allowable_speed_sqr(double acceleration, double target_velocity_sqr, double distance) {
return( target_velocity_sqr-2*acceleration*60*60*distance );
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
// using the acceleration within the allotted distance.
// NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
// in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
// BLOCK_BUFFER_SIZE calls per planner cycle.
static double max_allowable_speed(double acceleration, double target_velocity, double distance) {
return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) );
}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if (!current) { return; }
if (!current) { return; } // Cannot operate on nothing.
double entry_speed_sqr = current->max_entry_speed_sqr; // Reset and check to ensure max possible speed
if (next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed != current->max_entry_speed) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false.
if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
current->entry_speed = min( current->max_entry_speed,
max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
} else {
current->entry_speed = current->max_entry_speed;
}
current->recalculate_flag = true;
// If nominal length true, nominal speed is guaranteed to be reached. No need to re-compute.
// But, if forward planner changed entry speed, reset to max entry speed just to be sure.
if (!current->nominal_length_flag) {
if (next) {
// If the required deceleration across the block is too rapid, reduce entry_speed_sqr accordingly.
if (entry_speed_sqr > next->entry_speed_sqr) {
entry_speed_sqr = min( entry_speed_sqr,
max_allowable_speed_sqr(-settings.acceleration,next->entry_speed_sqr,current->millimeters));
}
} else {
// Assume last block has zero exit velocity.
entry_speed_sqr = min( entry_speed_sqr,
max_allowable_speed_sqr(-settings.acceleration,0.0,current->millimeters));
}
}
// Check for junction speed change
if (current->entry_speed_sqr != entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
current->recalculate_flag = true; // Note: Newest block already set to true
}
} // Skip last block. Already initialized and set for recalculation.
}
@ -140,34 +140,29 @@ static void planner_reverse_pass() {
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
// Skip buffer tail to prevent over-writing the initial entry speed.
// Skip buffer tail/first block to prevent over-writing the initial entry speed.
}
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!current) { return; }
if(!previous) { return; } // Begin planning after buffer_tail
// If the previous block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the entry speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
if (!previous->nominal_length_flag) {
if (previous->entry_speed < current->entry_speed) {
double entry_speed = min( current->entry_speed,
max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
if(previous) {
// If nominal length true, nominal speed is guaranteed to be reached. No need to recalculate.
if (!previous->nominal_length_flag) {
// If the previous block is an acceleration block, but it is not long enough to
// complete the full speed change within the block, we need to adjust the entry
// speed accordingly.
if (previous->entry_speed_sqr < current->entry_speed_sqr) {
double entry_speed_sqr = min( current->entry_speed_sqr, current->max_entry_speed_sqr );
entry_speed_sqr = min( entry_speed_sqr,
max_allowable_speed_sqr(-settings.acceleration,previous->entry_speed_sqr,previous->millimeters) );
// Check for junction speed change
if (current->entry_speed_sqr != entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
current->recalculate_flag = true;
}
// Check for junction speed change
if (current->entry_speed != entry_speed) {
current->entry_speed = entry_speed;
current->recalculate_flag = true;
}
}
}
}
}
@ -243,25 +238,23 @@ static void planner_recalculate_trapezoids() {
int8_t block_index = block_buffer_tail;
block_t *current;
block_t *next = NULL;
while(block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current) {
// Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) {
// Compute entry and exit factors for trapezoid calculations.
// NOTE: sqrt(square velocities) now performed only when required in trapezoid calculation.
double entry_factor = sqrt( current->entry_speed_sqr ) / current->nominal_speed;
double exit_factor = sqrt( next->entry_speed_sqr ) / current->nominal_speed;
calculate_trapezoid_for_block(current, entry_factor, exit_factor);
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
next->entry_speed/current->nominal_speed);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
}
}
block_index = next_block_index( block_index );
}
// Last/newest block in buffer. Exit speed is zero.
calculate_trapezoid_for_block(next, sqrt( next->entry_speed_sqr ) / next->nominal_speed, 0.0);
// Last/newest block in buffer. Exit speed is zero. Always recalculated.
calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, 0.0);
next->recalculate_flag = false;
}
@ -373,9 +366,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
// Calculate speed in mm/minute for each axis
double multiplier = 60.0*1000000.0/microseconds;
block->speed_x = delta_mm[X_AXIS] * multiplier;
block->speed_y = delta_mm[Y_AXIS] * multiplier;
block->speed_z = delta_mm[Z_AXIS] * multiplier;
block->nominal_speed = block->millimeters * multiplier;
block->nominal_rate = ceil(block->step_event_count * multiplier);
@ -404,16 +394,15 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Does not actually deviate from path, but used as a robust way to compute cornering speeds.
// 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.
// NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
double vmax_junction_sqr = 0.0; // Set default zero max junction speed
// 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.
double vmax_junction = 0.0; // Set default zero max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
@ -423,25 +412,33 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default zero max junction speed for 0 degree acute junction.
if (cos_theta < 1.0) {
vmax_junction_sqr = square( min(previous_nominal_speed,block->nominal_speed) );
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -1.0) {
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction_sqr = min(vmax_junction_sqr,
settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) );
vmax_junction = min(vmax_junction,
sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
block->max_entry_speed_sqr = vmax_junction_sqr;
block->entry_speed_sqr = vmax_junction_sqr;
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to rest (zero speed).
double v_allowable = max_allowable_speed(-settings.acceleration,0.0,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach nominal speed regardless of entry/exit speeds.
if (block->nominal_speed <= sqrt(max_allowable_speed_sqr(-settings.acceleration,0.0,0.5*block->millimeters)) )
{ block->nominal_length_flag = true; }
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
@ -471,6 +468,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
void plan_set_current_position(double x, double y, double z) {
position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
previous_nominal_speed = 0.0;
position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
clear_vector_double(previous_unit_vec);
}

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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
@ -34,10 +34,9 @@ typedef struct {
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
// Fields used by the motion planner to manage acceleration
double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis
double nominal_speed; // The nominal speed for this block in mm/min
double entry_speed_sqr; // Square of entry speed at previous-current junction in (mm/min)^2
double max_entry_speed_sqr; // Square of maximum allowable entry speed in (mm/min)^2
double entry_speed; // Entry speed at previous-current junction in mm/min
double max_entry_speed; // Maximum allowable junction entry speed in mm/min
double millimeters; // The total travel of this block in mm
uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
uint8_t nominal_length_flag; // Planner flag for nominal speed always reached

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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

View File

@ -8,6 +8,8 @@ G-code preprocessor for grbl (BETA!)
- OPTIONAL: Remove unsupported grbl G and M commands
TODO:
- Number precision truncation
- Arc conversion option
- More robust error checking
- Improve interface to command line options
- Improve g-code parsing to NIST standards

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 Sungeun 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
@ -52,7 +52,7 @@ typedef struct {
#define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 500.0
#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0)
#define DEFAULT_JUNCTION_DEVIATION 0.1
#define DEFAULT_JUNCTION_DEVIATION 0.05
#define DEFAULT_STEPPING_INVERT_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT))
void settings_reset() {

View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
Copyright (c) 2011 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
@ -26,11 +26,11 @@
#include <math.h>
#include <inttypes.h>
#define GRBL_VERSION "0.7b"
#define GRBL_VERSION "0.7c"
// Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl
// when firmware is upgraded. Always stored in byte 0 of eeprom
#define SETTINGS_VERSION 2
#define SETTINGS_VERSION 3
// Current global settings (persisted in EEPROM from byte 1 onwards)
typedef struct {