Skip to content

Commit

Permalink
Arc mm_per_segment removed, now in terms of tolerance. Stepper ramp c…
Browse files Browse the repository at this point in the history 
…ounter variable type corrected.

- Arc mm_per_segment parameter was removed and replaced with an
arc_tolerance parameter, which scales all arc segments automatically to
radius, such that the line segment error doesn't exceed the tolerance.
Significantly improves arc performance through larger radius arc,
because the segments are much longer and the planner buffer has more to
work with.

- Moved n_arc correction from the settings to config.h. Mathematically
this doesn't need to be a setting anymore, as the default config value
will work for all known CNC applications. The error does not accumulate
as much anymore, since the small angle approximation used by the arc
generation has been updated to a third-order approximation and how the
line segment length scale with radius and tolerance now. Left in
config.h for extraneous circumstances.

- Corrected the st.ramp_count variable (acceleration tick counter) to a
8-bit vs. 32-bit variable. Should make the stepper algorithm just a
touch faster overall.
  • Loading branch information
@chamnit
chamnit committed Dec 20, 2012
1 parent a1397f6 commit 3dfffa6
Show file tree
Hide file tree
Showing 8 changed files with 207 additions and 127 deletions.
26 changes: 18 additions & 8 deletions config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,14 +105,6 @@
#define CMD_CYCLE_START '~'
#define CMD_RESET 0x18 // ctrl-x

// 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
// very high accelerations, this will reduce the error between how the planner plans the velocity
// 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 100L

// The "Stepper Driver Interrupt" employs the Pramod Ranade inverse time algorithm to manage the
// Bresenham line stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which
// the Ranade algorithm ticks at. Maximum step frequencies are limited by the Ranade frequency by
Expand All @@ -124,6 +116,17 @@
// 20kHz by performing two steps per step event, rather than just one.
#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
// very high accelerations, this will reduce the error between how the planner plans the velocity
// 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 100L

// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters.
#define INTERRUPTS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)

// The Ranade algorithm can use either floating point or long integers for its counters, but for
// integers the counter values must be scaled since these values can be very small (10^-6). This
// multiplier value scales the floating point counter values for use in a long integer. Long integers
Expand Down Expand Up @@ -193,6 +196,13 @@
// parser state depending on user preferences.
#define N_STARTUP_LINE 2 // Integer (1-5)

// Number of arc generation iterations by small angle approximation before exact arc trajectory
// correction. This parameter maybe decreased if there are issues with the accuracy of the arc
// generations. In general, the default value is more than enough for the intended CNC applications
// of grbl, and should be on the order or greater than the size of the buffer to help with the
// computational efficiency of generating arcs.
#define N_ARC_CORRECTION 20 // Integer (1-255)

// ---------------------------------------------------------------------------------------
// FOR ADVANCED USERS ONLY:

Expand Down
12 changes: 4 additions & 8 deletions defaults.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#define DEFAULT_Y_STEPS_PER_MM 250.0
#define DEFAULT_Z_STEPS_PER_MM 250.0
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
#define DEFAULT_ARC_TOLERANCE 0.005 // mm
#define DEFAULT_RAPID_FEEDRATE 500.0 // mm/min
#define DEFAULT_FEEDRATE 250.0
#define DEFAULT_ACCELERATION (10.0*60*60) // 10 mm/min^2
Expand All @@ -51,7 +51,6 @@
#define DEFAULT_HOMING_PULLOFF 1.0 // mm
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255)
#define DEFAULT_DECIMAL_PLACES 3
#define DEFAULT_N_ARC_CORRECTION 25
#endif

#ifdef DEFAULTS_SHERLINE_5400
Expand All @@ -64,7 +63,7 @@
#define DEFAULT_Y_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
#define DEFAULT_ARC_TOLERANCE 0.005 // mm
#define DEFAULT_RAPID_FEEDRATE 635.0 // mm/min (25ipm)
#define DEFAULT_FEEDRATE 254.0 // mm/min (10ipm)
#define DEFAULT_ACCELERATION 50.0*60*60 // 50 mm/min^2
Expand All @@ -82,7 +81,6 @@
#define DEFAULT_HOMING_PULLOFF 1.0 // mm
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255)
#define DEFAULT_DECIMAL_PLACES 3
#define DEFAULT_N_ARC_CORRECTION 25
#endif

