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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
gcode.c
View File

@ -3,6 +3,7 @@
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
@ -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;
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));
} else { // Offset mode specific computations
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
/*
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.
// 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; }
| <- 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))
*/
// 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];
// 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
}

123
motion_control.c
View File

@ -3,6 +3,7 @@
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
@ -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,47 +38,113 @@ 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

14
motion_control.h
View File

@ -3,6 +3,7 @@
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
@ -35,13 +36,12 @@
#define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z)
#ifdef __AVR_ATmega328P__
// 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.
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);
// 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.
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);
#endif
// Dwell for a couple of time units

1
nuts_bolts.h
View File

@ -3,6 +3,7 @@
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

201
planner.c
View File

@ -3,7 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun Jeon
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
@ -90,10 +90,11 @@ static double intersection_distance(double initial_rate, double final_rate, doub
}
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
static double max_allowable_speed(double acceleration, double target_velocity, double distance) {
return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) );
// Calculates the square of the maximum allowable speed at this point when you must be able to reach
// target_velocity using the acceleration within the allotted distance.
// NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
static double max_allowable_speed_sqr(double acceleration, double target_velocity_sqr, double distance) {
return( target_velocity_sqr-2*acceleration*60*60*distance );
}
@ -101,19 +102,29 @@ static double max_allowable_speed(double acceleration, double target_velocity, d
static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if (!current) { return; }
double entry_speed = current->max_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));
}
}
// 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);
// 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
}
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<<X_DIRECTION_BIT); }
if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
// Number of steps for each axis
block->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 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
// 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.
// Use default for first block or when planner is reset by previous_nominal_speed = 0.0
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_sqr = vmax_junction_sqr;
block->entry_speed_sqr = vmax_junction_sqr;
block->max_entry_speed = vmax_junction;
block->entry_speed = vmax_junction;
// 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<<X_DIRECTION_BIT); }
if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position

11
planner.h
View File

@ -3,6 +3,7 @@
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
@ -35,9 +36,11 @@ typedef struct {
// Fields used by the motion planner to manage acceleration
double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis
double nominal_speed; // The nominal speed for this block in mm/min
double entry_speed_sqr; // Square of entry speed at previous-current junction in (mm/min)^2
double max_entry_speed_sqr; // Square of maximum allowable entry speed in (mm/min)^2
double millimeters; // The total travel of this block in mm
double entry_speed; // Entry speed at previous-current junction
double max_entry_speed; // Maximum allowable entry speed
uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
uint8_t nominal_length_flag; // Planner flag for nominal speed always reached
// Settings for the trapezoid generator
uint32_t initial_rate; // The jerk-adjusted step rate at start of block
@ -54,7 +57,7 @@ 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
// 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);
void plan_buffer_line(double x, double y, double z, double 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.
@ -64,7 +67,7 @@ void plan_discard_current_block();
block_t *plan_get_current_block();
// Enables or disables acceleration-management for upcoming blocks
void plan_set_acceleration_manager_enabled(int enabled);
void plan_set_acceleration_manager_enabled(uint8_t enabled);
// Is acceleration-management currently enabled?
int plan_is_acceleration_manager_enabled();

1
protocol.c
View File

@ -3,6 +3,7 @@
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

3
script/grbl_preprocess.py
View File

@ -1,6 +1,6 @@
#!/usr/bin/env python
"""\
G-code preprocessor for grbl
G-code preprocessor for grbl (BETA!)
- Converts G02/03 arcs to G01 linear interpolations
- Removes comments, block delete characters, and line numbers
- Removes spaces and capitalizes commands
@ -16,6 +16,7 @@ TODO:
Based on GRBL 0.7b source code by Simen Svale Skogsrud
By: Sungeun (Sonny) Jeon
Version: 20100825
"""
import re

1
settings.c
View File

@ -3,6 +3,7 @@
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

1
settings.h
View File

@ -3,6 +3,7 @@
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