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

Significantly improved junction control and fixed computation bugs in planner

Browse Source 
- Junction jerk now re-defined as junction_deviation. The distance from
the junction to the edge of a circle tangent to both previous and
current path lines. The circle radii is used to compute the maximum
junction velocity by centripetal acceleration. More robust and
simplified way to compute jerk.   - Fixed bugs related to entry and exit
factors. They were computed based on the current nominal speeds but not
when computing exit factors for neighboring blocks. Removed factors and
replaced with entry speeds only. Factors now only computed for stepper
trapezoid rate conversions.  - Misc: Added min(), next_block_index,
prev_block_index functions for clarity.
This commit is contained in:
Sonny J 2011-09-03 15:31:48 -06:00
parent badb638df9
commit 5c2150daa9
5 changed files with 119 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
nuts_bolts.h
View File

@ -33,6 +33,7 @@
#define clear_vector(a) memset(a, 0, sizeof(a))
#define max(a,b) (((a) > (b)) ? (a) : (b))
#define min(a,b) (((a) < (b)) ? (a) : (b))
// Read a floating point value from a string. Line points to the input buffer, char_counter
// is the indexer pointing to the current character of the line, while double_ptr is

217
planner.c
View File

@ -3,7 +3,8 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Modifications Copyright (c) 2011 Sungeun 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
@ -48,15 +49,28 @@ static uint8_t acceleration_manager_enabled; // Acceleration management active
#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
// Returns the index of the next block in the ring buffer
static int8_t next_block_index(int8_t block_index) {
return( (block_index + 1) % BLOCK_BUFFER_SIZE );
}
// Returns the index of the previous block in the ring buffer
static int8_t prev_block_index(int8_t block_index) {
block_index--;
if (block_index < 0) { block_index = BLOCK_BUFFER_SIZE-1; }
return(block_index);
}
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) {
return(
(target_rate*target_rate-initial_rate*initial_rate)/
(2L*acceleration)
);
return( (target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration) );
}
/* + <- some maximum rate we don't care about
/|\
/ | \
@ -66,97 +80,53 @@ static double estimate_acceleration_distance(double initial_rate, double target_
^ ^
| |
intersection_distance distance */
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
return(
(2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
(4*acceleration)
);
return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
}
// 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)
);
}
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
static double junction_jerk(block_t *before, block_t *after) {
return(sqrt(
pow(before->speed_x-after->speed_x, 2)+
pow(before->speed_y-after->speed_y, 2)+
pow(before->speed_z-after->speed_z, 2))
);
}
// Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the
// speed under which you cannot exceed max_jerk no matter what you do.
static double factor_for_safe_speed(block_t *block) {
if(settings.max_jerk < block->nominal_speed) {
return(settings.max_jerk/block->nominal_speed);
} else {
return(1.0);
}
return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) );
}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!current) { return; }
double entry_factor = 1.0;
double exit_factor;
if (next) {
exit_factor = next->entry_factor;
} else {
exit_factor = factor_for_safe_speed(current);
}
// Calculate the entry_factor for the current block.
if (!current) { return; }
// Calculate the entry speed for the current block.
if (previous) {
// Reduce speed so that junction_jerk is within the maximum allowed
double jerk = junction_jerk(previous, current);
if (jerk > settings.max_jerk) {
entry_factor = (settings.max_jerk/jerk);
}
double entry_speed = current->max_entry_speed;
double exit_speed;
if (next) {
exit_speed = next->entry_speed;
} else {
exit_speed = 0.0;
}
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
if (entry_factor > exit_factor) {
double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor,
current->millimeters);
double max_entry_factor = max_entry_speed/current->nominal_speed;
if (max_entry_factor < entry_factor) {
entry_factor = max_entry_factor;
}
if (entry_speed > exit_speed) {
entry_speed =
min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed);
}
current->entry_speed = entry_speed;
} else {
entry_factor = factor_for_safe_speed(current);
current->entry_speed = 0.0;
}
// Store result
current->entry_factor = entry_factor;
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
static void planner_reverse_pass() {
auto int8_t block_index = block_buffer_head;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_tail) {
block_index--;
if(block_index < 0) {
block_index = BLOCK_BUFFER_SIZE-1;
}
block_index = prev_block_index( block_index );
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
@ -165,25 +135,23 @@ static void planner_reverse_pass() {
planner_reverse_pass_kernel(NULL, block[0], block[1]);
}
// 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 out entry
// speed accordingly. Remember current->entry_factor equals the exit factor of
// the previous block.¨
// complete the full speed change within the block, we need to adjust the entry
// speed accordingly.
if(previous) {
if(previous->entry_factor < current->entry_factor) {
double max_entry_speed = max_allowable_speed(-settings.acceleration,
current->nominal_speed*previous->entry_factor, previous->millimeters);
double max_entry_factor = max_entry_speed/current->nominal_speed;
if (max_entry_factor < current->entry_factor) {
current->entry_factor = max_entry_factor;
}
if (previous->entry_speed < current->entry_speed) {
current->entry_speed =
min( max_allowable_speed(-settings.acceleration,current->entry_speed,previous->millimeters),
min( current->max_entry_speed, current->entry_speed ) );
}
}
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass.
static void planner_forward_pass() {
@ -195,11 +163,12 @@ static void planner_forward_pass() {
block[1] = block[2];
block[2] = &block_buffer[block_index];
planner_forward_pass_kernel(block[0],block[1],block[2]);
block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
block_index = next_block_index( block_index );
}
planner_forward_pass_kernel(block[1], block[2], NULL);
}
/*
+--------+ <- nominal_rate
/ \
@ -208,10 +177,10 @@ static void planner_forward_pass() {
+-------------+
time -->
*/
// 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.
static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
block->initial_rate = ceil(block->nominal_rate*entry_factor);
block->final_rate = ceil(block->nominal_rate*exit_factor);
int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0;
@ -236,6 +205,7 @@ 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_factor for each junction. Must be called by planner_recalculate() after
// updating the blocks.
@ -243,34 +213,37 @@ static void planner_recalculate_trapezoids() {
int8_t block_index = block_buffer_tail;
block_t *current;
block_t *next = NULL;
while(block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current) {
calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor);
// 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);
}
block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
block_index = next_block_index( block_index );
}
calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next));
calculate_trapezoid_for_block(next, next->entry_speed, 0.0);
}
// 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_factor)
// 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 jerk is within the set limit
// a. The maximum junction speed is within the set 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 reduction values if
// a. The speed increase within one block would require faster accelleration than the one, true
// 2. Go over every block in chronological order and dial down junction speed values if
// a. The speed increase within one block would require faster acceleration than the one, true
// constant acceleration.
//
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
// the set limit. Finally it will:
// 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:
//
// 3. Recalculate trapezoids for all blocks using the recently updated factors
// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds.
static void planner_recalculate() {
planner_reverse_pass();
@ -298,7 +271,7 @@ int plan_is_acceleration_manager_enabled() {
void plan_discard_current_block() {
if (block_buffer_head != block_buffer_tail) {
block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;
block_buffer_tail = next_block_index( block_buffer_tail );
}
}
@ -307,6 +280,7 @@ block_t *plan_get_current_block() {
return(&block_buffer[block_buffer_tail]);
}
// 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.
@ -320,10 +294,11 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
// Calculate the buffer head after we push this byte
int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
int next_buffer_head = next_block_index( block_buffer_head );
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Number of steps for each axis
@ -331,15 +306,16 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-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; };
double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm));
block->delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
block->delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
block->delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
block->millimeters = sqrt(square(block->delta_mm[X_AXIS]) + square(block->delta_mm[Y_AXIS]) +
square(block->delta_mm[Z_AXIS]));
uint32_t microseconds;
if (!invert_feed_rate) {
microseconds = lround((block->millimeters/feed_rate)*1000000);
@ -349,17 +325,16 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
// Calculate speed in mm/minute for each axis
double multiplier = 60.0*1000000.0/microseconds;
block->speed_x = delta_x_mm * multiplier;
block->speed_y = delta_y_mm * multiplier;
block->speed_z = delta_z_mm * multiplier;
block->speed_x = block->delta_mm[X_AXIS] * multiplier;
block->speed_y = block->delta_mm[Y_AXIS] * multiplier;
block->speed_z = block->delta_mm[Z_AXIS] * multiplier;
block->nominal_speed = block->millimeters * multiplier;
block->nominal_rate = ceil(block->step_event_count * multiplier);
block->entry_factor = 0.0;
// This is a temporary fix to avoid a situation where very low nominal_speeds would be rounded
// down to zero and cause a division by zero. TODO: Grbl deserves a less patchy fix for this problem
if (block->nominal_speed < 60.0) { block->nominal_speed = 60.0; }
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
// average travel per step event changes. For a line along one axis the travel per step event
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
@ -370,10 +345,32 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
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
if (acceleration_manager_enabled) {
// compute a preliminary conservative acceleration trapezoid
double safe_speed_factor = factor_for_safe_speed(block);
calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor);
if (acceleration_manager_enabled) {
// Compute initial trapazoid and maximum entry speed at junction
double vmax_junction = 0.0;
// Skip first block, set default zero max junction speed.
if (block_buffer_head != block_buffer_tail) {
block_t *previous = &block_buffer[ prev_block_index(block_buffer_head) ];
// Compute cosine of angle between previous and current path
double cos_theta = ( -previous->delta_mm[X_AXIS] * block->delta_mm[X_AXIS] +
-previous->delta_mm[Y_AXIS] * block->delta_mm[Y_AXIS] +
-previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/
( previous->millimeters * block->millimeters );
// Avoid divide by zero and set zero max junction velocity for highly acute angles.
if (cos_theta < 0.9) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity
vmax_junction =
sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2));
vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed)));
}
}
block->max_entry_speed = vmax_junction;
block->entry_speed = 0.0;
calculate_trapezoid_for_block(block, block->entry_speed, 0.0);
} else {
block->initial_rate = block->nominal_rate;
block->final_rate = block->nominal_rate;

8
planner.h
View File

@ -36,10 +36,10 @@ typedef struct {
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 millimeters; // The total travel of this block in mm
double entry_factor; // The factor representing the change in speed at the start of this trapezoid.
// (The end of the curren speed trapezoid is defined by the entry_factor of the
// next block)
double delta_mm[3]; // XYZ travel components of this block in mm
double entry_speed; // Entry speed at previous-current junction
double max_entry_speed; // Maximum allowable entry speed
// Settings for the trapezoid generator
uint32_t initial_rate; // The jerk-adjusted step rate at start of block
uint32_t final_rate; // The minimal rate at exit

12
settings.c
View File

@ -51,7 +51,7 @@ typedef struct {
#define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 500.0
#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0)
#define DEFAULT_MAX_JERK 300.0
#define DEFAULT_JUNCTION_DEVIATION 0.1
#define DEFAULT_STEPPING_INVERT_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT))
void settings_reset() {
@ -64,7 +64,7 @@ void settings_reset() {
settings.acceleration = DEFAULT_ACCELERATION;
settings.mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT;
settings.invert_mask = DEFAULT_STEPPING_INVERT_MASK;
settings.max_jerk = DEFAULT_MAX_JERK;
settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION;
}
void settings_dump() {
@ -78,8 +78,8 @@ void settings_dump() {
printPgmString(PSTR(" (mm/arc segment)\r\n$7 = ")); printInteger(settings.invert_mask);
printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2);
printPgmString(PSTR(")\r\n$8 = ")); printFloat(settings.acceleration);
printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); printFloat(settings.max_jerk);
printPgmString(PSTR(" (max instant cornering speed change in delta mm/min)"));
printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); printFloat(settings.junction_deviation);
printPgmString(PSTR(" (junction deviation for cornering in mm)"));
printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n"));
}
@ -129,7 +129,7 @@ int read_settings() {
return(false);
}
settings.acceleration = DEFAULT_ACCELERATION;
settings.max_jerk = DEFAULT_MAX_JERK;
settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION;
} else {
return(false);
}
@ -156,7 +156,7 @@ void settings_store_setting(int parameter, double value) {
case 6: settings.mm_per_arc_segment = value; break;
case 7: settings.invert_mask = trunc(value); break;
case 8: settings.acceleration = value; break;
case 9: settings.max_jerk = fabs(value); break;
case 9: settings.junction_deviation = fabs(value); break;
default:
printPgmString(PSTR("Unknown parameter\r\n"));
return;

2
settings.h
View File

@ -41,7 +41,7 @@ typedef struct {
uint8_t invert_mask;
double mm_per_arc_segment;
double acceleration;
double max_jerk;
double junction_deviation;
} settings_t;
extern settings_t settings;