#ifdef DEFAULTS_SHAPEOKO
Expand All @@ -98,7 +96,7 @@
#define DEFAULT_Y_STEPS_PER_MM (MICROSTEPS_XY*STEP_REVS_XY/MM_PER_REV_XY)
#define DEFAULT_Z_STEPS_PER_MM (MICROSTEPS_Z*STEP_REVS_Z/MM_PER_REV_Z)
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
#define DEFAULT_ARC_TOLERANCE 0.005 // mm
#define DEFAULT_RAPID_FEEDRATE 1000.0 // mm/min
#define DEFAULT_FEEDRATE 250.0
#define DEFAULT_ACCELERATION (15.0*60*60) // 15 mm/min^2
Expand All @@ -116,7 +114,6 @@
#define DEFAULT_HOMING_PULLOFF 1.0 // mm
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-255)
#define DEFAULT_DECIMAL_PLACES 3
#define DEFAULT_N_ARC_CORRECTION 25
#endif

#ifdef DEFAULTS_ZEN_TOOLWORKS_7x7
Expand All @@ -130,7 +127,7 @@
#define DEFAULT_Y_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
#define DEFAULT_ARC_TOLERANCE 0.005 // mm
#define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min
#define DEFAULT_FEEDRATE 1000.0 // mm/min
#define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2
Expand All @@ -148,7 +145,6 @@
#define DEFAULT_HOMING_PULLOFF 1.0 // mm
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255)
#define DEFAULT_DECIMAL_PLACES 3
#define DEFAULT_N_ARC_CORRECTION 25
#endif

#endif
156 changes: 82 additions & 74 deletions motion_control.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,8 +91,9 @@ void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rat
// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
// for vector transformation direction.
// 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.
// The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance
// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal
// distance from segment to the circle when the end points both lie on the circle.
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise)
{
Expand All @@ -111,83 +112,90 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
} else {
if (angular_travel <= 0) { angular_travel += 2*M_PI; }
}


// NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
// (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
// is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
// Computes: mm_per_arc_segment = sqrt(4*arc_tolerance*(2*radius-arc_tolerance)),
// segments = millimeters_of_travel/mm_per_arc_segment
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (millimeters_of_travel == 0.0) { return; }
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; }

float theta_per_segment = angular_travel/segments;
float linear_per_segment = linear_travel/segments;
uint16_t segments = floor(millimeters_of_travel/
sqrt(4*settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );

/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. 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
float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
float sin_T = theta_per_segment;
if (segments) {
// 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; }

float theta_per_segment = angular_travel/segments;
float linear_per_segment = linear_travel/segments;

/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. 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. Single precision values can accumulate error greater than tool precision in some
cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very
expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute.
float arc_target[3];
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
int8_t count = 0;

// Initialize the linear axis
arc_target[axis_linear] = position[axis_linear];
Small angle approximation may be used to reduce computation overhead further. A third-order approximation
(second order sin() has too much error) holds for nearly all CNC applications, except for possibly very
small radii (~0.5mm). In other words, theta_per_segment would need to be greater than 0.25 rad(14 deg)
and N_ARC_CORRECTION would need to be large to cause an appreciable drift error (>5% of radius, for very
small radii this is very, very small). N_ARC_CORRECTION~=20 should be more than small enough to correct
for numerical drift error.
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.
*/
// 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.1666667*(cos_T + 4);
cos_T *= 0.5;

for (i = 1; i<segments; i++) { // Increment (segments-1)

if (count < settings.n_arc_correction) {
// Apply vector rotation matrix
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else {
// Arc correction to radius vector. Computed only every n_arc_correction increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;
float arc_target[N_AXIS];
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
int8_t count = 0;

// Initialize the linear axis
arc_target[axis_linear] = position[axis_linear];

for (i = 1; i<segments; i++) { // Increment (segments-1)

if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix. ~40 usec
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;
}

// Update arc_target location
arc_target[axis_0] = center_axis0 + r_axis0;
arc_target[axis_1] = center_axis1 + r_axis1;
arc_target[axis_linear] += linear_per_segment;
mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate);

// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
if (sys.abort) { return; }
}

// Update arc_target location
arc_target[axis_0] = center_axis0 + r_axis0;
arc_target[axis_1] = center_axis1 + r_axis1;
arc_target[axis_linear] += linear_per_segment;
mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate);

// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
if (sys.abort) { return; }
}
// Ensure last segment arrives at target location.
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
Expand Down
Loading

0 comments on commit 3dfffa6

Please sign in to comment.