diff --git a/gcode.c b/gcode.c index 62fec77..511bc59 100644 --- a/gcode.c +++ b/gcode.c @@ -3,7 +3,8 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Modifications Copyright (c) 2011 Sungeun (Sonny) 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 the Free Software Foundation, either version 3 of the License, or @@ -97,25 +98,6 @@ static float to_millimeters(double value) { return(gc.inches_mode ? (value * MM_PER_INCH) : value); } -#ifdef __AVR_ATmega328P__ -// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, -// positive to the right. -static double theta(double x, double y) -{ - double theta = atan(x/fabs(y)); - if (y>0) { - return(theta); - } else { - if (theta>0) - { - return(M_PI-theta); - } else { - return(-M_PI-theta); - } - } -} -#endif - // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase // characters and signed floating point values (no whitespace). Comments and block delete // characters have been removed. @@ -125,7 +107,7 @@ uint8_t gc_execute_line(char *line) { double value; double unit_converted_value; double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified - int radius_mode = false; + uint8_t radius_mode = false; uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */ uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */ @@ -331,50 +313,29 @@ uint8_t gc_execute_line(char *line) { // 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; } + 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] = (x-(y*h_x2_div_d))/2; - offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2; - } - - /* - This segment sets up an clockwise or counterclockwise arc from the current position to the target position around - the center designated by the offset vector. All theta-values measured in radians of deviance from the positive - y-axis. + 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)); - | <- theta == 0 - * * * - * * - * * - * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) - * / - C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) + } else { // Offset mode specific computations + + r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc - */ - - // calculate the theta (angle) of the current point - double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]); - // calculate the theta (angle) of the target point - double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0], - target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]); - // ensure that the difference is positive so that we have clockwise travel - if (theta_end < theta_start) { theta_end += 2*M_PI; } - double angular_travel = theta_end-theta_start; - // Invert angular motion if the g-code wanted a counterclockwise arc - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { - angular_travel = angular_travel-2*M_PI; } - // Find the radius - double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); - // Calculate the motion along the depth axis of the helix - double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2]; + + // Set clockwise/counter-clockwise sign for mc_arc computations + int8_t clockwise_sign = 1; + if (gc.motion_mode == MOTION_MODE_CW_ARC) { clockwise_sign = -1; } + // Trace the arc - mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, + mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, - gc.position); - // Finish off with a line to make sure we arrive exactly where we think we are - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], - (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); + r, clockwise_sign); + break; #endif } diff --git a/motion_control.c b/motion_control.c index ef4a6b3..bf2ea44 100644 --- a/motion_control.c +++ b/motion_control.c @@ -3,7 +3,8 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Modifications Copyright (c) 2011 Sungeun (Sonny) 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 the Free Software Foundation, either version 3 of the License, or @@ -28,6 +29,8 @@ #include "stepper.h" #include "planner.h" +#define N_ARC_CORRECTION 25 // (0-255) Number of iterations before arc trajectory correction + void mc_dwell(uint32_t milliseconds) { @@ -35,51 +38,117 @@ void mc_dwell(uint32_t milliseconds) _delay_ms(milliseconds); } -// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, -// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the -// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining -// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. -// position is a pointer to a vector representing the current position in millimeters. +// Execute an arc in offset mode format. position == current xyz, target == target xyz, +// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is +// the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used +// for vector transformation direction. +// position, target, and offset are pointers to vectors from gcode.c #ifdef __AVR_ATmega328P__ // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. -void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, - int axis_linear, double feed_rate, int invert_feed_rate, double *position) +void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign) { - int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); - plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc - double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel)); +// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); +// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc + + double center_axis0 = position[axis_0] + offset[axis_0]; + double center_axis1 = position[axis_1] + offset[axis_1]; + double linear_travel = target[axis_linear] - position[axis_linear]; + double r_axis0 = -offset[axis_0]; // Radius vector from center to current location + double r_axis1 = -offset[axis_1]; + double rt_axis0 = target[axis_0] - center_axis0; + double rt_axis1 = target[axis_1] - center_axis1; + + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); + if (angular_travel < 0) { angular_travel += 2*M_PI; } + if (clockwise_sign < 0) { angular_travel = 2*M_PI-angular_travel; } + + double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); if (millimeters_of_travel == 0.0) { return; } - uint16_t segments = round(millimeters_of_travel/settings.mm_per_arc_segment); + uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment); // Multiply inverse feed_rate to compensate for the fact that this movement is approximated // by a number of discrete segments. The inverse feed_rate should be correct for the sum of // all segments. if (invert_feed_rate) { feed_rate *= segments; } - // The angular motion for each segment + double theta_per_segment = angular_travel/segments; - // The linear motion for each segment double linear_per_segment = linear_travel/segments; - // Compute the center of this circle - double center_x = position[axis_1]-sin(theta)*radius; - double center_y = position[axis_2]-cos(theta)*radius; - // a vector to track the end point of each segment - double target[3]; - int i; + + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Based on the solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; + + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. This requires only two cos() and sin() computations to form the rotation + matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since + all double numbers are single precision on the Arduino. (True double precision will not have + round off issues for CNC applications.) Single precision error can accumulate to be greater than + tool precision in some cases. Therefore, arc path correction is implemented. + + Small angle approximation may be used to reduce computation overhead further. This approximation + holds for everything, but very small circles and large mm_per_arc_segment values. In other words, + theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large + to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for + numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an + issue for CNC machines with the single precision Arduino calculations. + + This approximation also allows mc_arc to immediately insert a line segment into the planner + without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied + a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation + double sin_T = clockwise_sign*theta_per_segment; + + double trajectory[3]; + double sin_Ti; + double cos_Ti; + double r_axisi; + uint16_t i; + int8_t count = 0; + // Initialize the linear axis - target[axis_linear] = position[axis_linear]; - for (i=0; imax_entry_speed; // Re-write to ensure at max possible speed - double exit_speed; - if (next) { - exit_speed = next->entry_speed; - } else { - exit_speed = 0.0; // Assume last block has zero exit velocity + double entry_speed_sqr = current->max_entry_speed_sqr; // Reset and check to ensure max possible speed + + // If nominal length true, nominal speed is guaranteed to be reached. No need to re-compute. + // But, if forward planner changed entry speed, reset to max entry speed just to be sure. + if (!current->nominal_length_flag) { + if (next) { + // If the required deceleration across the block is too rapid, reduce entry_speed_sqr accordingly. + if (entry_speed_sqr > next->entry_speed_sqr) { + entry_speed_sqr = min( entry_speed_sqr, + max_allowable_speed_sqr(-settings.acceleration,next->entry_speed_sqr,current->millimeters)); + } + } else { + // Assume last block has zero exit velocity. + entry_speed_sqr = min( entry_speed_sqr, + max_allowable_speed_sqr(-settings.acceleration,0.0,current->millimeters)); + } + } + + // Check for junction speed change + if (current->entry_speed_sqr != entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + current->recalculate_flag = true; // Note: Newest block already set to true } - // If the required deceleration across the block is too rapid, reduce the entry_speed accordingly. - if (entry_speed > exit_speed) { - entry_speed = - min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); - } - current->entry_speed = entry_speed; } @@ -136,13 +147,26 @@ static void planner_reverse_pass() { // The kernel called by planner_recalculate() when scanning the plan from first to last entry. static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } - // If the previous block is an acceleration block, but it is not long enough to - // complete the full speed change within the block, we need to adjust the entry - // speed accordingly. + if(previous) { - if (previous->entry_speed < current->entry_speed) { - current->entry_speed = min( min( current->entry_speed, current->max_entry_speed ), - max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) ); + + // If nominal length true, nominal speed is guaranteed to be reached. No need to recalculate. + if (!previous->nominal_length_flag) { + // If the previous block is an acceleration block, but it is not long enough to + // complete the full speed change within the block, we need to adjust the entry + // speed accordingly. + if (previous->entry_speed_sqr < current->entry_speed_sqr) { + double entry_speed_sqr = min( current->entry_speed_sqr, current->max_entry_speed_sqr ); + entry_speed_sqr = min( entry_speed_sqr, + max_allowable_speed_sqr(-settings.acceleration,previous->entry_speed_sqr,previous->millimeters) ); + + // Check for junction speed change + if (current->entry_speed_sqr != entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + current->recalculate_flag = true; + } + } + } } } @@ -165,7 +189,7 @@ static void planner_forward_pass() { } -/* +/* STEPPER RATE DEFINITION +--------+ <- nominal_rate / \ nominal_rate*entry_factor -> + \ @@ -175,6 +199,7 @@ static void planner_forward_pass() { */ // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // The factors represent a factor of braking and must be in the range 0.0-1.0. +// This converts the planner parameters to the data required by the stepper controller. static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); @@ -201,10 +226,19 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d block->decelerate_after = accelerate_steps+plateau_steps; } - -// Recalculates the trapezoid speed profiles for all blocks in the plan according to the -// entry_speed for each junction. Must be called by planner_recalculate() after -// updating the blocks. +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed + +-------------+ + time --> +*/ +// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the +// entry_speed for each junction and the entry_speed of the next junction. Must be called by +// planner_recalculate() after updating the blocks. Any recalulate flagged junction will +// compute the two adjacent trapezoids to the junction, since the junction speed corresponds +// to exit speed and entry speed of one another. static void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; @@ -214,21 +248,28 @@ static void planner_recalculate_trapezoids() { current = next; next = &block_buffer[block_index]; if (current) { - // Compute entry and exit factors for trapezoid calculations - double entry_factor = current->entry_speed/current->nominal_speed; - double exit_factor = next->entry_speed/current->nominal_speed; - calculate_trapezoid_for_block(current, entry_factor, exit_factor); + // Recalculate if current block entry or exit junction speed has changed. + if (current->recalculate_flag || next->recalculate_flag) { + // Compute entry and exit factors for trapezoid calculations. + // NOTE: sqrt(square velocities) now performed only when required in trapezoid calculation. + double entry_factor = sqrt( current->entry_speed_sqr ) / current->nominal_speed; + double exit_factor = sqrt( next->entry_speed_sqr ) / current->nominal_speed; + calculate_trapezoid_for_block(current, entry_factor, exit_factor); + current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + } } block_index = next_block_index( block_index ); } - calculate_trapezoid_for_block(next, next->entry_speed, 0.0); // Last block + // Last/newest block in buffer. Exit speed is zero. + calculate_trapezoid_for_block(next, sqrt( next->entry_speed_sqr ) / next->nominal_speed, 0.0); + next->recalculate_flag = false; } // 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 maximum junction speed is within the set limit +// 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 one, true constant // acceleration. // 2. Go over every block in chronological order and dial down junction speed values if @@ -237,9 +278,10 @@ static void planner_recalculate_trapezoids() { // // When these stages are complete all blocks have an entry speed that will allow all speed changes to // be performed using only the one, true constant acceleration, and where no junction speed is greater -// than the set limit. Finally it will: +// than the max limit. Finally it will: // -// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. +// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids +// with no updated junction speeds will not be recalculated and assumed ok as is. static void planner_recalculate() { planner_reverse_pass(); @@ -256,7 +298,7 @@ void plan_init() { previous_nominal_speed = 0.0; } -void plan_set_acceleration_manager_enabled(int enabled) { +void plan_set_acceleration_manager_enabled(uint8_t enabled) { if ((!!acceleration_manager_enabled) != (!!enabled)) { st_synchronize(); acceleration_manager_enabled = !!enabled; @@ -282,8 +324,7 @@ block_t *plan_get_current_block() { // 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 // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. -void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) { - // The target position of the tool in absolute steps +void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) { // Calculate target position in absolute steps int32_t target[3]; @@ -299,6 +340,13 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; + + // Compute direction bits for this block + block->direction_bits = 0; + if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); @@ -308,7 +356,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; - // Compute path vector in terms of quantized step target and current positions + // Compute path vector in terms of absolute step target and current positions double delta_mm[3]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; @@ -341,53 +389,68 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: - double travel_per_step = block->millimeters/block->step_event_count; - block->rate_delta = ceil( - ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick - travel_per_step); // convert to: acceleration steps/min/acceleration_tick + double step_per_travel = block->step_event_count/block->millimeters; // Compute inverse to remove divide + block->rate_delta = step_per_travel * ceil( // convert to: acceleration steps/min/acceleration_tick + settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // acceleration mm/sec/sec per acceleration_tick + // Perform planner-enabled calculations if (acceleration_manager_enabled) { - + // Compute path unit vector - double unit_vec[3]; - unit_vec[X_AXIS] = delta_mm[X_AXIS]/block->millimeters; - unit_vec[Y_AXIS] = delta_mm[Y_AXIS]/block->millimeters; - unit_vec[Z_AXIS] = delta_mm[Z_AXIS]/block->millimeters; + double unit_vec[3]; + double inv_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inv_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inv_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Does not actually deviate from path, but used as a robust way to compute cornering speeds. // Let a circle be tangent to both previous and current path line segments, where the junction - // deviation is defined as the distance from the junction to the edge of the circle. 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, which may be also viewed as path width or max_jerk. - double vmax_junction = 0.0; // Set default zero max junction speed - - // Use default for first block or when planner is reset by previous_nominal_speed = 0.0 + // deviation is defined as the distance from the junction to the closest edge of the circle, + // colinear with the circle center. The circular segment joining the two paths represents the + // path of centripetal acceleration. Solve for max velocity based on max acceleration about the + // radius of the circle, defined indirectly by junction deviation. This may be also viewed as + // path width or max_jerk in the previous grbl version. + // NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity. + + double vmax_junction_sqr = 0.0; // Set default zero max junction speed + + // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { - // Compute cosine of angle between previous and current path - double cos_theta = ( -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - -previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - -previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ); + // 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. + double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - // Avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - vmax_junction = min(previous_nominal_speed,block->nominal_speed); - if (cos_theta > -1.0) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity - vmax_junction = max(0.0, min(vmax_junction, - sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ) ); + // Skip and use default zero max junction speed for 0 degree acute junction. + if (cos_theta < 1.0) { + vmax_junction_sqr = square( min(previous_nominal_speed,block->nominal_speed) ); + // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. + if (cos_theta > -1.0) { + // Compute maximum junction velocity based on maximum acceleration and junction deviation + double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + vmax_junction_sqr = min(vmax_junction_sqr, + settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) ); + } } } - - block->max_entry_speed = vmax_junction; - block->entry_speed = vmax_junction; + block->max_entry_speed_sqr = vmax_junction_sqr; + block->entry_speed_sqr = vmax_junction_sqr; + + // Initialize planner efficiency flags + // Set flag if block will always reach nominal speed regardless of entry/exit speeds. + if (block->nominal_speed <= sqrt(max_allowable_speed_sqr(-settings.acceleration,0.0,0.5*block->millimeters)) ) + { block->nominal_length_flag = true; } + else { block->nominal_length_flag = false; } + block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; } else { - // Set at nominal rates only for disabled acceleration planner + // Acceleration planner disabled. Set minimum that is required. block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; block->accelerate_until = 0; @@ -395,12 +458,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert block->rate_delta = 0; } - // Compute direction bits for this block - block->direction_bits = 0; - if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<