From 3082fdbb6d7e24527915ebc16d5dcf545bc2929c Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 10 Dec 2012 18:50:18 -0700 Subject: [PATCH] Planner execution time halved and bug fixes. Increased step rate limit to 30kHz. - Planner execute speed has been more than halved from 4ms to 1.9ms when computing a plan for a single line segment during arc generation. This means that Grbl can now run through an arc (or complex curve) twice as fast as before without starving the buffer. For 0.1mm arc segments, this means about the theoretical feed rate limit is about 3000mm/min for arcs now. - Increased the Ranade timer frequency to 30kHz, as there doesn't seem to be any problems with increasing the frequency. This means that the maximum step frequency is now back at 30kHz. - Added Zen Toolworks 7x7 defaults. --- config.h | 5 +- defaults.h | 36 ++++++++++- planner.c | 175 ++++++++++++++++++++++++++--------------------------- report.c | 2 +- stepper.c | 9 ++- 5 files changed, 129 insertions(+), 98 deletions(-) diff --git a/config.h b/config.h index 9b028f3..b164648 100644 --- a/config.h +++ b/config.h @@ -25,7 +25,7 @@ // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h -#define DEFAULTS_SHERLINE_5400 +#define DEFAULTS_GENERIC // Serial baud rate #define BAUD_RATE 9600 @@ -122,7 +122,7 @@ // frequency goes up. So there will be little left for other processes like arcs. // In future versions, more work will be done to increase the step rates but still stay around // 20kHz by performing two steps per step event, rather than just one. -#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz) +#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) // The Ranade algorithm can use either floating point or long integers for its counters, but for // integers the counter values must be scaled since these values can be very small (10^-6). This @@ -143,6 +143,7 @@ // in the stepper program and never runs slower than this value. If the RANADE_MULTIPLIER value // changes, it will affect how this value works. So, if a zero is add/subtracted from the // RANADE_MULTIPLIER value, do the same to this value if you want to same response. +// NOTE: Compute by (desired_step_rate/60) * RANADE_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) #define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic) // Minimum stepper rate. Only used by homing at this point. May be removed in later releases. diff --git a/defaults.h b/defaults.h index ab91038..30cc71d 100644 --- a/defaults.h +++ b/defaults.h @@ -55,8 +55,8 @@ #endif #ifdef DEFAULTS_SHERLINE_5400 - // Description: Sherline 5400 mill with three NEMA 23 185 oz-in stepper motors, driven by - // three Pololu A4988 stepper drivers with a 30V, 6A power supply at 1.5A per winding. + // Description: Sherline 5400 mill with three NEMA 23 Keling KL23H256-21-8B 185 oz-in stepper motors, + // driven by three Pololu A4988 stepper drivers with a 30V, 6A power supply at 1.5A per winding. #define MICROSTEPS 2 #define STEPS_PER_REV 200.0 #define MM_PER_REV (0.050*MM_PER_INCH) // 0.050 inch/rev leadscrew @@ -119,4 +119,36 @@ #define DEFAULT_N_ARC_CORRECTION 25 #endif +#ifdef DEFAULTS_ZEN_TOOLWORKS_7x7 + // Description: Zen Toolworks 7x7 mill with three Shinano SST43D2121 65oz-in NEMA 17 stepper motors. + // Leadscrew is different from some ZTW kits, where most are 1.25mm/rev rather than 8.0mm/rev here. + // Driven by 30V, 6A power supply and TI DRV8811 stepper motor drivers. + #define MICROSTEPS 8 + #define STEPS_PER_REV 200.0 + #define MM_PER_REV 8.0 // 8 mm/rev leadscrew + #define DEFAULT_X_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV) + #define DEFAULT_Y_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV) + #define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV) + #define DEFAULT_STEP_PULSE_MICROSECONDS 10 + #define DEFAULT_MM_PER_ARC_SEGMENT 0.1 + #define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min + #define DEFAULT_FEEDRATE 1000.0 // mm/min + #define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2 + #define DEFAULT_JUNCTION_DEVIATION 0.05 // mm + #define DEFAULT_STEPPING_INVERT_MASK (1<initial_rate = ceil(entry_speed*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - // First determine intersection distance from the exit point for a triangular profile. - float steps_per_mm = block->step_event_count/block->millimeters; - int32_t intersect_distance = ceil( steps_per_mm * - (0.5*block->millimeters+(entry_speed*entry_speed-exit_speed*exit_speed)/(4*settings.acceleration)) ); + // Compute efficiency variable for following calculations. Removes a float divide and multiply. + // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. + float steps_per_mm_div_2_acc = block->step_event_count/(2*settings.acceleration*block->millimeters); + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed*entry_speed-exit_speed*exit_speed)) ); // Check if this is a pure acceleration block by a intersection distance less than zero. Also // prevents signed and unsigned integer conversion errors. if (intersect_distance <= 0) { block->decelerate_after = 0; } else { - // Determine deceleration distance from nominal speed to exit speed for a trapezoidal profile. + // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - block->decelerate_after = ceil(steps_per_mm * - (block->nominal_speed*block->nominal_speed-exit_speed*exit_speed)/(2*settings.acceleration)); + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + block->decelerate_after = ceil(steps_per_mm_div_2_acc * + (block->nominal_speed*block->nominal_speed-exit_speed*exit_speed)); // The lesser of the two triangle and trapezoid distances always defines the velocity profile. if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } @@ -168,83 +171,66 @@ static void planner_recalculate() // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to // find out where in the planner buffer is the a safe place to begin re-planning from. - // Perform reverse planner pass. + // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the + // tail(first) block to prevent over-writing of the initial entry speed. uint8_t block_index = block_buffer_head; - block_t *next = NULL; - block_t *current = NULL; - block_t *previous = NULL; - - while(block_index != block_buffer_tail) { - block_index = prev_block_index( block_index ); + block_t *current = &block_buffer[block_index]; // Head block. + block_t *next; + if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); } + while (block_index != block_buffer_tail) { next = current; - current = previous; - previous = &block_buffer[block_index]; - - if (current) { // Cannot operate on nothing. - 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; - - } - } // Skip last block. Already initialized and set for recalculation. + current = &block_buffer[block_index]; + // 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, + calculate_final_velocity(settings.acceleration,next->entry_speed,current->millimeters)); // Back-compute + } else { + current->entry_speed = current->max_entry_speed; + } + current->recalculate_flag = true; } - } - // Skip buffer tail/first block to prevent over-writing the initial entry speed. - - // Perform forward planner pass. + block_index = prev_block_index( block_index ); + } + + // Perform forward planner pass. Begins junction speed adjustments after tail(first) block. + // Also recalculate trapezoids, block by block, as the forward pass completes the plan. block_index = block_buffer_tail; next = NULL; - while(block_index != block_buffer_head) { + while (block_index != block_buffer_head) { current = next; next= &block_buffer[block_index]; - // Begin planning after buffer_tail if (current) { - // If the previous block is an acceleration block, but it is not long enough to complete the + + // If the current 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 (!current->nominal_length_flag) { - if (current->entry_speed < current->entry_speed) { + if (current->entry_speed < next->entry_speed) { float entry_speed = min( next->entry_speed, - max_allowable_speed(-settings.acceleration,current->entry_speed,current->millimeters) ); + calculate_final_velocity(settings.acceleration,current->entry_speed,current->millimeters) ); // Check for junction speed change if (next->entry_speed != entry_speed) { next->entry_speed = entry_speed; next->recalculate_flag = true; } - } + } + + // Recalculate if current block entry or exit junction speed has changed. + if (current->recalculate_flag || next->recalculate_flag) { + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed); + current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + } } - } - block_index = next_block_index( block_index ); - } - - // Recalculate stepper algorithm data for any adjacent blocks with a modified junction speed. - block_index = block_buffer_tail; - 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) { - // NOTE: Entry and exit factors always > 0 by all previous logic operations. - calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed - } + } block_index = next_block_index( block_index ); } @@ -311,12 +297,6 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); - - // Compute direction bits for this block - block->direction_bits = 0; - if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); @@ -328,31 +308,50 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert if (block->step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions + // TODO: Store last xyz in memory to remove steps_per_mm divides. Or come up with another way to save cycles. float delta_mm[3]; - delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS]; - delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; - delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; + delta_mm[X_AXIS] = block->steps_x/settings.steps_per_mm[X_AXIS]; + delta_mm[Y_AXIS] = block->steps_y/settings.steps_per_mm[Y_AXIS]; + delta_mm[Z_AXIS] = block->steps_z/settings.steps_per_mm[Z_AXIS]; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + + // Compute path unit vector + float unit_vec[3]; + 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 direction bits and correct unit vector directions + block->direction_bits = 0; + if (target[X_AXIS] < pl.position[X_AXIS]) { + block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<millimeters/feed_rate; } block->nominal_speed = feed_rate; // (mm/min) Always > 0 + // TODO: When acceleration independence is installed, it can be kept in terms of 2*acceleration for + // each block. This could save some 2*acceleration multiplications elsewhere. Need to check though. + // Compute the acceleration, nominal rate, and distance traveled per step event for the stepper algorithm. block->rate_delta = ceil(settings.acceleration* ((RANADE_MULTIPLIER/(60*60))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) block->nominal_rate = ceil(block->nominal_speed*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - // Compute path unit vector - float unit_vec[3]; - 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 // deviation is defined as the distance from the junction to the closest edge of the circle, @@ -392,8 +391,8 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert } block->max_entry_speed = vmax_junction; - // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - float v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. + float v_allowable = calculate_final_velocity(settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags diff --git a/report.c b/report.c index 8b12243..04eec17 100644 --- a/report.c +++ b/report.c @@ -296,7 +296,7 @@ void report_realtime_status() // Report current machine state switch (sys.state) { case STATE_IDLE: printPgmString(PSTR("