From 6de805441fe00b436c1f44997df4034543164b50 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Sun, 18 Sep 2011 05:36:55 -0600 Subject: [PATCH] Fixed minor bugs in planner. Increased max dwell time. Long slope bug stop-gap solution note. - Fixed the planner TODO regarding minimum nominal speeds. Re-arranged calculations to be both more efficient and guaranteed to be greater than zero. - Missed a parenthesis location on the rate_delta calculation. Should fix a nearly in-perceptible issue with incorrect acceleration ramping in diagonal directions. - Increased maximum dwell time from 6.5sec to an 18hour max. A crazy amount more, but that's how the math works out. - Converted the internal feedrate values to mm/min only, as it was switching between mm/min to mm/sec and back to mm/min. Also added a feedrate > 0 check in gcode.c. - Identified the long slope at the end of rapid de/ac-celerations noted by stephanix. Problem with the numerical integration truncation error between the exact solution of estimate_acceleration_distance and how grbl actually performs the acceleration ramps discretely. Increasing the ACCELERATION_TICKS_PER_SECOND in config.h helps fix this problem. Investigating further. --- config.h | 5 +++++ gcode.c | 11 ++++++----- motion_control.c | 16 ++++++++++++---- motion_control.h | 4 ++-- planner.c | 48 +++++++++++++++++++++--------------------------- 5 files changed, 46 insertions(+), 38 deletions(-) diff --git a/config.h b/config.h index e2811cd..952f7a2 100644 --- a/config.h +++ b/config.h @@ -55,6 +55,11 @@ // The temporal resolution of the acceleration management subsystem. Higher number // give smoother acceleration but may impact performance +// NOTE: Increasing this parameter will help remove the long slow motion bug at the end +// of very fast de/ac-celerations. This is due to the increased resolution of the +// acceleration steps that more accurately predicted by the planner exact integration +// of acceleration distance. An efficient solution to this bug is under investigation. +// In general, setting this parameter is high as your system will allow is suggested. #define ACCELERATION_TICKS_PER_SECOND 40L #endif diff --git a/gcode.c b/gcode.c index 7127932..1027a7f 100644 --- a/gcode.c +++ b/gcode.c @@ -88,8 +88,8 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) void gc_init() { memset(&gc, 0, sizeof(gc)); - gc.feed_rate = settings.default_feed_rate/60; - gc.seek_rate = settings.default_seek_rate/60; + gc.feed_rate = settings.default_feed_rate; + gc.seek_rate = settings.default_seek_rate; select_plane(X_AXIS, Y_AXIS, Z_AXIS); gc.absolute_mode = true; } @@ -180,13 +180,14 @@ uint8_t gc_execute_line(char *line) { unit_converted_value = to_millimeters(value); switch(letter) { case 'F': + if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero if (gc.inverse_feed_rate_mode) { inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only } else { if (gc.motion_mode == MOTION_MODE_SEEK) { - gc.seek_rate = unit_converted_value/60; + gc.seek_rate = unit_converted_value; } else { - gc.feed_rate = unit_converted_value/60; // millimeters pr second + gc.feed_rate = unit_converted_value; // millimeters per minute } } break; @@ -213,7 +214,7 @@ uint8_t gc_execute_line(char *line) { // Perform any physical actions switch (next_action) { case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; - case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; + case NEXT_ACTION_DWELL: mc_dwell(p); break; case NEXT_ACTION_SET_COORDINATE_OFFSET: mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]); break; diff --git a/motion_control.c b/motion_control.c index aec1c1c..98931f8 100644 --- a/motion_control.c +++ b/motion_control.c @@ -29,13 +29,21 @@ #include "stepper.h" #include "planner.h" -#define N_ARC_CORRECTION 25 // (0-255) Number of iterations before arc trajectory correction +// Number of arc generation iterations with small angle approximation before exact arc +// trajectory correction. Value must be 1-255. +#define N_ARC_CORRECTION 25 -void mc_dwell(uint32_t milliseconds) +// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. +void mc_dwell(double seconds) { - st_synchronize(); - _delay_ms(milliseconds); + uint16_t i = floor(seconds); + st_synchronize(); + _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder + while (i > 0) { + _delay_ms(1000); // Delay one second + i--; + } } // Execute an arc in offset mode format. position == current xyz, target == target xyz, diff --git a/motion_control.h b/motion_control.h index cfff84c..b91650d 100644 --- a/motion_control.h +++ b/motion_control.h @@ -44,8 +44,8 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise); #endif -// Dwell for a couple of time units -void mc_dwell(uint32_t milliseconds); +// Dwell for a specific number of seconds +void mc_dwell(double seconds); // Send the tool home (not implemented) void mc_go_home(); diff --git a/planner.c b/planner.c index 32f7a98..4be9c88 100644 --- a/planner.c +++ b/planner.c @@ -48,8 +48,6 @@ static double previous_nominal_speed; // Nominal speed of previous path line s static uint8_t acceleration_manager_enabled; // Acceleration management active? -#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 - // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. @@ -62,8 +60,8 @@ static int8_t next_block_index(int8_t block_index) { // Returns the index of the previous block in the ring buffer static int8_t prev_block_index(int8_t block_index) { - if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE-1; } - else { block_index--; } + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; return(block_index); } @@ -197,9 +195,9 @@ static void planner_forward_pass() { // This converts the planner parameters to the data required by the stepper controller. static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { - block->initial_rate = ceil(block->nominal_rate*entry_factor); - block->final_rate = ceil(block->nominal_rate*exit_factor); - int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; + block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) + block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) + int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2) int32_t accelerate_steps = ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); int32_t decelerate_steps = @@ -356,42 +354,38 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); - - uint32_t microseconds; + double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + + // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks. + // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c + double inverse_minute; if (!invert_feed_rate) { - microseconds = lround((block->millimeters/feed_rate)*1000000); + inverse_minute = feed_rate * inverse_millimeters; } else { - microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate); + inverse_minute = 1.0 / feed_rate; } + block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0 + block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0 - // Calculate speed in mm/minute for each axis - double multiplier = 60.0*1000000.0/microseconds; - block->nominal_speed = block->millimeters * multiplier; - block->nominal_rate = ceil(block->step_event_count * multiplier); - - // This is a temporary fix to avoid a situation where very low nominal_speeds would be rounded - // down to zero and cause a division by zero. TODO: Grbl deserves a less patchy fix for this problem - if (block->nominal_speed < 60.0) { block->nominal_speed = 60.0; } - // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: - double step_per_travel = block->step_event_count/block->millimeters; // Compute inverse to remove divide - block->rate_delta = step_per_travel * ceil( // convert to: acceleration steps/min/acceleration_tick - settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // acceleration mm/sec/sec per acceleration_tick + // Convert universal acceleration for direction-dependent stepper rate change parameter + block->rate_delta = ceil( block->step_event_count*inverse_millimeters * + settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick) // Perform planner-enabled calculations if (acceleration_manager_enabled) { // Compute path unit vector double unit_vec[3]; - double inv_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides - unit_vec[X_AXIS] = delta_mm[X_AXIS]*inv_millimeters; - unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inv_millimeters; - unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters; + + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // 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