ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Optimized planner re-write. Significantly faster. Full arc support enabled by rotation matrix approach.

Browse Source 
- Significant improvements in the planner. Removed or reordered
repetitive and expensive calculations by order of importance:
recalculating unchanged blocks, trig functions [sin(), cos(), tan()],
sqrt(), divides, and multiplications. Blocks long enough for nominal
speed to be guaranteed to be reached ignored by planner. Done by
introducing two uint8_t flags per block. Reduced computational overhead
by an order of magnitude.   - Arc motion generation completely
re-written and optimized. Now runs with acceleration planner. Removed
all but one trig function (atan2) from initialization. Streamlined
computations. Segment target locations generated by vector
transformation and small angle approximation. Arc path correction
implemented for accumulated error of approximation and single precision
calculation of Arduino. Bug fix in message passing.
This commit is contained in:
Sonny J
2011-09-06 19:39:14 -06:00
parent d75ad82e49
commit ffcc3470a3
10 changed files with 274 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
motion_control.c
View File

@ -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; i<segments; i++) {
target[axis_linear] += linear_per_segment;
theta += theta_per_segment;
target[axis_1] = center_x+sin(theta)*radius;
target[axis_2] = center_y+cos(theta)*radius;
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
trajectory[axis_linear] = position[axis_linear];
for (i = 1; i<segments; i++) { // Increment (segments-1)
if (count < 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 = clockwise_sign*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 trajectory location
trajectory[axis_0] = center_axis0 + r_axis0;
trajectory[axis_1] = center_axis1 + r_axis1;
trajectory[axis_linear] += linear_per_segment;
plan_buffer_line(trajectory[X_AXIS], trajectory[Y_AXIS], trajectory[Z_AXIS], feed_rate, invert_feed_rate);
}
plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
// Ensure last segment arrives at target location.
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
}
#endif
void mc_go_home()
{
st_go_home();
}
}