diff --git a/gcode.c b/gcode.c index 511bc59..29afe14 100644 --- a/gcode.c +++ b/gcode.c @@ -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 diff --git a/motion_control.c b/motion_control.c index bf2ea44..8b55779 100644 --- a/motion_control.c +++ b/motion_control.c @@ -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; diff --git a/motion_control.h b/motion_control.h index f763e6b..ba01315 100644 --- a/motion_control.h +++ b/motion_control.h @@ -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 diff --git a/nuts_bolts.h b/nuts_bolts.h index 5e2b04c..37f3aa6 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -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 diff --git a/planner.c b/planner.c index f5139ff..32f7a98 100644 --- a/planner.c +++ b/planner.c @@ -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); } diff --git a/planner.h b/planner.h index 541efad..f995210 100644 --- a/planner.h +++ b/planner.h @@ -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 diff --git a/protocol.c b/protocol.c index e6c4ea4..4835c90 100644 --- a/protocol.c +++ b/protocol.c @@ -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 diff --git a/script/grbl_preprocess.py b/script/grbl_preprocess.py index ab6a853..bbb08c8 100755 --- a/script/grbl_preprocess.py +++ b/script/grbl_preprocess.py @@ -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 diff --git a/settings.c b/settings.c index 5c9613d..b60f35e 100644 --- a/settings.c +++ b/settings.c @@ -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< #include -#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 {