diff --git a/config.h b/config.h index 3791b31..c1c450e 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_GENERIC +#define DEFAULTS_ZEN_TOOLWORKS_7x7 // Serial baud rate #define BAUD_RATE 9600 @@ -113,7 +113,7 @@ // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but // CPU overhead increases exponentially as this frequency goes up. So there will be little left for // other processes like arcs. -#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) +#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz) // The temporal resolution of the acceleration management subsystem. Higher number give smoother // acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and @@ -121,7 +121,8 @@ // profiles and how the stepper program actually performs them. The correct value for this parameter // is machine dependent, so it's advised to set this only as high as needed. Approximate successful // values can widely range from 50 to 200 or more. Cannot be greater than ISR_TICKS_PER_SECOND/2. -#define ACCELERATION_TICKS_PER_SECOND 120L +// NOTE: Ramp count variable type in stepper module may need to be updated if changed. +#define ACCELERATION_TICKS_PER_SECOND 100L // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) @@ -134,7 +135,7 @@ // applications, the following multiplier value will work more than well enough. If you do have // happened to weird stepper motion issues, try modifying this value by adding or subtracting a // zero and report it to the Grbl administrators. -#define RANADE_MULTIPLIER 100000000.0 +#define INV_TIME_MULTIPLIER 10000000.0 // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed diff --git a/gcode.c b/gcode.c index d87ce52..7623d1e 100644 --- a/gcode.c +++ b/gcode.c @@ -39,7 +39,9 @@ parser_state_t gc; #define FAIL(status) gc.status_code = status; -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); +static void gc_convert_arc_radius_mode(float *target) __attribute__((noinline)); + static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) { @@ -48,6 +50,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) gc.plane_axis_2 = axis_2; } + void gc_init() { memset(&gc, 0, sizeof(gc)); @@ -61,20 +64,24 @@ void gc_init() } } + // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard // limit pull-off routines. -void gc_set_current_position(int32_t x, int32_t y, int32_t z) +void gc_sync_position(int32_t x, int32_t y, int32_t z) { - gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS]; - gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS]; - gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS]; + uint8_t i; + for (i=0; i C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| - - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O - - Expanding the equations: - - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - - Which can be written: - - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - - Which we for size and speed reasons optimize to: - - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 - - */ - - // Calculate the change in position along each selected axis - float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; - float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; - - clear_vector(offset); - // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller - // than d. If so, the sqrt of a negative number is complex and error out. - float h_x2_div_d = 4 * r*r - x*x - y*y; - if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); } - // Finish computing h_x2_div_d. - h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } - - /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. - - - T <-- Target position - - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / - center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | - - C <-- Current position */ - - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), - // even though it is advised against ever generating such circles in a single line of g-code. By - // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of - // travel and thus we get the unadvisably long arcs as prescribed. - if (r < 0) { - h_x2_div_d = -h_x2_div_d; - r = -r; // Finished with r. Set to positive for mc_arc - } - // Complete the operation by calculating the actual center of the arc - offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); - offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); - + if (gc.arc_radius != 0) { // Arc Radius Mode + // Compute arc radius and offsets + gc_convert_arc_radius_mode(target); + if (gc.status_code) { return(gc.status_code); } } else { // Arc Center Format Offset Mode - r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc + gc.arc_radius = hypot(gc.arc_offset[gc.plane_axis_0], gc.arc_offset[gc.plane_axis_1]); // Compute arc radius for mc_arc } // Set clockwise/counter-clockwise sign for mc_arc computations @@ -523,9 +443,9 @@ uint8_t gc_execute_line(char *line) 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, + mc_arc(gc.position, target, gc.arc_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, isclockwise); + gc.arc_radius, isclockwise); } break; } @@ -557,7 +477,7 @@ uint8_t gc_execute_line(char *line) // Parses the next statement and leaves the counter on the first character following // the statement. Returns 1 if there was a statements, 0 if end of string was reached // or there was an error (check state.status_code). -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) { if (line[*char_counter] == 0) { return(0); // No more statements @@ -576,6 +496,100 @@ static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *c return(1); } + +static void gc_convert_arc_radius_mode(float *target) +{ +/* We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the + length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point + [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h + + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 */ + + // Calculate the change in position along each selected axis + float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; + float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; + + clear_vector(gc.arc_offset); + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y; + if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; } + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position */ + + + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), + // even though it is advised against ever generating such circles in a single line of g-code. By + // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of + // travel and thus we get the unadvisably long arcs as prescribed. + if (gc.arc_radius < 0) { + h_x2_div_d = -h_x2_div_d; + gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); + gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); +} + /* Not supported: diff --git a/gcode.h b/gcode.h index 50b4228..6b8949a 100644 --- a/gcode.h +++ b/gcode.h @@ -82,7 +82,11 @@ typedef struct { float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine // position in mm. Loaded from EEPROM when called. float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to - // machine zero in mm. Non-persistent. Cleared upon reset and boot. + // machine zero in mm. Non-persistent. Cleared upon reset and boot. + + float arc_radius; + float arc_offset[N_AXIS]; + } parser_state_t; extern parser_state_t gc; @@ -93,6 +97,6 @@ void gc_init(); uint8_t gc_execute_line(char *line); // Set g-code parser position. Input in steps. -void gc_set_current_position(int32_t x, int32_t y, int32_t z); +void gc_sync_position(); #endif diff --git a/limits.c b/limits.c index 3507785..a03d32e 100644 --- a/limits.c +++ b/limits.c @@ -96,16 +96,17 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl // and speedy homing routine. // NOTE: For each axes enabled, the following calculations assume they physically move // an equal distance over each time step until they hit a limit switch, aka dogleg. - uint32_t step_event_count, steps[N_AXIS]; + uint32_t step_event_count = 0; uint8_t i, dist = 0; + uint32_t steps[N_AXIS]; clear_vector(steps); for (i=0; i 0 || target[X_AXIS] < -settings.max_travel[X_AXIS] || - target[Y_AXIS] > 0 || target[Y_AXIS] < -settings.max_travel[Y_AXIS] || - target[Z_AXIS] > 0 || target[Z_AXIS] < -settings.max_travel[Z_AXIS] ) { - - // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within - // workspace volume so just come to a controlled stop so position is not lost. When complete - // enter alarm mode. - if (sys.state == STATE_CYCLE) { - st_feed_hold(); - while (sys.state == STATE_HOLD) { - protocol_execute_runtime(); - if (sys.abort) { return; } - } - } + uint8_t idx; + for (idx=0; idx 0 || target[idx] < settings.max_travel[idx]) { // NOTE: max_travel is stored as negative - mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event - protocol_execute_runtime(); // Execute to enter critical event loop and system abort + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + if (sys.state == STATE_CYCLE) { + st_feed_hold(); + while (sys.state == STATE_HOLD) { + protocol_execute_runtime(); + if (sys.abort) { return; } + } + } + + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event + protocol_execute_runtime(); // Execute to enter critical event loop and system abort + return; + + } } } diff --git a/main.c b/main.c index 84b0313..9decae8 100644 --- a/main.c +++ b/main.c @@ -72,7 +72,8 @@ int main(void) // Sync cleared gcode and planner positions to current system position, which is only // cleared upon startup, not a reset/abort. - sys_sync_current_position(); + plan_sync_position(); + gc_sync_position(); // Reset system variables. sys.abort = false; @@ -101,12 +102,12 @@ int main(void) } protocol_execute_runtime(); - protocol_process(); // ... process the serial protocol // When the serial protocol returns, there are no more characters in the serial read buffer to // be processed and executed. This indicates that individual commands are being issued or // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves. mc_auto_cycle_start(); + protocol_process(); // ... process the serial protocol } return 0; /* never reached */ diff --git a/motion_control.c b/motion_control.c index 2a2f508..e420c98 100644 --- a/motion_control.c +++ b/motion_control.c @@ -72,7 +72,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) else { break; } } while (1); - plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); + plan_buffer_line(target, feed_rate, invert_feed_rate); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. @@ -148,8 +148,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 This is important when there are successive arc motions. */ // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec - float cos_T = 2 - theta_per_segment*theta_per_segment; - float sin_T = theta_per_segment*0.16666667*(cos_T + 4); + float cos_T = 2.0 - theta_per_segment*theta_per_segment; + float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); cos_T *= 0.5; float arc_target[N_AXIS]; @@ -229,37 +229,34 @@ void mc_go_home() // At the same time, set up pull-off maneuver from axes limit switches that have been homed. // This provides some initial clearance off the switches and should also help prevent them // from falsely tripping when hard limits are enabled. - // TODO: Need to improve dir_mask[] to be more axes independent. float pulloff_target[N_AXIS]; clear_vector_float(pulloff_target); // Zero pulloff target. clear_vector_long(sys.position); // Zero current position for now. - uint8_t dir_mask[N_AXIS]; - dir_mask[X_AXIS] = (1< + - +--------+ <- nominal_rate /|\ - / \ / | \ - initial_rate -> + \ / | + <- next->initial_rate - | + <- next->initial_rate / | | - +-------------+ initial_rate -> +----+--+ - time --> ^ ^ ^ ^ - | | | | - decelerate distance decelerate distance - - Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be - described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block - reaches the nominal speed of the block and cruises for a period of time. A triangle occurs - when the nominal speed is not reached within the block. Some other special cases exist, - such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that - has no deceleration period when the next block resumes acceleration. - - The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. In this case, only - the new initial rate and n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. -*/ -static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) -{ - // Compute new initial rate for stepper algorithm - block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Compute new nominal rate if a feedrate override occurs. - // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // 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*block->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_sqr-exit_speed_sqr)) ); - - // 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 (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. - // 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_sqr - exit_speed_sqr)); - - // 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; } - - // Finally, check if this is a pure deceleration block. - if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } - } -} - + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed @@ -185,178 +126,130 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, */ static void planner_recalculate() { - -// float entry_speed_sqr; -// uint8_t block_index = block_buffer_head; -// block_t *previous = NULL; -// block_t *current = NULL; -// block_t *next; -// while (block_index != block_buffer_tail) { -// block_index = prev_block_index( block_index ); -// next = current; -// current = previous; -// previous = &block_buffer[block_index]; -// -// if (next && current) { -// if (next != block_buffer_planned) { -// if (previous == block_buffer_tail) { block_buffer_planned = next; } -// else { -// -// if (current->entry_speed_sqr != current->max_entry_speed_sqr) { -// current->recalculate_flag = true; // Almost always changes. So force recalculate. -// 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; -// } -// } else { -// block_buffer_planned = current; -// } -// } -// } else { -// break; -// } -// } -// } -// -// block_index = block_buffer_planned; -// next = &block_buffer[block_index]; -// current = prev_block_index(block_index); -// while (block_index != block_buffer_head) { -// -// // 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 exit speed accordingly. Entry -// // speeds have already been reset, maximized, and reverse planned by reverse planner. -// if (current->entry_speed_sqr < next->entry_speed_sqr) { -// // Compute block exit speed based on the current block speed and distance -// // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance -// entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// -// // If it's less than the stored value, update the exit speed and set recalculate flag. -// if (entry_speed_sqr < next->entry_speed_sqr) { -// next->entry_speed_sqr = entry_speed_sqr; -// 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_sqr, next->entry_speed_sqr); -// current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed -// } -// -// current = next; -// next = &block_buffer[block_index]; -// block_index = next_block_index( block_index ); -// } -// -// // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. -// calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); -// next->recalculate_flag = false; + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer - // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but - // 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. + // Ping the stepper algorithm to check if we can alter the parameters of the currently executing + // block. If not, skip it and work on the next block. + // TODO: Need to look into if there are conditions where this fails. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + // TODO: Need to recompute buffer tail millimeters based on how much is completed. + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on -// if (block_buffer_head != block_buffer_tail) { - float entry_speed_sqr; + block_buffer_planned = block_buffer_safe; +// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - // 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 = prev_block_index( block_buffer_head ); // Assume buffer is not empty. - block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended 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 = &block_buffer[block_index]; - - // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter - // the planner nominal speeds at any time. This calc could be done in the override handler, but - // this could require an additional variable to be stored to differentiate the programmed nominal - // speeds, max junction speed, and override speeds/scalar. - - // 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_sqr != current->max_entry_speed_sqr) { + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); +// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - current->entry_speed_sqr = current->max_entry_speed_sqr; - current->recalculate_flag = true; // Almost always changes. So force recalculate. - - if (next->entry_speed_sqr < current->max_entry_speed_sqr) { - // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + plan_block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + 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; } - } - } - block_index = prev_block_index( block_index ); - } + } + 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 = next_block_index(block_buffer_tail); - next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // 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 exit speed accordingly. Entry - // speeds have already been reset, maximized, and reverse planned by reverse planner. + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_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) { - // Compute block exit speed based on the current block speed and distance - // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance + block_buffer_planned = block_index; entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - - // If it's less than the stored value, update the exit speed and set recalculate flag. if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; - next->recalculate_flag = true; + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. } } - - // 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_sqr, next->entry_speed_sqr); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; } - - block_index = next_block_index( block_index ); - } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. +// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + block_index = next_block_index( block_index ); + } + + } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - next->recalculate_flag = false; -// } } + void plan_init() { + block_buffer_head = 0; block_buffer_tail = block_buffer_head; next_buffer_head = next_block_index(block_buffer_head); -// block_buffer_planned = block_buffer_head; + block_buffer_planned = block_buffer_head; memset(&pl, 0, sizeof(pl)); // Clear planner struct } -inline void plan_discard_current_block() + +void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { block_buffer_tail = next_block_index( block_buffer_tail ); } } -inline block_t *plan_get_current_block() + +plan_block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } return(&block_buffer[block_buffer_tail]); } + +plan_block_t *plan_get_block_by_index(uint8_t block_index) +{ + if (block_buffer_head == block_index) { return(NULL); } + return(&block_buffer[block_index]); +} + + // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { @@ -364,6 +257,7 @@ uint8_t plan_check_full_buffer() return(false); } + // Block until all buffered steps are executed or in a cycle state. Works with feed hold // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() @@ -374,43 +268,54 @@ void plan_synchronize() } } -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed + +// 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. -// 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). -void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) +// 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). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { - // Prepare to set up new block - block_t *block = &block_buffer[block_buffer_head]; + // 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 - // Calculate target position in absolute steps - int32_t target[N_AXIS]; - 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 and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[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_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() - // Number of steps for each axis - block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); - block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); - block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]); - block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); - // Bail if this is a zero-length block - if (block->step_event_count == 0) { return; }; + if (block->step_event_count == 0) { return; } - // Compute path vector in terms of absolute step target and current positions - float delta_mm[N_AXIS]; - delta_mm[X_AXIS] = x-pl.last_x; - delta_mm[Y_AXIS] = y-pl.last_y; - delta_mm[Z_AXIS] = z-pl.last_z; - 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]); - // 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 @@ -422,117 +327,151 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert // and axes properties as well. // 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. - uint8_t i; - float unit_vec[N_AXIS], inverse_unit_vec_value; + float inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop - for (i=0; iacceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value); + float junction_cos_theta = 0; + for (idx=0; idxacceleration = 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]; } } - // Compute nominal speed and rates - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 - block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + /* 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. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // 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. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } - // Compute the acceleration and distance traveled per step event for the stepper algorithm. - block->rate_delta = ceil(block->acceleration* - ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - - // Compute direction bits. Bit enabled always means direction is negative. - block->direction_bits = 0; - if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; - if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) { - // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) - // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. - float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95) { - block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr); - // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -0.95) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = min(block->max_entry_speed_sqr, - block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)); - } - } - } - - // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. - // TODO: This could be moved to the planner recalculate function. - block->entry_speed_sqr = min( block->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters); - - // Set new block to be recalculated for conversion to stepper data. - block->recalculate_flag = true; + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 +// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) +// +// // Compute and store acceleration and distance traveled per step event. +// block->rate_delta = ceil(block->acceleration* +// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) +// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) // 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, sizeof(target)); // pl.position[] = target[] - pl.last_x = x; - pl.last_y = y; - pl.last_z = z; - - // Update buffer head and next buffer head indices - block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); } + // Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_set_current_position(int32_t x, int32_t y, int32_t z) +void plan_sync_position() { - pl.position[X_AXIS] = x; - pl.position[Y_AXIS] = y; - pl.position[Z_AXIS] = z; - pl.last_x = x/settings.steps_per_mm[X_AXIS]; - pl.last_y = y/settings.steps_per_mm[Y_AXIS]; - pl.last_z = z/settings.steps_per_mm[Z_AXIS]; + uint8_t idx; + for (idx=0; idx + + +--------+ <- nominal_rate /|\ + / \ / | \ + initial_rate -> + \ / | + <- next->initial_rate + | + <- next->initial_rate / | | + +-------------+ initial_rate -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be + described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block + reaches the nominal speed of the block and cruises for a period of time. A triangle occurs + when the nominal speed is not reached within the block. Some other special cases exist, + such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that + has no deceleration period when the next block resumes acceleration. + + The following function determines the type of velocity profile and stores the minimum required + information for the stepper algorithm to execute the calculated profiles. In this case, only + the new initial rate and n_steps until deceleration are computed, since the stepper algorithm + already handles acceleration and cruising and just needs to know when to start decelerating. +*/ +int32_t calculate_trapezoid_for_block(uint8_t block_index) +{ + plan_block_t *current_block = &block_buffer[block_index]; + + // Determine current block exit speed + float exit_speed_sqr; + uint8_t next_index = next_block_index(block_index); + plan_block_t *next_block = plan_get_block_by_index(next_index); + if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed. + else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block + + // 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) ) + float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); + + // 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 ) { + float decelerate_distance; + // 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. + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; } + + return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters)); + } + return(0); +} + + // 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(int32_t step_events_remaining) { - block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block + plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block // Only remaining millimeters and step_event_count need to be updated for planner recalculate. // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to @@ -540,9 +479,9 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; block->step_event_count = step_events_remaining; - // Re-plan from a complete stop. Reset planner entry speeds and flags. + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = 0.0; - block->recalculate_flag = true; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index 830735d..e371bb0 100644 --- a/planner.h +++ b/planner.h @@ -2,8 +2,8 @@ planner.h - buffers movement commands and manages the acceleration profile plan Part of Grbl + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 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 @@ -21,10 +21,11 @@ #ifndef planner_h #define planner_h - +#include "nuts_bolts.h" + // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE - #define BLOCK_BUFFER_SIZE 17 + #define BLOCK_BUFFER_SIZE 18 #endif // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in @@ -32,43 +33,43 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - uint32_t steps_x, steps_y, steps_z; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + int32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block // Fields used by the motion planner to manage acceleration - float nominal_speed_sqr; // The nominal speed for this block in mm/min - float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min - float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min - float millimeters; // The total travel of this block in mm - float acceleration; - uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction - + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + float millimeters; // The total travel for this block to be executed in mm + // Settings for the trapezoid generator - uint32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t decelerate_after; // The index of the step event on which to start decelerating - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t d_next; // Scaled distance to next step -} block_t; +// int32_t decelerate_after; // The index of the step event on which to start decelerating + +} plan_block_t; // Initialize the motion plan subsystem void plan_init(); -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed +// 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. -void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate); +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate); // Called when the current block is no longer needed. Discards the block and makes the memory // availible for new blocks. void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty -block_t *plan_get_current_block(); +plan_block_t *plan_get_current_block(); + +plan_block_t *plan_get_block_by_index(uint8_t block_index); + +int32_t calculate_trapezoid_for_block(uint8_t block_index); // Reset the planner position vector (in steps) -void plan_set_current_position(int32_t x, int32_t y, int32_t z); +void plan_sync_position(); // Reinitialize plan with a partially completed block void plan_cycle_reinitialize(int32_t step_events_remaining); diff --git a/planner_old.c b/planner_old.c new file mode 100644 index 0000000..1cf6fb4 --- /dev/null +++ b/planner_old.c @@ -0,0 +1,476 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + 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 . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +#include +#include +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "protocol.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 block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static volatile 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 + float last_target[N_AXIS]; // Target position of previous path line segment +} planner_t; +static planner_t pl; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t 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 prev_block_index(uint8_t block_index) +{ + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + + +/* STEPPER VELOCITY PROFILE DEFINITION + less than nominal rate-> + + +--------+ <- nominal_rate /|\ + / \ / | \ + initial_rate -> + \ / | + <- next->initial_rate + | + <- next->initial_rate / | | + +-------------+ initial_rate -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be + described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block + reaches the nominal speed of the block and cruises for a period of time. A triangle occurs + when the nominal speed is not reached within the block. Some other special cases exist, + such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that + has no deceleration period when the next block resumes acceleration. + + The following function determines the type of velocity profile and stores the minimum required + information for the stepper algorithm to execute the calculated profiles. In this case, only + the new initial rate and n_steps until deceleration are computed, since the stepper algorithm + already handles acceleration and cruising and just needs to know when to start decelerating. +*/ +static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) +{ + // Compute new initial rate for stepper algorithm + block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Compute new nominal rate if a feedrate override occurs. + // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // 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*block->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_sqr-exit_speed_sqr)) ); + + // 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 (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. + // 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_sqr - exit_speed_sqr)); + + // 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; } + + // Finally, check if this is a pure deceleration block. + if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } + } +} + + +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed + +-------------+ + time --> + + Recalculates the motion plan according to the following algorithm: + + 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) + so that: + a. The junction speed is equal to or less than the maximum junction speed limit + b. No speed reduction within one block requires faster deceleration than the acceleration limits. + c. The last (or newest appended) block is planned from a complete stop. + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The speed increase within one block would require faster acceleration than the acceleration limits. + + When these stages are complete, all blocks have a junction entry speed that will allow all speed changes + to be performed using the overall limiting acceleration value, and where no junction speed is greater + than the max limit. In other words, it just computed the fastest possible velocity profile through all + buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully + executed. Finally it will: + + 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a + a planner-modified junction speed with be updated, the others are assumed ok as is. + + All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only + when planned values are converted to stepper rate parameters(3), these are integers. If another motion block + is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. + + Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's + constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and + nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile + through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting + conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. + + Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line + segments, like arcs, 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 stop at the end of the + buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be + able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost + steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the + bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory + is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering + up to a hundred or more. + + NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient + as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments + can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading + to weird hiccup-like jerky motions. +*/ +static void planner_recalculate() +{ + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer + + // Determine safe point for which to plan to. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + + block_buffer_planned = block_buffer_safe; + calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); + calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + 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; + } + } + block_index = prev_block_index(block_index); + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_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) { + block_buffer_planned = block_index; + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; + } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. + calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + block_index = next_block_index( block_index ); + } + + } + +} + + +void plan_init() +{ + block_buffer_tail = block_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); + block_buffer_planned = block_buffer_head; + memset(&pl, 0, sizeof(pl)); // Clear planner struct +} + + +inline void plan_discard_current_block() +{ + if (block_buffer_head != block_buffer_tail) { + block_buffer_tail = next_block_index( block_buffer_tail ); + } +} + + +inline block_t *plan_get_current_block() +{ + if (block_buffer_head == block_buffer_tail) { return(NULL); } + return(&block_buffer[block_buffer_tail]); +} + + +// 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); +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void plan_synchronize() +{ + while (plan_get_current_block() || sys.state == STATE_CYCLE) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +// 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). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +{ + // Prepare and initialize new block + 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 + + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[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. + delta_mm = target[idx] - pl.last_target[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Incrementally compute total move distance by Euclidean norm + block->millimeters += delta_mm*delta_mm; + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation + + // Bail if this is a zero-length block + 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; } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited + // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so + // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction + // and axes properties as well. + // 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; idxacceleration = 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]; + } + } + + /* 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. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // 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. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } + + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 + block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute and store acceleration and distance traveled per step event. + block->rate_delta = ceil(block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) + + // 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[] + memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[] + + planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position() +{ + uint8_t idx; + for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + block->entry_speed_sqr = 0.0; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} diff --git a/planner_old.h b/planner_old.h new file mode 100644 index 0000000..84ecc4b --- /dev/null +++ b/planner_old.h @@ -0,0 +1,83 @@ +/* + planner.h - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef planner_h +#define planner_h +#include "nuts_bolts.h" + +// The number of linear motions that can be in the plan at any give time +#ifndef BLOCK_BUFFER_SIZE + #define BLOCK_BUFFER_SIZE 17 +#endif + +// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in +// the source g-code and may never actually be reached if acceleration management is active. +typedef struct { + + // Fields used by the bresenham algorithm for tracing the line + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + uint32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block + + // Fields used by the motion planner to manage acceleration + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float millimeters; // The total travel of this block in mm + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + + // Settings for the trapezoid generator + uint32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t decelerate_after; // The index of the step event on which to start decelerating + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t d_next; // Scaled distance to next step + +} block_t; + +// Initialize the motion plan subsystem +void plan_init(); + +// 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. +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate); + +// Called when the current block is no longer needed. Discards the block and makes the memory +// availible for new blocks. +void plan_discard_current_block(); + +// Gets the current block. Returns NULL if buffer empty +block_t *plan_get_current_block(); + +// Reset the planner position vector (in steps) +void plan_sync_position(); + +// Reinitialize plan with a partially completed block +void plan_cycle_reinitialize(int32_t step_events_remaining); + +// Returns the status of the block ring buffer. True, if buffer is full. +uint8_t plan_check_full_buffer(); + +// Block until all buffered steps are executed +void plan_synchronize(); + +#endif diff --git a/print.c b/print.c index 2f820d5..7a7686b 100644 --- a/print.c +++ b/print.c @@ -77,7 +77,7 @@ void print_uint8_base2(uint8_t n) serial_write('0' + buf[i - 1]); } -static void print_uint32_base10(unsigned long n) +void print_uint32_base10(unsigned long n) { unsigned char buf[10]; uint8_t i = 0; diff --git a/print.h b/print.h index 9983aee..8a161c1 100644 --- a/print.h +++ b/print.h @@ -31,6 +31,8 @@ void printPgmString(const char *s); void printInteger(long n); +void print_uint32_base10(uint32_t n); + void print_uint8_base2(uint8_t n); void printFloat(float n); diff --git a/protocol.c b/protocol.c index a75a141..253ec1f 100644 --- a/protocol.c +++ b/protocol.c @@ -104,6 +104,7 @@ ISR(PINOUT_INT_vect) // limit switches, or the main program. void protocol_execute_runtime() { + st_prep_buffer(); if (sys.execute) { // Enter only if any bit flag is true uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times diff --git a/report.c b/report.c index 8c90a17..e71ac54 100644 --- a/report.c +++ b/report.c @@ -157,9 +157,9 @@ void report_grbl_settings() { printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability - printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(settings.max_travel[X_AXIS]); - printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(settings.max_travel[Y_AXIS]); - printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(settings.max_travel[Z_AXIS]); + printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative. printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); diff --git a/settings.c b/settings.c index 9225a91..8f628a5 100644 --- a/settings.c +++ b/settings.c @@ -169,9 +169,9 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. - case 9: settings.max_travel[X_AXIS] = value; break; - case 10: settings.max_travel[Y_AXIS] = value; break; - case 11: settings.max_travel[Z_AXIS] = value; break; + case 9: settings.max_travel[X_AXIS] = -value; break; // Store as negative for grbl internal use. + case 10: settings.max_travel[Y_AXIS] = -value; break; // Store as negative for grbl internal use. + case 11: settings.max_travel[Z_AXIS] = -value; break; // Store as negative for grbl internal use. case 12: if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } settings.pulse_microseconds = round(value); break; @@ -206,7 +206,10 @@ uint8_t settings_store_global_setting(int parameter, float value) { break; case 24: if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; } - else { settings.flags &= ~BITFLAG_HOMING_ENABLE; } + else { + settings.flags &= ~BITFLAG_HOMING_ENABLE; + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; + } break; case 25: settings.homing_dir_mask = trunc(value); break; case 26: settings.homing_feed_rate = value; break; diff --git a/stepper.c b/stepper.c index acb3282..05ddd9e 100644 --- a/stepper.c +++ b/stepper.c @@ -19,20 +19,30 @@ along with Grbl. If not, see . */ -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - #include #include "stepper.h" #include "config.h" #include "settings.h" #include "planner.h" +#include "nuts_bolts.h" // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +#define ST_NOOP 0 +#define ST_END_OF_BLOCK 1 +#define ST_DECEL 2 +#define ST_DECEL_EOB 3 + +#define SEGMENT_BUFFER_SIZE 10 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -40,44 +50,92 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint32_t event_count; // Total event count. Retained for feed holds. - uint32_t step_events_remaining; // Steps remaining in motion + uint8_t segment_steps_remaining; // Steps remaining in line motion - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick + uint32_t d_per_tick; + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; } stepper_t; static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output +// Stores stepper buffer common data. Can change planner mid-block in special conditions. +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + int32_t decelerate_after; + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +// Primary stepper motion buffer +typedef struct { + uint8_t n_step; + uint8_t st_data_index; + uint8_t flag; +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +static volatile uint8_t segment_buffer_tail; +static uint8_t segment_buffer_head; +static uint8_t segment_next_head; +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer +static uint8_t pl_prep_index; +static st_data_t *st_prep_data; +static uint8_t st_data_prep_index; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + +static uint8_t next_block_pl_index(uint8_t block_index) +{ + block_index++; + if (block_index == 18) { block_index = 0; } + return(block_index); +} + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -91,14 +149,17 @@ void st_wake_up() } if (sys.state == STATE_CYCLE) { // Initialize stepper output bits - out_bits = settings.invert_mask; + st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. - step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Enable stepper driver interrupt st.execute_step = false; + st.load_flag = LOAD_BLOCK; + TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. + // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is + // a small chance that this will load at the same time as a step event. Hopefully, + // the overhead for this loading event isn't too much.. possibly 2-5 usec. + + // NOTE: The stepper algorithm must control the planner buffer tail as it completes + // the block moves. Otherwise, a feed hold can leave a few step buffer line moves + // without the correct planner block information. - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; + st_current_segment = &segment_buffer[segment_buffer_tail]; - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + // Load number of steps to execute from stepper buffer + st.segment_steps_remaining = st_current_segment->n_step; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + + // During feed hold, do not update rate or ramp type. Keep decelerating. +// if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; +// if (st.delta_d == st_current_data->nominal_rate) { +// st.ramp_type = RAMP_NOOP_CRUISE; + st.ramp_type = RAMP_ACCEL; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid +// } +// } + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + + } + + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { + if (st.ramp_type == RAMP_NOOP_CRUISE) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + st.ramp_type = RAMP_DECEL; } + st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + } else { + // Can't discard planner block here if a feed hold stops in middle of block. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } + } // Adjust inverse time counter for ac/de-celerations if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; + } else { // Adjust velocity for deceleration + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; } else { + + // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // Check for and handle feed hold exit? At this point, machine is stopped. + } } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } } } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.d_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; + st.counter_x -= pl_current_block->steps[X_AXIS]; if (st.counter_x < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_y; + st.counter_y -= pl_current_block->steps[Y_AXIS]; if (st.counter_y < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_z; + st.counter_z -= pl_current_block->steps[Z_AXIS]; if (st.counter_z < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } + st.segment_steps_remaining--; // Decrement step events count + if (st.segment_steps_remaining == 0) { + + // Line move is complete, set load line flag to check for new move. + // Check if last line move in planner block. Discard if so. + if (st_current_segment->flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_LINE; } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); + + // Discard current segment + segment_buffer_tail = next_block_index( segment_buffer_tail ); + + // NOTE: sys.position updates could be done here. The bresenham counters can have + // their own fast 8-bit addition-only counters. Here we would check the direction and + // apply it to sys.position accordingly. However, this could take too much time. + } - out_bits ^= settings.invert_mask; // Apply step port invert mask + st.out_bits ^= settings.invert_mask; // Apply step port invert mask } busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.delta_d = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } sys.state = STATE_IDLE; - } + } + + +/* Preps stepper buffer. Called from main program. + + NOTE: There doesn't seem to be a great way to figure out how many steps occur within + a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a + critical problem. So, either numerical round-off checks could be made to account for + them, while CPU overhead could be minimized in some way, or we can flip the algorithm + around to have the stepper algorithm track number of steps over an indeterminant amount + of time instead. + In other words, we use the planner velocity floating point data to get an estimate of + the number of steps we want to execute. We then back out the approximate velocity for + the planner to use, which should be much more robust to round-off error. The main problem + now is that we are loading the stepper algorithm to handle acceleration now, rather than + pre-calculating with the main program. This approach does make sense in the way that + planner velocities and stepper profiles can be traced more accurately. + Which is better? Very hard to tell. The time-based algorithm would be able to handle + Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would + require some additional math in the stepper algorithm to adjust on the fly, plus adaptation + would occur in a non-deterministic manner. + I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + + TODO: Need to describe the importance of continuations of step pulses between ramp states + and planner blocks. This has to do with Alden's problem with step "phase". The things I've + been doing here limit this phase issue by truncating some of the ramp timing for certain + events like deceleration initialization and end of block. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); + if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + + // Prepare commonly shared planner block data for the ensuing step buffer moves + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + // Calculate trapezoid data from planner. + st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index); + + } + + + /* + TODO: Need to check for a planner flag to indicate a change to this planner block. + If so, need to check for a change in acceleration state, from deceleration to acceleration, + to reset the stepper ramp counters and the initial_rate data to trace the new + ac/de-celeration profile correctly. + No change conditions: + - From nominal speed to acceleration from feedrate override + - From nominal speed to new deceleration. + - From acceleration to new deceleration point later or cruising point. + - From acceleration to immediate deceleration? Can happen during feedrate override + and slowing down, but likely ok by enforcing the normal ramp counter protocol. + Change conditions: + - From deceleration to acceleration, i.e. common with jogging when new blocks are added. + */ + + st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head]; + st_prep_segment->st_data_index = st_data_prep_index; + + // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. + st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step ); +// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (st_prep_segment->n_step > st_prep_data->step_events_remaining) { + st_prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step exceeds decelerate point in block. Need to perform this so that the + // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should + // be OK since it is likely moving at a fast rate already. + if (st_prep_data->decelerate_after > 0) { + if (st_prep_segment->n_step > st_prep_data->decelerate_after) { + st_prep_segment->n_step = st_prep_data->decelerate_after; + } + } + +// float distance, exit_speed_sqr; +// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero +// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { +// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; +// } else { // Acceleration or cruising ramp +// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { +// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; +// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } +// } else { +// exit_speed_sqr = pl_prep_block->nominal_speed_sqr; +// } +// } + + // Update planner block variables. +// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); +// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. +// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. +// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. + + // Update stepper block variables. + st_prep_data->step_events_remaining -= st_prep_segment->n_step; + if ( st_prep_data->step_events_remaining == 0 ) { + // Move planner pointer to next block + if (st_prep_data->decelerate_after == 0) { + st_prep_segment->flag = ST_DECEL_EOB; + } else { + st_prep_segment->flag = ST_END_OF_BLOCK; + } + pl_prep_index = next_block_pl_index(pl_prep_index); + pl_prep_block = NULL; +printString("EOB"); + } else { + if (st_prep_data->decelerate_after == 0) { + st_prep_segment->flag = ST_DECEL; + } else { + st_prep_segment->flag = ST_NOOP; + } +printString("x"); + } + st_prep_data->decelerate_after -= st_prep_segment->n_step; + + // New step block completed. Increment step buffer indices. + segment_buffer_head = segment_next_head; + segment_next_head = next_block_index(segment_buffer_head); + +printInteger((long)st_prep_segment->n_step); +printString(" "); +printInteger((long)st_prep_data->decelerate_after); +printString(" "); +printInteger((long)st_prep_data->step_events_remaining); + } +} diff --git a/stepper.h b/stepper.h index 0cb4189..74ab717 100644 --- a/stepper.h +++ b/stepper.h @@ -45,4 +45,6 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); +void st_prep_buffer(); + #endif diff --git a/stepper_new2.c b/stepper_new2.c new file mode 100644 index 0000000..c248d4d --- /dev/null +++ b/stepper_new2.c @@ -0,0 +1,601 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t step_events_remaining; // Steps remaining in line motion + + // Used by inverse time algorithm + int32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t d_counter; // Inverse time distance traveled since last step event + int32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; +} stepper_t; +static stepper_t st; + +#define STEPPER_BUFFER_SIZE 5 +typedef struct { + int32_t event_count; + int32_t rate; + uint8_t end_of_block; + uint8_t tick_count; + + int32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + int32_t decelerate_after; // The index of the step event on which to start decelerating + int32_t nominal_rate; // The nominal step rate for this block in step_events/minute + int32_t d_next; // Scaled distance to next step + +} stepper_buffer_t; +static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE]; + +static volatile uint8_t step_buffer_tail; +static uint8_t step_buffer_head; +static uint8_t step_next_head; + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + st.counter_x = (plan_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // This is correct. Sets the total time before the next step occurs. + st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d. + + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + + } + + st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.delta_d; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += plan_current_block->d_next; // Reload inverse time counter + + st.out_bits = plan_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= plan_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<step_event_count; + //st.step_events_remaining = st.event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: The initial rate needs to be sent back to the planner to update the entry speed + block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. + block->rate_delta = ceil(plan_current_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step) + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { } + } + + +// Track instead the trapezoid line and use the average of the entry and exit velocities +// to determine step rate. This should take care of the deceleration issue automatically... +// i think. +// First need to figure out what type of profile is segment is, i.e. acceleration only, accel +// to decel triangle, cruise to decel, or all three. May need more profile data to compute this +// from the planner itself, like accelerate until. +// Another issue. This is only tracking the velocity profile, not the distance covered over that +// time period. This can lead to an unsynchronized velocity profile and steps executed. But, +// how much drift is there really? Enough to be a problem? Not sure. I would think typical +// drift would be on the order of a few steps, more depending on step resolution. +entry_rate = last_exit_rate; +time = 250 ISR ticks per acceleration tick. +distance = 0; +if (distance_traveled < accelerate_until) + exit_rate = entry_rate + acceleration*time; + if (exit_rate > nominal_rate) { + exit_rate = nominal_rate; + time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate); + // distance = accelerate_until; // Enforce distance? + // Truncate this segment. + } +} else if (distance_traveled >= decelerate_after) { + if (accelerate_until == decelerate_after) { + time = last time; + exit_rate = entry_rate; + } else { + exit_rate = entry_rate - acceleration*time; + } +} else { + exit_rate = nominal_rate; // Just cruise + distance = nominal_rate*time; + if (distance > decelerate_after) { // Truncate segment at nominal rate. + time = (decelerate_after-distance_traveled)/(nominal_rate); + distance = decelerate_after; + } +} +mean_rate = 0.5*(entry_rate+exit_rate); +distance = mean_rate*time; + + +if (entry_rate < nominal_rate) { + if (entry_distance < decelerate_after) { // Acceleration case + exit_rate = entry_rate + acceleration*time + exit_rate = min(exit_rate,nominal_rate); +mean_rate = 0.5*(entry_rate + exit_rate); +distance = mean_rate*time; +if (distance > decelerate_after) { + exit_rate = + + + // If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be + // rate adjustements that have less than one step per tick. + // How do you deal with the remainer? +time = 250 ISR ticks per acceleration tick. (30000/120) +delta_d*time // mm per acceleration tick +delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow. +delta_d*time/d_next + last_remainder. // steps/acceleration_tick. +n_step*d_next/delta_d // number of ISR ticks for enforced n_steps. + +// In floating point? Then convert? +// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just +// checking if the total step event counts are performed. +// Could be limited by float conversion and about 1e7 steps per block. +line_mm = feed_rate / acc_tick // mm per acc_tick +n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits +millimeters_remaining -= line_mm; +step_events_remaining -= n_steps; + +// There doesn't seem to be a way to avoid this divide here. +line_mm = feed_rate / acc_tick // mm per acc_tick +n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits +line_remainder = line_mm - n_steps*(millimeters/step_event_count); + +// Need to handle when rate is very very low, i.e. less than one step per accel tick. +// Could be bounded by MINIMUM_STEP_RATE. + +// 1. Figure out how many steps occur exactly within n ISR ticks. +// 2. Account for step-time remainder for next line motion exactly. +// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\ + have the ISR track steps to exit on time. It would require an extra counter. + +// NOTE: There doesn't seem to be a great way to figure out how many steps occur within +// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a +// critical problem. So, either numerical round-off checks could be made to account for +// them, while CPU overhead could be minimized in some way, or we can flip the algorithm +// around to have the stepper algorithm track number of steps over an indeterminant amount +// of time instead. +// In other words, we use the planner velocity floating point data to get an estimate of +// the number of steps we want to execute. We then back out the approximate velocity for +// the planner to use, which should be much more robust to round-off error. The main problem +// now is that we are loading the stepper algorithm to handle acceleration now, rather than +// pre-calculating with the main program. This approach does make sense in the way that +// planner velocities and stepper profiles can be traced more accurately. +// Which is better? Very hard to tell. The time-based algorithm would be able to handle +// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would +// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation +// would occur in a non-deterministic manner. +// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + +feed_rate/120 = millimeters per acceleration tick + + + steps? + d_next // (mult*mm/step) + rate // (mult*mm/isr_tic) + rate/d_next // step/isr_tic + + if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) { + // Determine line segment velocity and associated inverse time counter. + if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + step_block.delta_d += plan_current_block->rate_delta; + if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state. + step_block.ramp_type = CRUISE_RAMP; + step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity + } + } + } else { // Adjust velocity for deceleration + if (step_block.delta_d > plan_current_block->rate_delta) { + step_block.delta_d -= plan_current_block->rate_delta; + } else { + step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + + // Incorrect. Can't overwrite delta_d. Needs to override instead. + if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; } + + /* - Compute the number of steps needed to complete this move over the move time, i.e. + ISR_TICKS_PER_ACCELERATION_TICK. + - The first block in the buffer is half of the move time due to midpoint rule. + - Check if this reaches the deceleration after location. If so, truncate move. Also, + if this is a triangle move, double the truncated move to stay with midpoint rule. + NOTE: This can create a stepper buffer move down to just one step in length. + - Update the planner block entry speed for the planner to compute from end of the + stepper buffer location. + - If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules. + When the deceleration is complete, all we need to do is update the planner block + entry speed and force a replan. + */ + + // Planner block move completed. + // TODO: planner buffer tail no longer needs to be volatile. only accessed by main program. + if (st.step_events_remaining == 0) { + plan_current_block = NULL; // Set flag that we are done with this planner block. + plan_discard_current_block(); + } + + + + + + step_buffer_head = step_next_head; + step_next_head = next_block_index(step_buffer_head); + + } +} diff --git a/stepper_new.c b/stepper_new_dual_ISR.c similarity index 96% rename from stepper_new.c rename to stepper_new_dual_ISR.c index ae6bd8b..87f87bc 100644 --- a/stepper_new.c +++ b/stepper_new_dual_ISR.c @@ -34,9 +34,7 @@ // Stepper state variable. Contains running data and trapezoid variables. typedef struct { // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; + int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer uint32_t event_count; // Total event count. Retained for feed holds. uint32_t step_events_remaining; // Steps remaining in motion @@ -182,26 +180,29 @@ won't take too much time in the interrupt. // Prepare Bresenham step event, when it's time to do so. if (st.d_counter < 0) { st.d_counter += current_block->d_next; - - // Load next step - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; + + // Configure next step + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; - if (st.counter_x < 0) { + st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up. + if (st.counter[X_AXIS] < 0) { out_bits |= (1<steps_y; - if (st.counter_y < 0) { + st.counter[Y_AXIS] -= current_block->steps_y; + if (st.counter[Y_AXIS] < 0) { out_bits |= (1<steps_z; - if (st.counter_z < 0) { + st.counter[Z_AXIS] -= current_block->steps_z; + if (st.counter[Z_AXIS] < 0) { out_bits |= (1<. +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +#define ST_NOOP 0 +#define ST_END_OF_BLOCK 1 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int8_t segment_steps_remaining; // Steps remaining in line motion + + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + int32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; +} stepper_t; +static stepper_t st; + +#define SEGMENT_BUFFER_SIZE 5 +// Stores stepper buffer common data. Can change planner mid-block in special conditions. +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + int32_t d_next; // Scaled distance to next step + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; + +// Primary stepper motion buffer +typedef struct { + uint8_t n_step; + int32_t rate; + uint8_t st_data_index; + uint8_t flag; +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +static volatile uint8_t segment_buffer_tail; +static uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer +static uint8_t pl_prep_index; +static st_data_t *st_prep_data; +static uint8_t st_data_prep_index; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + st.delta_d = st_current_segment->rate; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[st_current_segment->st_data_index]; + + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + } + st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.delta_d; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<flag == ST_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_LINE; + } + + // Discard current block + if (segment_buffer_head != segment_buffer_tail) { + segment_buffer_tail = next_block_index( segment_buffer_tail ); + } + + // NOTE: sys.position updates could be done here. The bresenham counters can have + // their own fast 8-bit addition-only counters. Here we would check the direction and + // apply it to sys.position accordingly. However, this could take too much time. + + } + + st.out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); + st.ramp_type = RAMP_ACCEL; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + st.delta_d = 0; + sys.state = STATE_QUEUED; + } else { + sys.state = STATE_IDLE; + } +} + + +/* Preps stepper buffer. Called from main program. + + NOTE: There doesn't seem to be a great way to figure out how many steps occur within + a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a + critical problem. So, either numerical round-off checks could be made to account for + them, while CPU overhead could be minimized in some way, or we can flip the algorithm + around to have the stepper algorithm track number of steps over an indeterminant amount + of time instead. + In other words, we use the planner velocity floating point data to get an estimate of + the number of steps we want to execute. We then back out the approximate velocity for + the planner to use, which should be much more robust to round-off error. The main problem + now is that we are loading the stepper algorithm to handle acceleration now, rather than + pre-calculating with the main program. This approach does make sense in the way that + planner velocities and stepper profiles can be traced more accurately. + Which is better? Very hard to tell. The time-based algorithm would be able to handle + Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would + require some additional math in the stepper algorithm to adjust on the fly, plus adaptation + would occur in a non-deterministic manner. + I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + + TODO: Need to describe the importance of continuations of step pulses between ramp states + and planner blocks. This has to do with Alden's problem with step "phase". The things I've + been doing here limit this phase issue by truncating some of the ramp timing for certain + events like deceleration initialization and end of block. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); + if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + + // Prepare commonly shared planner block data for the ensuing step buffer moves + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. +// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) +// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. +// st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* +// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + } + + + /* + // Check if planner has changed block exit parameters. If so, then we have to update the + // velocity profile for the remainder of this block. Otherwise, the block hasn't changed. + if (exit_speed_sqr != last_exit_speed_sqr) { + intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) ); + if (intersect_distance <= 0) { // Deceleration only. + final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance; + block->decelerate_after = 0; + } else { + decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration); + if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } + if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; } + } + } + + + + + + n_step = 100; // Estimate distance we're going to travel in this segment + if (n_step > step_events_remaining) { n_step = step_events_remaining; }; + + segment_distance = n_step*mm_per_step; // True distance traveled + + // ISSUE: Either weighted average speed or truncated segments must be used. Otherwise + // accelerations, in particular high value, will not perform correctly. + if (distance_traveled < decelerate_after) { + if (segment_distance + distance_traveled > decelerate_after) { + n_step = ceil((decelerate_after-distance_traveled)/mm_per_step); + segment_distance = n_step*mm_per_step; + } + } + + + + + // based on time? + time = incremental time; + v_exit = v_entry + acceleration*time; + if (v_exit > v_nominal) { + time_accel = (v_nominal-v_entry)/acceleration; + distance = v_entry*time_accel + 0.5*acceleration*time_accel**2; + time_remaining = time - time_accel; + } + distance = v_entry*time + 0.5*acceleration*time*time; + + + t_accel = (v_nominal - v_entry) / acceleration; + t_decel = (v_exit - v_nominal) / -acceleration; + dt = (v_entry-v_exit)/acceleration; + if + + + + + // What's the speed of this segment? Computing per segment can get expensive, but this + // would allow S-curves to be installed pretty easily here. + if (initial_speed < nominal_speed) { + if (initial_distance < decelerate_after) { // Acceleration + exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance); + if (exit_speed > nominal_speed) { exit_speed = nominal_speed; } + } else { // Deceleration + exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance); + } + average_speed = 0.5*(initial_speed + exit_speed); + } else { // Cruise + average_speed = nominal_speed; + } + segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; } + + + + + */ + + + + /* + TODO: Need to check for a planner flag to indicate a change to this planner block. + If so, need to check for a change in acceleration state, from deceleration to acceleration, + to reset the stepper ramp counters and the initial_rate data to trace the new + ac/de-celeration profile correctly. + No change conditions: + - From nominal speed to acceleration from feedrate override + - From nominal speed to new deceleration. + - From acceleration to new deceleration point later or cruising point. + - From acceleration to immediate deceleration? Can happen during feedrate override + and slowing down, but likely ok by enforcing the normal ramp counter protocol. + Change conditions: + - From deceleration to acceleration, i.e. common with jogging when new blocks are added. + */ + + st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head]; + st_prep_block->st_data_index = st_data_prep_index; + + // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. + st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step ); +// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (st_prep_block->n_step > st_prep_data->step_events_remaining) { + st_prep_block->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step exceeds decelerate point in block. Need to perform this so that the + // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should + // be OK since it is likely moving at a fast rate already. + if (st_prep_block->n_step > pl_prep_block->decelerate_after) { + st_prep_block->n_step = pl_prep_block->decelerate_after; + } + + float distance, exit_speed_sqr; + distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero + if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { + exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; + // Set ISR tick reset flag for deceleration ramp. + } else { // Acceleration or cruising ramp + if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { + exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; + if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } + } else { + exit_speed_sqr = pl_prep_block->nominal_speed_sqr; + } + } + + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore + } + } else { // Adjust velocity for deceleration + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; + } else { + + // Moving near zero feed rate. Gracefully slow down. + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // Check for and handle feed hold exit? At this point, machine is stopped. + + } + } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + } + } + // During feed hold, do not update rate or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; + if (st.delta_d == st_current_data->nominal_rate) { + ramp_type = RAMP_NOOP_CRUISE; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st_current_segment->flag == ST_NOOP) { + if (st.ramp_type == RAMP_NOOP_CRUISE) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + st.ramp_type = RAMP_DECEL; + } + + + + + + // Update planner block variables. + pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); +// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. + pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. + pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. + + // Update stepper block variables. + st_prep_data->step_events_remaining -= st_prep_block->n_step; + if ( st_prep_data->step_events_remaining == 0 ) { + // Move planner pointer to next block + st_prep_block->flag = ST_END_OF_BLOCK; + pl_prep_index = next_block_index(pl_prep_index); + pl_prep_block = NULL; + } else { + st_prep_block->flag = ST_NOOP; + } + + // New step block completed. Increment step buffer indices. + segment_buffer_head = segment_next_head; + segment_next_head = next_block_index(segment_buffer_head); + + } +} diff --git a/stepper_old.c b/stepper_old.c new file mode 100644 index 0000000..3191007 --- /dev/null +++ b/stepper_old.c @@ -0,0 +1,387 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t event_count; // Total event count. Retained for feed holds. + int32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} stepper_t; +static stepper_t st; +static block_t *current_block; // A pointer to the block currently being traced + +// Used by the stepper driver interrupt +static uint8_t step_pulse_time; // Step pulse reset time after step rise +static uint8_t out_bits; // The next stepping-bits to be output + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration + if (st.delta_d > current_block->rate_delta) { + st.delta_d -= current_block->rate_delta; + } else { + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + } + } + + // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } + else { st.d_counter -= st.delta_d; } + + // Execute Bresenham step event, when it's time to do so. + if (st.d_counter < 0) { + st.d_counter += current_block->d_next; + + // Check for feed hold state and execute accordingly. + if (sys.state == STATE_HOLD) { + if (st.ramp_type != DECEL_RAMP) { + st.ramp_type = DECEL_RAMP; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). + + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + out_bits |= (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + out_bits |= (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + out_bits |= (1<decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } else { + // If current block is finished, reset pointer + current_block = NULL; + plan_discard_current_block(); + } + + out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<