diff --git a/nuts_bolts.h b/nuts_bolts.h index e5a2ea8..dc19801 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -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 diff --git a/planner.c b/planner.c index 9856489..0a4cdbe 100644 --- a/planner.c +++ b/planner.c @@ -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; diff --git a/planner.h b/planner.h index c39e8fc..e99ce78 100644 --- a/planner.h +++ b/planner.h @@ -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 diff --git a/settings.c b/settings.c index 82544d3..791fca7 100644 --- a/settings.c +++ b/settings.c @@ -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<